diff --git a/auv_navigation/auv_mapping/CMakeLists.txt b/auv_navigation/auv_mapping/CMakeLists.txt
index 930f07d4..b30c1051 100644
--- a/auv_navigation/auv_mapping/CMakeLists.txt
+++ b/auv_navigation/auv_mapping/CMakeLists.txt
@@ -1,11 +1,49 @@
 cmake_minimum_required(VERSION 3.10.2)
 project(auv_mapping)
 
-find_package(catkin REQUIRED)
+add_compile_options(-std=c++17)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+
+find_package(catkin REQUIRED
+  roscpp
+  tf2
+  tf2_ros
+  geometry_msgs
+  auv_msgs
+)
 
 catkin_package()
 
 include_directories(
-# include
-# ${catkin_INCLUDE_DIRS}
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+
+add_executable(object_map_tf_server_node
+  src/object_map_tf_server_node.cpp
+)
+
+# add_dependencies(object_map_tf_server_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
+
+target_link_libraries(object_map_tf_server_node
+  ${catkin_LIBRARIES}
+)
+
+install(TARGETS object_map_tf_server_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+install(DIRECTORY
+  launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
diff --git a/auv_navigation/auv_mapping/include/auv_mapping/object_map_tf_server_ros.hpp b/auv_navigation/auv_mapping/include/auv_mapping/object_map_tf_server_ros.hpp
new file mode 100644
index 00000000..5cafafd3
--- /dev/null
+++ b/auv_navigation/auv_mapping/include/auv_mapping/object_map_tf_server_ros.hpp
@@ -0,0 +1,155 @@
+#pragma once
+
+#include <auv_msgs/SetObjectTransform.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <ros/ros.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <mutex>
+#include <optional>
+#include <unordered_map>
+
+namespace auv_mapping {
+class ObjectMapTFServerROS {
+  using TransformMap =
+      std::unordered_map<std::string, geometry_msgs::TransformStamped>;
+
+ public:
+  ObjectMapTFServerROS(const ros::NodeHandle &nh)
+      : nh_{nh},
+        tf_buffer_{},
+        tf_listener_{tf_buffer_},
+        rate_{10.0},
+        static_frame_{""},
+        transforms_{},
+        tf_broadcaster_{} {
+    auto node_handler_private = ros::NodeHandle{"~"};
+
+    node_handler_private.param<std::string>("static_frame", static_frame_,
+                                            "odom");
+    node_handler_private.param<double>("rate", rate_, 10.0);
+
+    service_ = nh_.advertiseService(
+        "set_object_transform", &ObjectMapTFServerROS::set_transform_handler,
+        this);
+
+    ROS_INFO("ObjectMapTFServerROS initialized. Static frame: %s",
+             static_frame_.c_str());
+  }
+
+  bool set_transform_handler(auv_msgs::SetObjectTransform::Request &req,
+                             auv_msgs::SetObjectTransform::Response &res) {
+    const auto parent_frame = req.transform.header.frame_id;
+    const auto target_frame = req.transform.child_frame_id;
+    const auto static_to_parent_transform =
+        get_transform(static_frame_, parent_frame, ros::Duration(4.0));
+
+    if (!static_to_parent_transform.has_value()) {
+      ROS_ERROR("Error occurred while looking up transform:(ozanhakantunca)");
+      res.success = false;
+      res.message = "Failed to capture transform TODO (ozanhakantunca)";
+      return false;
+    }
+
+    const auto parent_to_target_quaternion = tf2::Quaternion(
+        req.transform.transform.rotation.x, req.transform.transform.rotation.y,
+        req.transform.transform.rotation.z, req.transform.transform.rotation.w);
+
+    auto parent_to_target_orientation =
+        tf2::Matrix3x3{parent_to_target_quaternion};
+
+    auto parent_to_target_translation =
+        tf2::Vector3{req.transform.transform.translation.x,
+                     req.transform.transform.translation.y,
+                     req.transform.transform.translation.z};
+
+    auto static_to_parent_quaternion = tf2::Quaternion{};
+    tf2::fromMsg(static_to_parent_transform->transform.rotation,
+                 static_to_parent_quaternion);
+
+    const auto static_to_parent_orientation =
+        tf2::Matrix3x3{static_to_parent_quaternion};
+
+    const auto static_to_parent_translation =
+        tf2::Vector3{static_to_parent_transform->transform.translation.x,
+                     static_to_parent_transform->transform.translation.y,
+                     static_to_parent_transform->transform.translation.z};
+
+    const auto static_to_target_orientation = tf2::Matrix3x3{
+        static_to_parent_orientation * parent_to_target_orientation};
+
+    const auto static_to_target_translation = tf2::Vector3{
+        static_to_parent_translation +
+        static_to_parent_orientation * parent_to_target_translation};
+
+    auto static_transform = geometry_msgs::TransformStamped{};
+    static_transform.header.stamp = ros::Time::now();
+    static_transform.header.frame_id = static_frame_;
+    static_transform.child_frame_id = target_frame;
+
+    static_transform.transform.translation.x = static_to_target_translation.x();
+    static_transform.transform.translation.y = static_to_target_translation.y();
+    static_transform.transform.translation.z = static_to_target_translation.z();
+
+    auto static_to_target_quaternion = tf2::Quaternion{};
+    static_to_target_orientation.getRotation(static_to_target_quaternion);
+    static_transform.transform.rotation =
+        tf2::toMsg(static_to_target_quaternion);
+
+    {
+      auto lock = std::scoped_lock(mutex_);
+      transforms_[target_frame] = static_transform;
+    }
+
+    ROS_INFO_STREAM("Stored static transform from " << static_frame_ << " to "
+                                                    << target_frame);
+    res.success = true;
+    res.message = "Stored transform for frame: " + target_frame;
+    return true;
+  }
+
+  void publishTransforms() {
+    auto rate = ros::Rate{rate_};
+    while (ros::ok()) {
+      {
+        auto lock = std::scoped_lock(mutex_);
+
+        for (const auto &entry : transforms_) {
+          auto transform = entry.second;
+          transform.header.stamp = ros::Time::now();
+          tf_broadcaster_.sendTransform(transform);
+        }
+      }
+      rate.sleep();
+    }
+  }
+
+ private:
+  std::optional<geometry_msgs::TransformStamped> get_transform(
+      const std::string &target_frame, const std::string &source_frame,
+      const ros::Duration timeout) {
+    try {
+      auto transform = tf_buffer_.lookupTransform(target_frame, source_frame,
+                                                  ros::Time(0), timeout);
+      return transform;
+    } catch (tf2::TransformException &ex) {
+      ROS_WARN_STREAM("Transform lookup failed: " << ex.what());
+      return std::nullopt;
+    }
+  }
+
+  ros::NodeHandle nh_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+  double rate_;
+  std::string static_frame_;
+  TransformMap transforms_;
+  tf2_ros::TransformBroadcaster tf_broadcaster_;
+  //
+  std::mutex mutex_;
+  ros::ServiceServer service_;
+};
+
+}  // namespace auv_mapping
diff --git a/auv_navigation/auv_mapping/launch/start.launch b/auv_navigation/auv_mapping/launch/start.launch
index cf8dcd26..3d8204cd 100644
--- a/auv_navigation/auv_mapping/launch/start.launch
+++ b/auv_navigation/auv_mapping/launch/start.launch
@@ -4,7 +4,7 @@
   <arg name="control_rate" default="10" />
 
   <group ns="$(arg namespace)">
-    <node pkg="auv_mapping" type="object_map_tf_server.py" name="object_map_tf_server">
+    <node pkg="auv_mapping" type="object_map_tf_server_node" name="object_map_tf_server_node">
       <param name="rate" value="$(arg control_rate)" />
       <param name="static_frame" value="odom" />
 
@@ -13,8 +13,4 @@
   </group>
 
   <node pkg="auv_control" type="prop_transform_publisher.py" name="prop_transform_publisher" output="screen" />
-
-  <!--
-  <node pkg="auv_control" type="bin_transform_publisher.py" name="bin_transform_publisher" />
-  </include> -->
 </launch>
diff --git a/auv_navigation/auv_mapping/package.xml b/auv_navigation/auv_mapping/package.xml
index 63951f11..63fe86bd 100644
--- a/auv_navigation/auv_mapping/package.xml
+++ b/auv_navigation/auv_mapping/package.xml
@@ -7,6 +7,10 @@
   <author email="senceryazici@gmail.com">Sencer Yazici</author>
   <license>BSD-3-Clause</license>
   <buildtool_depend>catkin</buildtool_depend>
+  <depend>roscpp</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>geometry_msgs</depend>
   <depend>auv_msgs</depend>
   <export>
 
diff --git a/auv_navigation/auv_mapping/scripts/object_map_tf_server.py b/auv_navigation/auv_mapping/scripts/object_map_tf_server.py
deleted file mode 100755
index 4b40d09a..00000000
--- a/auv_navigation/auv_mapping/scripts/object_map_tf_server.py
+++ /dev/null
@@ -1,134 +0,0 @@
-#!/usr/bin/env python3
-
-import rospy
-import tf
-import tf2_ros
-import threading
-from geometry_msgs.msg import TransformStamped
-from auv_msgs.srv import SetObjectTransform, SetObjectTransformResponse
-
-
-class ObjectMapTFServer:
-    def __init__(self):
-        rospy.init_node("object_map_tf_server", anonymous=True)
-
-        # Load parameters
-        self.static_frame = rospy.get_param("~static_frame", "odom")
-        self.update_rate = rospy.get_param("~rate", 10.0)  # Hz
-
-        # Transform storage
-        self.transforms = {}  # Dictionary to store target frames and their transforms
-
-        # Lock for thread-safe access to transforms
-        self.lock = threading.Lock()
-
-        # ROS service to set object transform
-        self.service = rospy.Service(
-            "set_object_transform", SetObjectTransform, self.handle_set_transform
-        )
-
-        # Transform broadcaster
-        self.tf_broadcaster = tf2_ros.TransformBroadcaster()
-
-        # Transform listener to get odom->base_link transform
-        self.tf_listener = tf.TransformListener()
-
-        rospy.loginfo(
-            "ObjectMapTFServer initialized. Static frame: %s", self.static_frame
-        )
-
-    def handle_set_transform(self, req):
-        parent_frame = req.transform.header.frame_id
-        target_frame = req.transform.child_frame_id
-
-        try:
-            # Wait for the transform from static_frame (odom) to parent_frame (base_link)
-            self.tf_listener.waitForTransform(
-                self.static_frame, parent_frame, rospy.Time(0), rospy.Duration(4.0)
-            )
-            (trans, rot) = self.tf_listener.lookupTransform(
-                self.static_frame, parent_frame, rospy.Time(0)
-            )
-
-            # Convert the provided transform to the static frame
-            static_transform = TransformStamped()
-            static_transform.header.stamp = rospy.Time.now()
-            static_transform.header.frame_id = self.static_frame
-            static_transform.child_frame_id = target_frame
-
-            # Parent to target transformation matrix
-            parent_to_target = tf.transformations.quaternion_matrix(
-                (
-                    req.transform.transform.rotation.x,
-                    req.transform.transform.rotation.y,
-                    req.transform.transform.rotation.z,
-                    req.transform.transform.rotation.w,
-                )
-            )
-            parent_to_target[0:3, 3] = [
-                req.transform.transform.translation.x,
-                req.transform.transform.translation.y,
-                req.transform.transform.translation.z,
-            ]
-
-            # Static frame to parent transformation matrix
-            static_to_parent = tf.transformations.quaternion_matrix(rot)
-            static_to_parent[0:3, 3] = trans
-
-            # Combined transformation: static frame to target
-            static_to_target = tf.transformations.concatenate_matrices(
-                static_to_parent, parent_to_target
-            )
-
-            # Extract translation and rotation
-            static_translation = tf.transformations.translation_from_matrix(
-                static_to_target
-            )
-            static_rotation = tf.transformations.quaternion_from_matrix(
-                static_to_target
-            )
-
-            static_transform.transform.translation.x = static_translation[0]
-            static_transform.transform.translation.y = static_translation[1]
-            static_transform.transform.translation.z = static_translation[2]
-            static_transform.transform.rotation.x = static_rotation[0]
-            static_transform.transform.rotation.y = static_rotation[1]
-            static_transform.transform.rotation.z = static_rotation[2]
-            static_transform.transform.rotation.w = static_rotation[3]
-
-            # Store or update the transform in the dictionary using the lock
-            with self.lock:
-                self.transforms[target_frame] = static_transform
-
-            rospy.loginfo("Stored static transform for frame: %s", target_frame)
-
-            return SetObjectTransformResponse(
-                success=True, message=f"Stored transform for frame: {target_frame}"
-            )
-
-        except (
-            tf.LookupException,
-            tf.ConnectivityException,
-            tf.ExtrapolationException,
-        ) as e:
-            rospy.logerr("Error occurred while looking up transform: %s", str(e))
-            return SetObjectTransformResponse(
-                success=False, message=f"Failed to capture transform: {str(e)}"
-            )
-
-    def publish_transforms(self):
-        rate = rospy.Rate(self.update_rate)
-        while not rospy.is_shutdown():
-            with self.lock:
-                for transform in self.transforms.values():
-                    transform.header.stamp = rospy.Time.now()
-                    self.tf_broadcaster.sendTransform(transform)
-            rate.sleep()
-
-
-if __name__ == "__main__":
-    try:
-        server = ObjectMapTFServer()
-        server.publish_transforms()
-    except rospy.ROSInterruptException:
-        pass
diff --git a/auv_navigation/auv_mapping/src/object_map_tf_server_node.cpp b/auv_navigation/auv_mapping/src/object_map_tf_server_node.cpp
new file mode 100755
index 00000000..44a20ea7
--- /dev/null
+++ b/auv_navigation/auv_mapping/src/object_map_tf_server_node.cpp
@@ -0,0 +1,15 @@
+#include "auv_mapping/object_map_tf_server_ros.hpp"
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "object_map_tf_server");
+  auto node_handle = ros::NodeHandle{};
+  auto server = auv_mapping::ObjectMapTFServerROS{node_handle};
+
+  auto spinner = ros::AsyncSpinner{2};
+  spinner.start();
+
+  server.publishTransforms();
+  ros::waitForShutdown();
+
+  return 0;
+}
diff --git a/auv_software/package.xml b/auv_software/package.xml
index a9c3583f..2af7cde1 100644
--- a/auv_software/package.xml
+++ b/auv_software/package.xml
@@ -13,7 +13,6 @@
   <depend>auv_navigation</depend>
   <depend>auv_localization</depend>
   <depend>auv_vision</depend>
-  
   <export>
 
   </export>
diff --git a/auv_vision/auv_detection/CMakeLists.txt b/auv_vision/auv_detection/CMakeLists.txt
index f7ecb46c..f4b20465 100644
--- a/auv_vision/auv_detection/CMakeLists.txt
+++ b/auv_vision/auv_detection/CMakeLists.txt
@@ -5,14 +5,14 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 catkin_package(
-  CATKIN_DEPENDS 
+  CATKIN_DEPENDS
 )
 
 include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-install(DIRECTORY 
-  launch 
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}  
+install(DIRECTORY
+  launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
diff --git a/auv_vision/auv_detection/launch/tracker.launch b/auv_vision/auv_detection/launch/tracker.launch
index d3b480a7..a5ad4cdf 100644
--- a/auv_vision/auv_detection/launch/tracker.launch
+++ b/auv_vision/auv_detection/launch/tracker.launch
@@ -20,7 +20,7 @@
   <arg name="result_labels" default="true"/>
   <arg name="result_boxes" default="true"/>
   <!-- Create symlink before launching the node -->
-  <param name="/create_model_symlink_cmd" 
+  <param name="/create_model_symlink_cmd"
          command="bash -c 'ln -sf $(arg model_source_path) $(arg ultralytics_models_dir)/$(arg yolo_model)'"/>
   <node name="tracker_node" pkg="ultralytics_ros" type="tracker_node.py" output="screen">
     <param name="yolo_model" value="$(arg yolo_model)"/>
diff --git a/auv_vision/auv_detection/package.xml b/auv_vision/auv_detection/package.xml
index a2ff9578..79c0ad0c 100644
--- a/auv_vision/auv_detection/package.xml
+++ b/auv_vision/auv_detection/package.xml
@@ -3,16 +3,12 @@
   <name>auv_detection</name>
   <version>0.1.0</version>
   <description>The auv_detection package</description>
-
   <author email="t.ozanhakan@gmail.com">Ozan Hakan Tunca</author>
   <maintainer email="t.ozanhakan@gmail.com">Ozan Hakan Tunca</maintainer>
-
   <license>BSD-3-Clause</license>
-
   <buildtool_depend>catkin</buildtool_depend>
   <depend>ultralytics_ros</depend>
   <depend>image_view</depend>
-
   <export>
   </export>
 </package>
diff --git a/auv_vision/auv_vision/CMakeLists.txt b/auv_vision/auv_vision/CMakeLists.txt
index d883fba0..9050124a 100644
--- a/auv_vision/auv_vision/CMakeLists.txt
+++ b/auv_vision/auv_vision/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED COMPONENTS
 )
 
 catkin_package(
-  CATKIN_DEPENDS 
+  CATKIN_DEPENDS
 )
 
 include_directories(
diff --git a/auv_vision/auv_vision/package.xml b/auv_vision/auv_vision/package.xml
index 47b8d0e8..453becb7 100644
--- a/auv_vision/auv_vision/package.xml
+++ b/auv_vision/auv_vision/package.xml
@@ -3,15 +3,11 @@
   <name>auv_vision</name>
   <version>0.1.0</version>
   <description>The auv_vision package</description>
-
   <maintainer email="mimarlarf@gmail.com">Faruk Mimarlar</maintainer>
   <author email="mimarlarf@gmail.com">Faruk Mimarlar</author>
-
   <license>BSD-3-Clause</license>
-
   <buildtool_depend>catkin</buildtool_depend>
   <depend>auv_detection</depend>
-
   <export>
   </export>
 </package>