diff --git a/CMakeLists.txt b/CMakeLists.txt
index 05e010a..c458e74 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -215,6 +215,15 @@ if(ODIN_TARGET_OBSERVATION_READY)
     message(STATUS "Target observation support enabled in odin_ros_driver")
 endif()
 
+function(set_runtime_search_path target_name runtime_path)
+    if(TARGET ${target_name})
+        set_target_properties(${target_name} PROPERTIES
+            BUILD_RPATH "${runtime_path}"
+            INSTALL_RPATH "${runtime_path}"
+        )
+    endif()
+endfunction()
+
 # ===== ROS1 Configuration =====
 if(ROS_VERSION STREQUAL "ROS1")
     message(STATUS "Configuring for ROS1 build")
@@ -323,9 +332,11 @@ if(ROS_VERSION STREQUAL "ROS1")
 # ===== ROS2 Configuration =====
 elseif(ROS_VERSION STREQUAL "ROS2")
     message(STATUS "Configuring for ROS2 build")
+    set(BUILD_SHARED_LIBS ON CACHE BOOL "Build shared libraries for ROS2 targets" FORCE)
     
     # Find all necessary ROS2 packages
     find_package(ament_cmake REQUIRED)
+    find_package(rosidl_default_generators REQUIRED)
     find_package(rclcpp REQUIRED)
     find_package(std_msgs REQUIRED)
     find_package(sensor_msgs REQUIRED)
@@ -339,6 +350,16 @@ elseif(ROS_VERSION STREQUAL "ROS2")
     find_package(tf2_ros REQUIRED)
     find_package(geometry_msgs REQUIRED)
     find_package(tf2_geometry_msgs REQUIRED)
+
+    rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/TargetObservation.msg"
+        DEPENDENCIES std_msgs geometry_msgs
+    )
+
+    rosidl_get_typesupport_target(odin_ros_driver_interfaces_target
+        ${PROJECT_NAME} "rosidl_typesupport_cpp")
+    rosidl_get_typesupport_target(odin_ros_driver_fastrtps_cpp_target
+        ${PROJECT_NAME} "rosidl_typesupport_fastrtps_cpp")
     
     # Create executable
     add_executable(host_sdk_sample 
@@ -423,8 +444,11 @@ elseif(ROS_VERSION STREQUAL "ROS2")
         src/cloud_reprojection_processing.cpp
     )
     target_compile_definitions(cloud_reprojection_ros2_node PRIVATE ROS2)
+    target_link_options(cloud_reprojection_ros2_node PRIVATE "-Wl,--no-as-needed")
     target_link_libraries(cloud_reprojection_ros2_node
         cloud_reprojector_ros2
+        ${odin_ros_driver_interfaces_target}
+        ${odin_ros_driver_fastrtps_cpp_target}
         ${OpenCV_LIBS}
         ${PCL_LIBRARIES}
         yaml-cpp
@@ -457,9 +481,31 @@ elseif(ROS_VERSION STREQUAL "ROS2")
         message_filters
     )
 
+    foreach(ros2_runtime_target
+        host_sdk_sample
+        pcd2depth_ros2_node
+        cloud_reprojection_ros2_node
+        image_overlay_node
+    )
+        set_runtime_search_path(${ros2_runtime_target} "\$ORIGIN/..")
+    endforeach()
+
+    foreach(ros2_library_target
+        pointcloud_depth_converter_ros2
+        depth_image_ros2_node_lib
+        cloud_reprojector_ros2
+    )
+        set_runtime_search_path(${ros2_library_target} "\$ORIGIN")
+    endforeach()
+
+    set_runtime_search_path(motcpp "\$ORIGIN")
+
     # Installation rules - ensure all install targets are defined before ament_package()
     # Install executable
     install(TARGETS 
+        pointcloud_depth_converter_ros2
+        depth_image_ros2_node_lib
+        cloud_reprojector_ros2
         host_sdk_sample 
         pcd2depth_ros2_node
         cloud_reprojection_ros2_node
@@ -469,6 +515,12 @@ elseif(ROS_VERSION STREQUAL "ROS2")
         LIBRARY DESTINATION lib
         RUNTIME DESTINATION lib/${PROJECT_NAME}
     )
