checked out right branch

.gitmodules

@@ -1,20 +1,10 @@
-[submodule "src/witmotion_ros"]
-	path = src/witmotion_ros
-	url = https://github.com/ElettraSciComp/witmotion_IMU_ros.git
-	branch = ros2
-[submodule "src/Livox-SDK2"]
-	path = src/Livox-SDK2
-	url = https://github.com/tu-darmstadt-ros-pkg/Livox-SDK2.git
-	branch = jazzy
-[submodule "src/livox_ros_driver2"]
-	path = src/livox_ros_driver2
-	url = https://github.com/tu-darmstadt-ros-pkg/livox_ros_driver2
-	branch = jazzy
-[submodule "src/FAST_LIO"]
-	path = src/FAST_LIO
-	url = https://github.com/Ericsii/FAST_LIO.git
-	branch = ros2
 [submodule "src/livox_ros2_driver"]
 	path = src/livox_ros2_driver
 	url = https://github.com/Livox-SDK/livox_ros2_driver
 	branch = fix_build_error
+[submodule "src/scans_merger"]
+	path = src/scans_merger
+	url = https://github.com/LCAS/scans_merger.git
+[submodule "src/micro-ROS-Agent"]
+	path = src/micro-ROS-Agent
+	url = https://github.com/micro-ROS/micro-ROS-Agent.git

scripts/calibrate_lidar.py

@@ -13,6 +13,7 @@ import threading
 import time
 import open3d as o3d
 from scipy.spatial.transform import Rotation as R
+from datetime import datetime
 
 class PointCloudSaver(Node):
     def __init__(self, node_name: str, pointcloud_topic: str, buffer, timeout_ms: int):
@@ -83,7 +84,7 @@ class LidarTransformPublisher(Node):
         self.lidar1_buffer = lidar1_buffer
         self.lidar2_buffer = lidar2_buffer
         self.lidar1_frame = lidar1_frame
-        self.lidar2_frame = lidar2_frame
+        self.lidar2_frame = "livox_base" # lidar2_frame
 
     def publish(self):
         self.pcd_1 = self.pcd_buffer_to_o3d(self.lidar1_buffer)
@@ -111,6 +112,11 @@ class LidarTransformPublisher(Node):
 
         self.br.sendTransform(t)
         self.get_logger().info("Published static transform.")
+        if input("Is it acurate? [y/N] ").lower() == 'y':
+            file_name = f"calibration/{datetime.now().strftime("%Y_%m_%d-%H_%M_%S")}.npy"
+            np.save(file_name, self.T)
+            self.get_logger().info(f"Saved {file_name}")
+        
 
     def pcd_buffer_to_o3d(self, buffer: BytesIO):
         buffer.seek(0)
@@ -195,7 +201,7 @@ class LidarTransformPublisher(Node):
         return transformation
 
 
-def monitor_nodes(nodes, publisher):
+def monitor_nodes(nodes, executor, publisher):
     while rclpy.ok():
         if all(node.finished for node in nodes):
             print("All pointclouds captured")
@@ -210,12 +216,12 @@ def main():
 
     executor = MultiThreadedExecutor()
     nodes = [
-        PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', buffer_velodyne, 350),
+        PointCloudSaver('velodyne_pcd_saver', '/lidar/vlp16/points', buffer_velodyne, 3_500),
-        PointCloudSaver('livox_pcd_saver', '/livox/lidar', buffer_livox, 1000),
+        PointCloudSaver('livox_pcd_saver', '/lidar/mid40/points', buffer_livox, 5_000),
     ]
     publisher = LidarTransformPublisher(buffer_velodyne, 'velodyne', buffer_livox, 'frame_default')
 
-    monitor_thread = threading.Thread(target=monitor_nodes, args=(nodes,publisher), daemon=True)
+    monitor_thread = threading.Thread(target=monitor_nodes, args=(nodes,executor,publisher), daemon=True)
     monitor_thread.start()
 
     for node in nodes:

scripts/plot.py

