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):
@@ -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('livox_pcd_saver', '/livox/lidar', buffer_livox, 1000),
+        PointCloudSaver('velodyne_pcd_saver', '/lidar/vlp16/points', buffer_velodyne, 3_500),
+        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/tracking.launch.py

@@ -64,71 +64,62 @@ def generate_launch_description():
         ],
     )
 
-    # ----- 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°  
-        ],
-    )
-
     # ----- pointcloud merging -----
     pointcloud_merger_node = Node(
         package='scans_merger',
         executable='scans_merger',
         parameters=[
             {"destination_frame": "lidar_base"},
-            {"input_cloud_1": "/lidar/vlp16/downsampled"},
-            {"input_cloud_2": "/lidar/mid40/downsampled"},
+            {"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/points"},
+            {"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": 125},
-            {"max_cluster_size": 2250},
-            {"cluster_tolerance": 0.15},
+            {"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=[
-            ('/cluster5', '/tracking/cluster0'),
-            ('/cluster4', '/tracking/cluster1'),
-            ('/cluster3', '/tracking/cluster2'),
-            ('/cluster2', '/tracking/cluster3'),
-            ('/cluster1', '/tracking/cluster4'),
-            ('/cluster0', '/tracking/cluster5'),
+            ('/cluster0', '/tracking/cluster0'),
+            ('/cluster1', '/tracking/cluster1'),
+            ('/cluster2', '/tracking/cluster2'),
+            ('/cluster3', '/tracking/cluster3'),
+            ('/cluster4', '/tracking/cluster4'),
+            ('/cluster5', '/tracking/cluster5'),
         ],
     )
 
@@ -150,9 +141,8 @@ def generate_launch_description():
     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)
     ld.add_action(pointcloud_merger_node)
+    ld.add_action(downsample_node)
     ld.add_action(cloud_clustering_node)
     ld.add_action(servo_tracking_node)
 

src/sherpa_lidar_utils/src/servo_angle_tf.py

@@ -1,6 +1,7 @@
 #!/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
@@ -46,7 +47,7 @@ class ServoTfNode(Node):
 
         t = TransformStamped()
         #t.header.stamp = (self.get_clock().now() - rclpy.duration.Duration(seconds=0.1)).to_msg()
-        t.header.stamp = self.get_clock().now().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"
 

src/sherpa_lidar_utils/src/track_cluster.py

@@ -42,6 +42,10 @@ class Cluster5Tracker(Node):
             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)
 
@@ -68,14 +72,15 @@ class Cluster5Tracker(Node):
 
         # Compute yaw angle in degrees
         x, y = pt_transformed.point.x, pt_transformed.point.y
-        yaw_deg = -math.degrees(math.atan2(y, x))
+        yaw_deg = math.degrees(math.atan2(y, x))
 
         self.buf.append(yaw_deg)
-        if len(self.buf) > 15:
+        if len(self.buf) > 5:
             self.buf.pop(0)
+        target_angle: float = sum(self.buf) / len(self.buf)
 
         angle_msg = Float32()
-        angle_msg.data = sum(self.buf) / len(self.buf)
+        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}°")

src/target_tracking/launch/target_tracking_launch.py

@@ -5,13 +5,8 @@ import math
 ################### user configure parameters for ros2 start ###################
 # Topics/Frames
 frame_id        = 'velodyne'
-<<<<<<< Updated upstream
-topic_preprocessing_in  = 'filtered_points'
-topic_preprocessing_out = 'new_filtered'
-=======
 topic_preprocessing_in  = 'merged_cloud'
 topic_preprocessing_out = 'filtered_points'
->>>>>>> Stashed changes
 
 # Preprocessing
 x_min           = 0.0
@@ -22,23 +17,21 @@ tan_h_fov       = math.pi / 4 # ±45°
 tan_v_fov       = math.pi / 6 # ±30°
 
 # Clustering
-<<<<<<< Updated upstream
-z_dim_scale     = 0.1
-cluster_tolerance   = 0.3
-min_cluster_size    = 10
-max_cluster_size    = 1000
+#z_dim_scale     = 0.1
+#cluster_tolerance   = 0.3
+#min_cluster_size    = 10
+#max_cluster_size    = 1000
 min_width   = 0.0
 min_height  = 0.0
 min_length  = 0.0
 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
->>>>>>> Stashed changes
 
 ################### user configure parameters for ros2 end ##################### 
 

src/target_tracking/src/cloud_clustering.cpp

@@ -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++) {
-          int cluster_index = objID[i];
-          float mean_intensity = cluster_vec[cluster_index].second;
-          kf_intensity.push_back(std::make_pair(i, mean_intensity));
+for (int kf_idx = 0; kf_idx < objID.size(); kf_idx++)
+{
+    int cluster_idx = objID[kf_idx];
+    float intensity = cluster_vec[cluster_idx].second; // already normalized
+    kf_intensity.emplace_back(kf_idx, intensity);
 }
 
 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;
           });
 
-      for (auto &[kf_idx, intensity] : kf_intensity) {
-        int cluster_index = objID[kf_idx];
-        auto &cluster = cluster_vec[cluster_index].first;
-        switch (i)
+      int pub_idx = 0;
+
+      for (const auto &[kf_idx, intensity] : kf_intensity)
       {
-        case 0:
+          int cluster_idx = objID[kf_idx];
+          auto &cluster = cluster_vec[cluster_idx].first;
+
+          switch (pub_idx)
           {
-          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 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;
           }
 
-        case 5:
-        {
-          publish_cloud(pub_cluster5, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
-        default:
-          break;
-        }
+          pub_idx++;
       }
     }
   }

src/target_tracking/src/cloud_preprocessing.cpp

@@ -158,7 +158,6 @@ private:
     filtered_publisher_->publish(out_msg);
 
     //RCLCPP_INFO(this->get_logger(), "Filtered %zu -> %zu points", input->points.size(), voxel_filtered->points.size());
-<<<<<<< Updated upstream
     //auto stop = std::chrono::high_resolution_clock::now();
     //auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
 
@@ -166,8 +165,6 @@ private:
     // member function on the duration object
     //std::cout << duration.count() << std::endl;
 
-=======
->>>>>>> Stashed changes
   }
 
   tf2_ros::Buffer tf_buffer_;

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