+    if(TARGET motcpp)
+        install(TARGETS motcpp
+            LIBRARY DESTINATION lib
+            ARCHIVE DESTINATION lib
+        )
+    endif()
     
     # Install package.xml
     install(FILES package.xml
@@ -505,6 +557,7 @@ elseif(ROS_VERSION STREQUAL "ROS2")
     # Declare dependencies
     ament_export_dependencies(
         rclcpp 
+        rosidl_default_runtime
         std_msgs 
         sensor_msgs 
         nav_msgs 
diff --git a/config/odin_ros2.rviz b/config/odin_ros2.rviz
index 48d42c3..bc932ba 100644
--- a/config/odin_ros2.rviz
+++ b/config/odin_ros2.rviz
@@ -11,8 +11,9 @@ Panels:
         - /Odometry1
         - /slam1
         - /dense_depth_demo1
+        - /Prediction1
       Splitter Ratio: 0.5
-    Tree Height: 593
+    Tree Height: 865
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -48,7 +49,7 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Class: rviz_default_plugins/Image
-      Enabled: true
+      Enabled: false
       Max Value: 1
       Median window: 5
       Min Value: 0
@@ -60,7 +61,7 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /odin1/image
-      Value: true
+      Value: false
     - Class: rviz_default_plugins/Image
       Enabled: false
       Max Value: 1
@@ -407,6 +408,67 @@ Visualization Manager:
             {}
       Update Interval: 0
       Value: true
+    - Class: rviz_common/Group
+      Displays:
+        - Class: rviz_default_plugins/Marker
+          Enabled: true
+          Name: Marker
+          Namespaces:
+            {}
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /target/pred_trajectory
+          Value: true
+        - Class: rviz_default_plugins/Image
+          Enabled: false
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: Image
+          Normalize Range: true
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /target/pred_image
+          Value: false
+        - Alpha: 1
+          Class: rviz_default_plugins/PointStamped
+          Color: 224; 27; 36
+          Enabled: true
+          History Length: 10
+          Name: target_pos_world
+          Radius: 0.30000001192092896
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /odin1/sync/target_pos_world
+          Value: true
+        - Alpha: 1
+          Class: rviz_default_plugins/PointStamped
+          Color: 204; 41; 204
+          Enabled: true
+          History Length: 1
+          Name: target_pos_cam
+          Radius: 0.20000000298023224
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /odin1/sync/target_pos_cam
+          Value: true
+      Enabled: true
+      Name: Prediction
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -452,26 +514,26 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 10.9336576461792
+      Class: rviz_default_plugins/ThirdPersonFollower
+      Distance: 23.027545928955078
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.2568470537662506
-        Y: 2.1451337337493896
-        Z: 0.3774382472038269
+        X: 0
+        Y: 0
+        Z: 0
       Focal Shape Fixed Size: false
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3953983187675476
-      Target Frame: odom
-      Value: Orbit (rviz)
-      Yaw: 3.230407953262329
+      Pitch: 0.5153985023498535
+      Target Frame: odin1_base_link
+      Value: ThirdPersonFollower (rviz_default_plugins)
+      Yaw: 2.1653902530670166
     Saved: ~
 Window Geometry:
   Displays:
@@ -483,16 +545,16 @@ Window Geometry:
     collapsed: false
   Image_undistort:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001d40000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000028e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d10000010a0000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a006500630074006500640000000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004910000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001d40000039efc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000001f7000001700000002800fffffffb0000000a0049006d006100670065000000036d0000006e0000002800fffffffb0000002200640065006e00730065005f00640065007000740068005f0069006d006100670065000000023e0000007a0000002800fffffffb0000001e0049006d006100670065005f0075006e0064006900730074006f00720074000000028f0000014c0000002800fffffffb0000002a0063006c006f007500640073006c0061006d005f0072006500700072006f006a006500630074006500640000000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000044b0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1920
-  X: 540
-  Y: 124
+  Width: 1850
+  X: 70
+  Y: 27
   cloudslam_reprojected:
     collapsed: false
   dense_depth_image:
diff --git a/include/cloud_reprojection_ros_node.hpp b/include/cloud_reprojection_ros_node.hpp
index eeac4c2..9dd792d 100644
--- a/include/cloud_reprojection_ros_node.hpp
+++ b/include/cloud_reprojection_ros_node.hpp
@@ -25,6 +25,7 @@ limitations under the License.
     #include <cv_bridge/cv_bridge.h>
     #include <message_filters/subscriber.h>
     #include <message_filters/sync_policies/approximate_time.h>
+    #include "odin_ros_driver/msg/target_observation.hpp"
 #else
     #include <ros/ros.h>
     #include <sensor_msgs/PointCloud2.h>
@@ -87,7 +88,6 @@ private:
     std::string sync_wiwc_topic_;
     std::string sync_image_topic_;
     std::string sync_overlay_image_topic_;
-    std::string sync_target_track_id_topic_;
     std::string sync_target_observation_topic_;
     std::string sync_target_pos_cam_topic_;
     std::string sync_target_pos_world_topic_;
@@ -100,8 +100,7 @@ private:
 
     rclcpp::Publisher<CompressedImage>::SharedPtr overlay_compressed_pub_;  // optional if send_overlay_
     rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_;  // optional if publish_combined_compressed_
-    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr target_track_id_pub_;
-    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr target_observation_pub_;
+    rclcpp::Publisher<odin_ros_driver::msg::TargetObservation>::SharedPtr target_observation_pub_;
     rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr target_pos_cam_pub_;
     rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr target_pos_world_pub_;
 
diff --git a/include/yaml_parser.h b/include/yaml_parser.h
index 81a2b65..8c2f8aa 100644
--- a/include/yaml_parser.h
+++ b/include/yaml_parser.h
@@ -87,9 +87,20 @@ private:
     std::map<std::string, std::string> register_keys_str_val_;
     std::map<std::string, ParameterValue> custom_parameters_;
 
-    std::unordered_set<std::string> allowed_key_w_str_val = {"relocalization_map_abs_path", "mapping_result_dest_dir", "mapping_result_file_name", "image_mask_abs_path"};
+    std::unordered_set<std::string> allowed_key_w_str_val = {
+        "relocalization_map_abs_path",
+        "mapping_result_dest_dir",
+        "mapping_result_file_name",
+        "image_mask_abs_path",
+        "overlay_reprojected_topic",
+        "overlay_camera_topic",
+        "overlay_output_topic",
+        "sync_camera_topic",
+        "sync_topic_prefix",
+        "combined_compressed_topic"
+    };
 };
 
 }
 