@@ -0,0 +1,103 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float32
+import matplotlib.pyplot as plt
+import matplotlib.animation as animation
+import threading
+
+class MultiPlotter(Node):
+    def __init__(self):
+        super().__init__("multi_plotter")
+
+        # Data buffers
+        self.angle = []
+        self.velocity = []
+        self.acceleration = []
+
+        # Subscriptions
+        self.create_subscription(Float32, '/nucleo/stepper0/position',
+                                 self.angle_callback, 10)
+        self.create_subscription(Float32, '/nucleo/stepper0/velocity',
+                                 self.velocity_callback, 10)
+        self.create_subscription(Float32, '/nucleo/stepper0/acceleration',
+                                 self.accel_callback, 10)
+
+    def angle_callback(self, msg):
+        self.angle.append(msg.data)
+        self._trim()
+
+    def velocity_callback(self, msg):
+        self.velocity.append(msg.data)
+        self._trim()
+
+    def accel_callback(self, msg):
+        self.acceleration.append(msg.data)
+        self._trim()
+
+    def _trim(self, limit=2000):
+        """Keep buffers short so memory stays small."""
+        if len(self.angle) > limit:
+            self.angle.pop(0)
+        if len(self.velocity) > limit:
+            self.velocity.pop(0)
+        if len(self.acceleration) > limit:
+            self.acceleration.pop(0)
+
+
+def animate(i, node, axes):
+    # Clear each subplot
+    for ax in axes:
+        ax.cla()
+
+    # Angle
+    if len(node.angle) > 0:
+        axes[0].plot(node.angle, color='red')
+    axes[0].set_title("Angle")
+    axes[0].grid(True)
+
+    # Velocity
+    if len(node.velocity) > 0:
+        axes[1].plot(node.velocity, color='green')
+    axes[1].set_title("Velocity")
+    axes[1].grid(True)
+
+    # Acceleration
+    if len(node.acceleration) > 0:
+        axes[2].plot(node.acceleration, color='blue')
+    axes[2].set_title("Acceleration")
+    axes[2].grid(True)
+
+    # Shared axis labels
+    axes[2].set_xlabel("Samples")
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = MultiPlotter()
+
+    # Start ROS spinning in the background
+    thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    thread.start()
+
+    # Create three subplots
+    fig, axes = plt.subplots(3, 1, figsize=(8, 8), sharex=True)
+
+    # Persist animation object
+    global ani
+    ani = animation.FuncAnimation(
+        fig,
+        animate,
+        fargs=(node, axes),
+        interval=16
+    )
+
+    plt.tight_layout()
+    plt.show()
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

scripts/publish_lidar_offset.py

@@ -1,87 +0,0 @@
-#!/usr/bin/env python3
-import rclpy
-from rclpy.node import Node
-from tf2_ros import StaticTransformBroadcaster
-from geometry_msgs.msg import TransformStamped
-import numpy as np
-import open3d as o3d
-from scipy.spatial.transform import Rotation as R
-
-class StaticTransformPublisher(Node):
-    def __init__(self):
-        super().__init__('static_transform_from_pointclouds')
-
-        # Static TF broadcaster
-        self.br = StaticTransformBroadcaster(self)
-
-        # Pointcloud files
-        self.velodyne_file = "/root/velodyne.pcd"
-        self.livox_file = "/root/livox.pcd"
-
-        # Frames
-        self.livox_frame = "frame_default"
-        self.velodyne_frame = "velodyne"
-
-        # Compute transform once
-        self.T = self.compute_transform()
-        self.get_logger().info(f"Computed initial transform:\n{self.T}")
-
-        # Prepare translation and rotation
-        T_copy = np.array(self.T, copy=True)
-        trans = T_copy[:3, 3]
-        rot_quat = R.from_matrix(T_copy[:3, :3]).as_quat()  # [x, y, z, w]
-
-        # Create static TransformStamped
-        t = TransformStamped()
-        t.header.stamp.sec = 0
-        t.header.stamp.nanosec = 0
-        t.header.frame_id = self.velodyne_frame
-        t.child_frame_id = self.livox_frame
-        t.transform.translation.x = trans[0]
-        t.transform.translation.y = trans[1]
-        t.transform.translation.z = trans[2]
-        t.transform.rotation.x = rot_quat[0]
-        t.transform.rotation.y = rot_quat[1]
-        t.transform.rotation.z = rot_quat[2]
-        t.transform.rotation.w = rot_quat[3]
-
-        # Publish once
-        self.br.sendTransform(t)
-        self.get_logger().info("Published static transform.")
-
-    def compute_transform(self):
-        # Load point clouds
-        pcd_vel = o3d.io.read_point_cloud(self.velodyne_file)
-        pcd_liv = o3d.io.read_point_cloud(self.livox_file)
-
-        # Downsample
-        voxel_size = 0.05
-        pcd_vel_ds = pcd_vel.voxel_down_sample(voxel_size)
-        pcd_liv_ds = pcd_liv.voxel_down_sample(voxel_size)
-
-        # Estimate normals
-        pcd_vel_ds.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(knn=20))
-        pcd_liv_ds.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(knn=20))
-
-        # ICP registration
-        threshold = 0.5
-        reg_result = o3d.pipelines.registration.registration_icp(
-            pcd_liv_ds, pcd_vel_ds, threshold,
-            np.eye(4),
-            o3d.pipelines.registration.TransformationEstimationPointToPoint()
-        )
-        return reg_result.transformation
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = StaticTransformPublisher()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-if __name__ == '__main__':
-    main()

scripts/save_pointcloud.py