-#endif
\ No newline at end of file
+#endif
diff --git a/msg/TargetObservation.msg b/msg/TargetObservation.msg
new file mode 100644
index 0000000..4415e15
--- /dev/null
+++ b/msg/TargetObservation.msg
@@ -0,0 +1,11 @@
+std_msgs/Header header
+bool valid
+int32 track_id
+int32 detection_index
+float32 confidence
+float32 depth
+float32 depth_confidence
+float32[4] bbox_xyxy
+float32[51] keypoints_xyc
+geometry_msgs/Point target_pos_cam
+geometry_msgs/Point target_pos_world
diff --git a/package.xml b/package.xml
index b277f0b..19b00f0 100755
--- a/package.xml
+++ b/package.xml
@@ -7,6 +7,7 @@
   <license>Apache 2.0</license>
   <!-- ROS2 uses colcon as the build tool -->
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
   <!-- ROS2 dependencies -->
   <depend>rclcpp</depend>
   <!-- System dependencies -->
@@ -21,8 +22,10 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
   <!-- Specify build type as ament -->
   <export>  
     <build_type>ament_cmake</build_type>  
   </export> 
-</package>
\ No newline at end of file
+</package>
diff --git a/package_ros2.xml b/package_ros2.xml
index b277f0b..19b00f0 100644
--- a/package_ros2.xml
+++ b/package_ros2.xml
@@ -7,6 +7,7 @@
   <license>Apache 2.0</license>
   <!-- ROS2 uses colcon as the build tool -->
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
   <!-- ROS2 dependencies -->
   <depend>rclcpp</depend>
   <!-- System dependencies -->
@@ -21,8 +22,10 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
   <!-- Specify build type as ament -->
   <export>  
     <build_type>ament_cmake</build_type>  
   </export> 
-</package>
\ No newline at end of file
+</package>
diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp
index f08ac63..8220c50 100644
--- a/src/cloud_reprojection_ros.cpp
+++ b/src/cloud_reprojection_ros.cpp
@@ -31,6 +31,7 @@ limitations under the License.
 #include <geometry_msgs/msg/point_stamped.hpp>
 #include <std_msgs/msg/float32_multi_array.hpp>
 #include <std_msgs/msg/int32.hpp>