@@ -1,113 +0,0 @@
-import rclpy
-from rclpy.node import Node
-from rclpy.executors import MultiThreadedExecutor
-from sensor_msgs.msg import PointCloud2
-import sensor_msgs_py.point_cloud2 as pc2
-import numpy as np
-import matplotlib.pyplot as plt
-import struct
-from io import BytesIO
-import threading
-import time
-
-class PointCloudSaver(Node):
-    def __init__(self, node_name: str, pointcloud_topic: str, buffer, timeout_ms: int):
-        super().__init__(node_name)
-        self.subscription = self.create_subscription(
-            PointCloud2,
-            pointcloud_topic,
-            self.callback,
-            10
-        )
-        self.buffer = buffer
-        self.finished = False
-        self.points = []
-        self.end_time = self.get_clock().now().nanoseconds + (timeout_ms * 1_000_000)
-        self.cmap = plt.get_cmap('jet') 
-
-    def callback(self, msg):
-        now = self.get_clock().now().nanoseconds
-        for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
-            self.points.append([p[0], p[1], p[2], p[3]])
-
-        if now > self.end_time:
-            if not self.points:
-                self.get_logger().warn("No points received!")
-                self.destroy_node()
-                self.finished = True
-                return
-
-            np_points = np.array(self.points, dtype=np.float32)
-            intensities = np_points[:, 3]
-            norm_int = (intensities - intensities.min()) / (intensities.ptp() + 1e-8)
-
-            # Map normalized intensity to RGB colormap
-            colors = self.cmap(norm_int)[:, :3]  # RGB 0-1
-            colors = (colors * 255).astype(np.uint8)
-            rgb_int = np.left_shift(colors[:,0].astype(np.uint32), 16) | \
-                      np.left_shift(colors[:,1].astype(np.uint32), 8)  | \
-                      colors[:,2].astype(np.uint32)
-
-            filename = "pointcloud.pcd"
-            self.write_pcd_with_intensity_rgb(filename, np_points, rgb_int)
-            self.get_logger().info(f"Saved {filename}")
-            self.destroy_node()
-            self.finished = True
-
-    def write_pcd_with_intensity_rgb(self, filename, points, rgb_int):
-        header = f"""# .PCD v0.7 - Point Cloud Data file format
-VERSION 0.7
-FIELDS x y z intensity rgb
-SIZE 4 4 4 4 4
-TYPE F F F F U
-COUNT 1 1 1 1 1
-WIDTH {points.shape[0]}
-HEIGHT 1
-VIEWPOINT 0 0 0 1 0 0 0
-POINTS {points.shape[0]}
-DATA binary
-"""
-        self.buffer.write(header.encode('ascii'))
-        for i in range(points.shape[0]):
-            # x, y, z, intensity as float32, rgb as uint32
-            self.buffer.write(struct.pack('ffffI', points[i,0], points[i,1], points[i,2], points[i,3], rgb_int[i]))
-
-def monitor_nodes(nodes):
-    """Separate thread that monitors node status and shuts down ROS when done."""
-    while rclpy.ok():
-        if all(node.finished for node in nodes):
-            print("All nodes finished. Shutting down ROS.")
-            rclpy.shutdown()
-            break
-        time.sleep(0.1)  # check periodically
-
-def main():
-    rclpy.init()
-    file_velodyne = open('/root/velodyne.pcd', "wb+")
-    file_livox = open('/root/livox.pcd', "wb+")
-
-    executor = MultiThreadedExecutor()
-
-    nodes = [
-        PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', file_velodyne, 5000),
-        PointCloudSaver('livox_pcd_saver', '/livox/lidar', file_livox, 5000),
-    ]
-
-    monitor_thread = threading.Thread(target=monitor_nodes, args=(nodes,), daemon=True)
-    monitor_thread.start()
-
-    for node in nodes:
-        executor.add_node(node)
-    try:
-        executor.spin()
-    finally:
-        monitor_thread.join()
-        print("Executor and monitor thread exited cleanly.")
-
-        file_velodyne.close()
-        file_livox.close()
-
-
-if __name__ == "__main__":
-    main()
-

src/sherpa/launch/calibration.launch.py

@@ -0,0 +1,104 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+import math
+
+def generate_launch_description():
+    # ----- microros -----
+    servo_uros_node = Node(
+        package='micro_ros_agent',
+        executable='micro_ros_agent',
+        arguments=["serial", "--dev", "/dev/ttyACM0", "--baud", "921600"],
+    )
+
+    servo_angle_tf_node = Node(
+        package='sherpa_lidar_utils',
+        executable='servo_angle_tf.py',
+    )
+
+    # ----- lidar position -----
+    lidar_position_base_tf_node = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=['0', '0', '0', '0', '0', '0', '1', 'velodyne', 'lidar_base'],
+    )
+
+    # ----- lidar topics -----
+    vlp16_driver_node = Node(
+        package='velodyne_driver',
+        executable='velodyne_driver_node',
+        arguments=["--ros-args", "-p", "model:=VLP16", "-p", "rpm:=600.0", "-p", "device_ip:=10.42.200.201"],
+        remappings=[
+            ('/velodyne_packets', '/lidar/vlp16/packets'),
+        ],
+    )
+
+    vlp16_pointcloud_node = Node(
+        package='velodyne_pointcloud',
+        executable='velodyne_transform_node',
+        arguments=["--ros-args", "-p", f"calibration:={os.path.join(get_package_share_directory('sherpa'), 'config', 'vlp-16-pointcloud.yaml')}", "-p", "model:=VLP16"],
+        remappings=[
+            ('/velodyne_packets', '/lidar/vlp16/packets'),
+            ('/velodyne_points', '/lidar/vlp16/points'),
+        ]
+    )
+
+    mid40_driver_node = Node(
+        package='livox_ros2_driver',
+        executable='livox_ros2_driver_node',
+        arguments=[],
+        remappings=[
+            ('/livox/lidar', '/lidar/mid40/points'),
+        ],
+    )
+
+    # ----- pointcloud downsampling -----
+    downsample_vlp16_node = Node(
+        package='target_tracking',
+        executable='cloud_preprocessing_node',
+        parameters=[
+            {"topic_in": "/lidar/vlp16/points"},
+            {"topic_out": "/lidar/vlp16/downsampled"},
+            {"x_min": 0.0},
+            {"x_max": 100.0},
+            {"z_min": -5.0},
+            {"z_max": 10.0},
+            {"tan_h_fov": math.pi}, # 180°  
+            {"tan_v_fov": math.pi / 5}, # ~36°  
+        ],
+    )
+
+    downsample_mid40_node = Node(
+        package='target_tracking',
+        executable='cloud_preprocessing_node',
+        parameters=[
+            {"topic_in": "/lidar/mid40/points"},
+            {"topic_out": "/lidar/mid40/downsampled"},
+            {"x_min": 0.0},
+            {"x_max": 100.0},
+            {"z_min": -5.0},
+            {"z_max": 10.0},
+            {"tan_h_fov": math.pi / 4}, # 45°  
+            {"tan_v_fov": math.pi / 4}, # 45°  
+        ],
+    )
+
+    # ----- create ld -----
+    ld = LaunchDescription()   
+
+    ld.add_action(servo_uros_node)
+    ld.add_action(servo_angle_tf_node)
+    ld.add_action(lidar_position_base_tf_node)
+    ld.add_action(vlp16_driver_node)
+    ld.add_action(vlp16_pointcloud_node)
+    ld.add_action(mid40_driver_node)
+    ld.add_action(downsample_vlp16_node)
+    ld.add_action(downsample_mid40_node)
+    
+    return ld

src/sherpa/launch/tracking.launch.py

@@ -0,0 +1,149 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+import math
+
+def generate_launch_description():
+    # ----- microros -----
+    servo_uros_node = Node(
+        package='micro_ros_agent',
+        executable='micro_ros_agent',
+        arguments=["serial", "--dev", "/dev/ttyACM0", "--baud", "921600"],
+    )
+
+    servo_angle_tf_node = Node(
+        package='sherpa_lidar_utils',
+        executable='servo_angle_tf.py',
+    )
+
+    # ----- lidar position -----
+    lidar_position_tf_node = Node(
+        package='sherpa_lidar_utils',
+        executable='publish_calibration.py',
+        arguments=["--ros-args", "-p", "calibration:=/home/sherpa/lidar_calibration"],
+    )
+
+    lidar_position_base_tf_node = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=['0', '0', '0', '0', '0', '0', '1', 'velodyne', 'lidar_base'],
+    )
+
+    # ----- lidar topics -----
+    vlp16_driver_node = Node(
+        package='velodyne_driver',
+        executable='velodyne_driver_node',
+        arguments=["--ros-args", "-p", "model:=VLP16", "-p", "rpm:=600.0", "-p", "device_ip:=10.42.200.201"],
+        remappings=[
+            ('/velodyne_packets', '/lidar/vlp16/packets'),
+        ],
+    )
+
+    vlp16_pointcloud_node = Node(
+        package='velodyne_pointcloud',
+        executable='velodyne_transform_node',
+        arguments=["--ros-args", "-p", f"calibration:={os.path.join(get_package_share_directory('sherpa'), 'config', 'vlp-16-pointcloud.yaml')}", "-p", "model:=VLP16"],
+        remappings=[
+            ('/velodyne_packets', '/lidar/vlp16/packets'),
+            ('/velodyne_points', '/lidar/vlp16/points'),
+        ]
+    )
+
+    mid40_driver_node = Node(
+        package='livox_ros2_driver',
+        executable='livox_ros2_driver_node',
+        arguments=[],
+        remappings=[
+            ('/livox/lidar', '/lidar/mid40/points'),
+        ],
+    )
+
+    # ----- pointcloud merging -----
+    pointcloud_merger_node = Node(
+        package='scans_merger',
+        executable='scans_merger',
+        parameters=[
+            {"destination_frame": "lidar_base"},
+            {"input_cloud_1": "/lidar/vlp16/points"},
+            {"input_cloud_2": "/lidar/mid40/points"},
+            {"merged_cloud": "/lidar/merged/points"},
+        ],
+    )
+
+     # ----- pointcloud downsampling -----
+    downsample_node = Node(
+        package='target_tracking',
+        executable='cloud_preprocessing_node',
+        parameters=[
+            {"topic_in": "/lidar/merged/points"},
+            {"topic_out": "/lidar/merged/downsampled"},
+            {"x_min": 0.0},
+            {"x_max": 100.0},
+            {"z_min": -5.0},
+            {"z_max": 10.0},
+            {"tan_h_fov": math.pi}, # 180°  
+            {"tan_v_fov": math.pi / 4}, # ~45°  
+        ],
+    )
+
+    # ----- tracking -----
+    cloud_clustering_node = Node(
+        package='target_tracking',
+        executable='cloud_clustering_node',
+        parameters=[
+            {"topic_in": "/lidar/merged/downsampled"},
+            {"frame_id": 'lidar_base'},
+            {"z_dim_scale": 1.0},
+            #{"cluster_tolerance": 0.065},
+            #{"min_cluster_size": 450},
+            #{"max_cluster_size": 1650},
+            {"min_cluster_size": 50},
+            {"max_cluster_size": 7500},
+            {"cluster_tolerance": 0.065},
+            {"min_width": 0.3},
+            {"min_height": 1.0},
+            {"min_length": 0.3},
+            {"max_width": 2.0},
+            {"max_height": 2.5},
+            {"max_length": 2.0},
+        ],
+        remappings=[
+            ('/cluster0', '/tracking/cluster0'),
+            ('/cluster1', '/tracking/cluster1'),
+            ('/cluster2', '/tracking/cluster2'),
+            ('/cluster3', '/tracking/cluster3'),
+            ('/cluster4', '/tracking/cluster4'),
+            ('/cluster5', '/tracking/cluster5'),
+        ],
+    )
+
+    servo_tracking_node = Node(
+        package='sherpa_lidar_utils',
+        executable='track_cluster.py',
+        remappings=[
+            ('/tracking/target', '/tracking/cluster0'),
+        ],
+    )
+
+    # ----- create ld -----
+    ld = LaunchDescription()   
+
+    ld.add_action(servo_uros_node)
+    ld.add_action(servo_angle_tf_node)
+    ld.add_action(lidar_position_tf_node)
+    ld.add_action(lidar_position_base_tf_node)
+    ld.add_action(vlp16_driver_node)
+    ld.add_action(vlp16_pointcloud_node)
+    ld.add_action(mid40_driver_node)
+    ld.add_action(pointcloud_merger_node)
+    ld.add_action(downsample_node)
+    ld.add_action(cloud_clustering_node)
+    ld.add_action(servo_tracking_node)
+
+    return ld