+#include "odin_ros_driver/msg/target_observation.hpp"
 #endif
 
 namespace {
@@ -131,7 +132,6 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
 #ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
         << "\n  process_target_observation: " << (enable_target_observation_ ? "on" : "off")
         << "\n  debug: " << (debug_target_observation_ ? "on" : "off")
-        << "\n  target_track_id_topic: " << sync_target_track_id_topic_
         << "\n  target_observation_topic: " << sync_target_observation_topic_
         << "\n  target_pos_cam_topic: " << sync_target_pos_cam_topic_
         << "\n  target_pos_world_topic: " << sync_target_pos_world_topic_
@@ -163,9 +163,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
     }
 #ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
     if (enable_target_observation_) {
-        target_track_id_pub_ = this->create_publisher<std_msgs::msg::Int32>(
-            sync_target_track_id_topic_, 10);
-        target_observation_pub_ = this->create_publisher<std_msgs::msg::Float32MultiArray>(
+        target_observation_pub_ = this->create_publisher<odin_ros_driver::msg::TargetObservation>(
             sync_target_observation_topic_, 10);
         target_pos_cam_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
             sync_target_pos_cam_topic_, 10);
@@ -173,8 +171,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
             sync_target_pos_world_topic_, 10);
         RCLCPP_INFO(
             this->get_logger(),
-            "Target observation publishers created successfully | track_id=%s | observation=%s | pos_cam=%s | pos_world=%s",
-            sync_target_track_id_topic_.c_str(),
+            "Target observation publishers created successfully | observation=%s | pos_cam=%s | pos_world=%s",
             sync_target_observation_topic_.c_str(),
             sync_target_pos_cam_topic_.c_str(),
             sync_target_pos_world_topic_.c_str());
@@ -236,7 +233,6 @@ void CloudReprojectionRosNode::loadParameters()
     sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
     sync_image_topic_ = sync_topic_prefix_ + "/image";
     sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
-    sync_target_track_id_topic_ = sync_topic_prefix_ + "/target_track_id";
     sync_target_observation_topic_ = sync_topic_prefix_ + "/target_observation";
     sync_target_pos_cam_topic_ = sync_topic_prefix_ + "/target_pos_cam";
     sync_target_pos_world_topic_ = sync_topic_prefix_ + "/target_pos_world";
@@ -442,7 +438,8 @@ void CloudReprojectionRosNode::syncCallback(
     cloud_cam_msg.header = image_msg->header;
     cloud_cam_msg.header.stamp = sync_stamp;
     if (cloud_cam_msg.header.frame_id.empty()) {
-        cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
+        // cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
+        cloud_cam_msg.header.frame_id = odom_msg->child_frame_id; // odin1_base_link
     }
 
 
@@ -567,29 +564,23 @@ void CloudReprojectionRosNode::syncCallback(
         }
 
         if (target_observation.valid) {
-            std_msgs::msg::Int32 track_id_msg;
-            track_id_msg.data = target_observation.track_id;
-            target_track_id_pub_->publish(track_id_msg);
-
-            std_msgs::msg::Float32MultiArray observation_msg;
-            observation_msg.data.reserve(4 + 17 * 3 + 3 + 3 + 3);
-            observation_msg.data.insert(
-                observation_msg.data.end(),
-                target_observation.bbox_xyxy.begin(),
-                target_observation.bbox_xyxy.end());
-            observation_msg.data.insert(
-                observation_msg.data.end(),
-                target_observation.keypoints_xyc.begin(),
-                target_observation.keypoints_xyc.end());
-            observation_msg.data.push_back(target_observation.target_pos_cam.x());
-            observation_msg.data.push_back(target_observation.target_pos_cam.y());
-            observation_msg.data.push_back(target_observation.target_pos_cam.z());
-            observation_msg.data.push_back(target_observation.target_pos_world.x());
-            observation_msg.data.push_back(target_observation.target_pos_world.y());
-            observation_msg.data.push_back(target_observation.target_pos_world.z());
-            observation_msg.data.push_back(target_observation.confidence);
-            observation_msg.data.push_back(target_observation.depth);
-            observation_msg.data.push_back(target_observation.depth_confidence);
+            odin_ros_driver::msg::TargetObservation observation_msg;
+            observation_msg.header = image_msg->header;
+            observation_msg.header.stamp = sync_stamp;
+            observation_msg.valid = target_observation.valid;
+            observation_msg.track_id = target_observation.track_id;
+            observation_msg.detection_index = target_observation.detection_index;
+            observation_msg.confidence = target_observation.confidence;
+            observation_msg.depth = target_observation.depth;
+            observation_msg.depth_confidence = target_observation.depth_confidence;
+            observation_msg.bbox_xyxy = target_observation.bbox_xyxy;
+            observation_msg.keypoints_xyc = target_observation.keypoints_xyc;
+            observation_msg.target_pos_cam.x = target_observation.target_pos_cam.x();
+            observation_msg.target_pos_cam.y = target_observation.target_pos_cam.y();
+            observation_msg.target_pos_cam.z = target_observation.target_pos_cam.z();
+            observation_msg.target_pos_world.x = target_observation.target_pos_world.x();
+            observation_msg.target_pos_world.y = target_observation.target_pos_world.y();
+            observation_msg.target_pos_world.z = target_observation.target_pos_world.z();
             target_observation_pub_->publish(observation_msg);
 
             geometry_msgs::msg::PointStamped pos_cam_msg;