src/sherpa_lidar_utils/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.8)
+project(sherpa_lidar_utils)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclpy REQUIRED)
+
+# Install the Python script into the lib directory of the package
+install(PROGRAMS
+  src/publish_calibration.py
+  src/servo_angle_tf.py
+  src/track_cluster.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

src/sherpa_lidar_utils/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>sherpa_lidar_utils</name>
+  <version>0.0.1</version>
+  <description>Studienprojekt</description>
+  <maintainer email="timoschneider04@gmail.com">timo</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <!-- Dependencies for the robot control system -->
+  <depend>rclpy</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/sherpa_lidar_utils/src/publish_calibration.py

@@ -0,0 +1,126 @@
+#!/usr/bin/env python3
+"""
+average_tf_publisher.py
+Compute the mean of all 4x4 transform .npy files in a folder
+and broadcast it as a static TF at 50 Hz.
+
+Usage:
+    ros2 run <your_package> average_tf_publisher.py [directory] [parent_frame] [child_frame]
+
+Default parameters:
+    directory     = "calibration"
+    parent_frame  = "world"
+    child_frame   = "average_lidar"
+"""
+
+import os
+import sys
+import glob
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+import rclpy
+from rclpy.node import Node
+from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
+from geometry_msgs.msg import TransformStamped
+
+
+class AverageTFPublisher(Node):
+    def __init__(self):
+        super().__init__("average_tf_publisher")
+        self.declare_parameter('calibration', 'calibration')
+        self.declare_parameter('parent_frame', 'velodyne')
+        self.declare_parameter('child_frame', 'livox_base')
+        self.declare_parameter('hz', 50.0)
+
+        # Read the values
+        directory    = self.get_parameter('calibration').value
+        self.parent_frame = self.get_parameter('parent_frame').value
+        self.child_frame  = self.get_parameter('child_frame').value
+
+        self.get_logger().info(f"Loading transforms from: {directory}")
+        transforms = self._load_transforms(directory)
+        if len(transforms) == 0:
+            raise RuntimeError(f"No valid .npy transforms found in {directory}")
+
+        self.get_logger().info(f"Loaded {len(transforms)} transforms.")
+        self.mean_t, self.mean_q = self._compute_mean_transform(transforms)
+        self.get_logger().info(
+            f"Average translation: {self.mean_t}\n"
+            f"Average quaternion:   {self.mean_q}"
+        )
+
+        self.br = StaticTransformBroadcaster(self)
+
+        # Publish at 50 Hz (static TF can be published once, but continuous
+        # publishing helps late-joining nodes)
+        period = 1.0 / self.get_parameter('hz').value
+        self.timer = self.create_timer(period, self._timer_cb)
+
+    def _load_transforms(self, directory):
+        """Load all 4x4 numpy transforms from directory."""
+        files = sorted(glob.glob(os.path.join(directory, "*.npy")))
+        mats = []
+        for f in files:
+            try:
+                T = np.load(f)
+                if T.shape == (4, 4):
+                    mats.append(T)
+                else:
+                    self.get_logger().warn(f"Skipping {f}: wrong shape {T.shape}")
+            except Exception as e:
+                self.get_logger().warn(f"Failed loading {f}: {e}")
+        return mats
+
+    def _compute_mean_transform(self, mats):
+        """Return (mean_translation, mean_quaternion)"""
+        translations = []
+        quats = []
+        for T in mats:
+            translations.append(T[:3, 3])
+            q = R.from_matrix(T[:3, :3]).as_quat()  # [x,y,z,w]
+            quats.append(q)
+
+        # mean translation
+        t_mean = np.mean(np.vstack(translations), axis=0)
+
+        # quaternion average using eigenvector method
+        Q = np.vstack(quats)
+        A = Q.T @ Q
+        eigvals, eigvecs = np.linalg.eigh(A)
+        q_mean = eigvecs[:, np.argmax(eigvals)]
+        if q_mean[3] < 0:  # ensure w positive for consistency
+            q_mean = -q_mean
+        return t_mean, q_mean  # q_mean = [x,y,z,w]
+
+    def _timer_cb(self):
+        t = TransformStamped()
+        t.header.stamp = self.get_clock().now().to_msg()
+        t.header.frame_id = self.parent_frame
+        t.child_frame_id = self.child_frame
+
+        t.transform.translation.x = float(self.mean_t[0])
+        t.transform.translation.y = float(self.mean_t[1])
+        t.transform.translation.z = float(self.mean_t[2])
+        t.transform.rotation.x = float(self.mean_q[0])
+        t.transform.rotation.y = float(self.mean_q[1])
+        t.transform.rotation.z = float(self.mean_q[2])
+        t.transform.rotation.w = float(self.mean_q[3])
+
+        self.br.sendTransform(t)
+
+
+def main(argv=None):
+    rclpy.init(args=argv)
+
+    try:
+        node = AverageTFPublisher()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/sherpa_lidar_utils/src/servo_angle_tf.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from rclpy.duration import Duration
+from std_msgs.msg import Float32
+from geometry_msgs.msg import TransformStamped
+import tf2_ros
+import math
+
+def euler_to_quaternion(roll, pitch, yaw):
+    """Convert Euler angles to quaternion (x, y, z, w)."""
+    cr = math.cos(roll / 2)
+    sr = math.sin(roll / 2)
+    cp = math.cos(pitch / 2)
+    sp = math.sin(pitch / 2)
+    cy = math.cos(yaw / 2)
+    sy = math.sin(yaw / 2)
+
+    w = cr * cp * cy + sr * sp * sy
+    x = sr * cp * cy - cr * sp * sy
+    y = cr * sp * cy + sr * cp * sy
+    z = cr * cp * sy - sr * sp * cy
+    return x, y, z, w
+
+
+class ServoTfNode(Node):
+    """Publishes TF for a servo based on its current position."""
+
+    def __init__(self):
+        super().__init__("stepper_tf_node")
+
+        # Subscribe to current servo position
+        self.pos_sub = self.create_subscription(
+            Float32,
+            "/nucleo/stepper0/position",
+            self.pos_callback,
+            10
+        )
+
+        # TF broadcaster
+        self.br = tf2_ros.TransformBroadcaster(self)
+        self.get_logger().info("Servo TF node started, subscribing to /nucleo/stepper0/position")
+
+    def pos_callback(self, msg: Float32):
+        """Publish a TF corresponding to the servo position."""
+        angle_deg = msg.data  # get float from subscriber
+
+        t = TransformStamped()
+        #t.header.stamp = (self.get_clock().now() - rclpy.duration.Duration(seconds=0.1)).to_msg()
+        t.header.stamp = (self.get_clock().now() + Duration(seconds=0.05)).to_msg()
+        t.header.frame_id = "livox_base"
+        t.child_frame_id = "frame_default"
+
+        t.transform.translation.x = 0.0
+        t.transform.translation.y = 0.0
+        t.transform.translation.z = 0.0
+
+        # Convert angle to yaw (radians) and then to quaternion
+        yaw = angle_deg * math.pi / 180.0
+        qx, qy, qz, qw = euler_to_quaternion(0, 0, yaw)
+
+        t.transform.rotation.x = qx
+        t.transform.rotation.y = qy
+        t.transform.rotation.z = qz
+        t.transform.rotation.w = qw
+
+        # Publish the transform
+        self.br.sendTransform(t)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ServoTfNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
+

src/sherpa_lidar_utils/src/track_cluster.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from geometry_msgs.msg import PointStamped
+from std_msgs.msg import Float32
+from tf2_ros import Buffer, TransformListener
+import tf2_geometry_msgs
+import numpy as np
+import math
+import sensor_msgs_py.point_cloud2 as pc2
+
+class Cluster5Tracker(Node):
+    def __init__(self):
+        super().__init__('cluster_tracker')
+
+        self.source_frame = 'lidar_base'
+        self.target_frame = 'livox_base'
+
+        self.buf = []
+
+        # Subscribe to point cloud of cluster 5
+        self.sub = self.create_subscription(
+            PointCloud2,
+            '/tracking/target',
+            self.cb,
+            10
+        )
+
+        # Publishers
+        self.centroid_pub = self.create_publisher(PointStamped, '/tracking/target_pos', 10)
+        self.angle_pub = self.create_publisher(Float32, '/nucleo/stepper0/target', 10)
+
+        # TF
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+    def cb(self, msg: PointCloud2):
+        # Extract raw x, y, z as a regular list
+        points = [[p[0], p[1], p[2]] for p in pc2.read_points(msg, field_names=("x","y","z"), skip_nans=True)]
+        if len(points) == 0:
+            self.get_logger().warn("No points in cluster5 PointCloud2")
+            return
+
+        if len(points) == 1:
+            self.get_logger().warn("Only one point found. Defaulting to search target")
+            centroid = (3, 0, 0)
+        else:
+            arr = np.array(points, dtype=np.float32)
+            centroid = np.mean(arr, axis=0)
+
+        pt = PointStamped()
+        pt.header.frame_id = msg.header.frame_id
+        pt.header.stamp = msg.header.stamp
+        pt.point.x, pt.point.y, pt.point.z = centroid
+
+        try:
+            # Transform to livox_base
+            t = self.tf_buffer.lookup_transform(
+                self.target_frame,
+                pt.header.frame_id,
+                pt.header.stamp,
+                timeout=rclpy.duration.Duration(seconds=1.0)
+            )
+            pt_transformed = tf2_geometry_msgs.do_transform_point(pt, t)
+        except Exception as e:
+            self.get_logger().warn(f"TF lookup failed: {e}")
+            return
+
+        # Publish centroid
+        self.centroid_pub.publish(pt_transformed)
+
+        # Compute yaw angle in degrees
+        x, y = pt_transformed.point.x, pt_transformed.point.y
+        yaw_deg = math.degrees(math.atan2(y, x))
+
+        self.buf.append(yaw_deg)
+        if len(self.buf) > 5:
+            self.buf.pop(0)
+        target_angle: float = sum(self.buf) / len(self.buf)
+
+        angle_msg = Float32()
+        angle_msg.data = target_angle
+        self.angle_pub.publish(angle_msg)
+
+        #self.get_logger().info(f"Centroid: ({x:.2f}, {y:.2f}, {pt_transformed.point.z:.2f}), Yaw: {yaw_deg:.2f}°")
+
+def main():
+    rclpy.init()
+    node = Cluster5Tracker()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

src/target_tracking/launch/target_tracking_launch.py

@@ -5,8 +5,8 @@ import math
 ################### user configure parameters for ros2 start ###################
 # Topics/Frames
 frame_id        = 'velodyne'
-topic_preprocessing_in  = 'filtered_points'
+topic_preprocessing_in  = 'merged_cloud'
-topic_preprocessing_out = 'new_filtered'
+topic_preprocessing_out = 'filtered_points'
 
 # Preprocessing
 x_min           = 0.0
@@ -17,10 +17,10 @@ tan_h_fov       = math.pi / 4 # ±45°
 tan_v_fov       = math.pi / 6 # ±30°
 
 # Clustering
-z_dim_scale     = 0.1
+#z_dim_scale     = 0.1
-cluster_tolerance   = 0.3
+#cluster_tolerance   = 0.3
-min_cluster_size    = 10
+#min_cluster_size    = 10
-max_cluster_size    = 1000
+#max_cluster_size    = 1000
 min_width   = 0.0
 min_height  = 0.0
 min_length  = 0.0
@@ -28,6 +28,11 @@ max_width   = 1.5
 max_height  = 2.5
 max_length  = 1.5
 
+z_dim_scale     = 1.0
+cluster_tolerance   = 0.1
+min_cluster_size    = 350
+max_cluster_size    = 1500
+
 ################### user configure parameters for ros2 end ##################### 
 
 cloud_preprocessing_params = [
@@ -68,7 +73,7 @@ def generate_launch_description():
                 'stderr': 'screen',
             }
         ),
-         Node(
+        Node(
             package='target_tracking',
             executable='cloud_clustering_node',
             name='cloud_clustering',
@@ -78,4 +83,4 @@ def generate_launch_description():
                 'stderr': 'screen',
             }
         )
-    ])
+    ])

src/target_tracking/src/cloud_clustering.cpp

@@ -100,7 +100,7 @@ namespace cloud_clustering
     // Create a ROS subscriber for the input point cloud
     sub = this->create_subscription<sensor_msgs::msg::PointCloud2>(
         topic_in,
-        10, // queue size
+        rclcpp::SensorDataQoS(), // queue size
         std::bind(&CloudClustering::cloud_cb, this, std::placeholders::_1));
 
     std::cout << "Started clustering node with parameters:\n"
@@ -628,63 +628,40 @@ namespace cloud_clustering
       //          { return cluster_vec[a].second < cluster_vec[b].second; });
 
       std::vector<std::pair<int, float>> kf_intensity;
+kf_intensity.reserve(objID.size());
 
-      for (int i = 0; i < objID.size(); i++) {
+for (int kf_idx = 0; kf_idx < objID.size(); kf_idx++)
-          int cluster_index = objID[i];
+{
-          float mean_intensity = cluster_vec[cluster_index].second;
+    int cluster_idx = objID[kf_idx];
-          kf_intensity.push_back(std::make_pair(i, mean_intensity));
+    float intensity = cluster_vec[cluster_idx].second; // already normalized
-      }
+    kf_intensity.emplace_back(kf_idx, intensity);
+}
 
-      std::sort(kf_intensity.begin(), kf_intensity.end(),
+std::sort(kf_intensity.begin(), kf_intensity.end(),
-                [](const auto &a, const auto &b) {
+          [](const auto &a, const auto &b)
-                    return a.second > b.second;
+          {
-                });
+              return a.second > b.second;
+          });
 
-      for (auto &[kf_idx, intensity] : kf_intensity) {
+      int pub_idx = 0;
-        int cluster_index = objID[kf_idx];
-        auto &cluster = cluster_vec[cluster_index].first;
-        switch (i)
-        {
-        case 0:
-        {
-          publish_cloud(pub_cluster0, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
-        case 1:
-        {
-          publish_cloud(pub_cluster1, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
-        case 2:
-        {
-          publish_cloud(pub_cluster2, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
-        case 3:
-        {
-          publish_cloud(pub_cluster3, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
-        case 4:
-        {
-          publish_cloud(pub_cluster4, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
 
-        case 5:
+      for (const auto &[kf_idx, intensity] : kf_intensity)
-        {
+      {
-          publish_cloud(pub_cluster5, cluster_vec[cluster_index].first);
+          int cluster_idx = objID[kf_idx];
-          i++;
+          auto &cluster = cluster_vec[cluster_idx].first;
-          break;
+
-        }
+          switch (pub_idx)
-        default:
+          {
-          break;
+          case 0: publish_cloud(pub_cluster0, cluster); break;
-        }
+          case 1: publish_cloud(pub_cluster1, cluster); break;
+          case 2: publish_cloud(pub_cluster2, cluster); break;
+          case 3: publish_cloud(pub_cluster3, cluster); break;
+          case 4: publish_cloud(pub_cluster4, cluster); break;
+          case 5: publish_cloud(pub_cluster5, cluster); break;
+          default: break;
+          }
+
+          pub_idx++;
       }
     }
   }

src/target_tracking/src/cloud_preprocessing.cpp

@@ -153,7 +153,7 @@ private:
     // Convert and publish
     sensor_msgs::msg::PointCloud2 out_msg;
     pcl::toROSMsg(*voxel_filtered, out_msg);
-    out_msg.header.stamp = this->get_clock()->now();
+    out_msg.header.stamp = msg->header.stamp;
     out_msg.header.frame_id = msg->header.frame_id;
     filtered_publisher_->publish(out_msg);
 
@@ -183,4 +183,4 @@ int main(int argc, char * argv[])
   rclcpp::spin(std::make_shared<CloudFilterNode>());
   rclcpp::shutdown();
   return 0;
-}
+}

tracking.rviz

@@ -0,0 +1,424 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Merged1/Topic1
+      Splitter Ratio: 0.5
+    Tree Height: 789
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Cluster0
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        frame_default:
+          Value: true
+        lidar_base:
+          Value: true
+        livox_base:
+          Value: true
+        velodyne:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        velodyne:
+          lidar_base:
+            {}
+          livox_base:
+            frame_default:
+              {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 0; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 100
+      Min Color: 0; 0; 0
+      Min Intensity: 1
+      Name: Cluster0
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /tracking/cluster0
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 170; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Cluster1
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /tracking/cluster1
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Cluster2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /tracking/cluster2
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 0; 255; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Cluster3
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /tracking/cluster3
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 0; 170; 255
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Cluster4
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /tracking/cluster4
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Cluster5
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /tracking/cluster5
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Merged
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /lidar/merged/points
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Downsampled
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /lidar/merged/downsampled
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/PointStamped
+      Color: 255; 255; 255
+      Enabled: true
+      History Length: 1
+      Name: Target
+      Radius: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /tracking/target_pos
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 0; 0; 0
+    Fixed Frame: lidar_base
+    Frame Rate: 60
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 7.419111251831055
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 3.313748836517334
+        Y: -0.11149537563323975
+        Z: -0.589261531829834
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.635202944278717
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 3.1103579998016357
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1080
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000039e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 1920
+  Y: 0