made tracking more stable, added size limits for cluster + changed cluster assignment to hungarian

.gitignore

@@ -3,3 +3,4 @@ build
 install
 log
 shapes
+src/sherpa/urdf/sherpa.urdf

.gitmodules

@@ -14,3 +14,7 @@
 	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

scripts/calibrate_lidar.py

@@ -0,0 +1,231 @@
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+from sensor_msgs.msg import PointCloud2
+from geometry_msgs.msg import TransformStamped
+import sensor_msgs_py.point_cloud2 as pc2
+from tf2_ros import StaticTransformBroadcaster
+import numpy as np
+import matplotlib.pyplot as plt
+import struct
+from io import BytesIO
+import threading
+import time
+import open3d as o3d
+from scipy.spatial.transform import Rotation as R
+
+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]):
+            self.buffer.write(struct.pack('ffffI', points[i,0], points[i,1], points[i,2], points[i,3], rgb_int[i]))
+
+
+class LidarTransformPublisher(Node):
+    def __init__(self, lidar1_buffer, lidar1_frame, lidar2_buffer, lidar2_frame):
+        super().__init__('static_transform_lidar_offsets')
+        self.br = StaticTransformBroadcaster(self)
+        self.lidar1_buffer = lidar1_buffer
+        self.lidar2_buffer = lidar2_buffer
+        self.lidar1_frame = lidar1_frame
+        self.lidar2_frame = lidar2_frame
+
+    def publish(self):
+        self.pcd_1 = self.pcd_buffer_to_o3d(self.lidar1_buffer)
+        self.pcd_2 = self.pcd_buffer_to_o3d(self.lidar2_buffer)
+
+        self.T = self.compute_transform()
+        self.get_logger().info(f"Computed initial transform:\n{self.T}")
+
+        T_copy = np.array(self.T, copy=True)
+        trans = T_copy[:3, 3]
+        rot_quat = R.from_matrix(T_copy[:3, :3]).as_quat()
+
+        t = TransformStamped()
+        t.header.stamp.sec = 0
+        t.header.stamp.nanosec = 0
+        t.header.frame_id = self.lidar1_frame
+        t.child_frame_id = self.lidar2_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]
+
+        self.br.sendTransform(t)
+        self.get_logger().info("Published static transform.")
+
+    def pcd_buffer_to_o3d(self, buffer: BytesIO):
+        buffer.seek(0)
+        header_lines = []
+        while True:
+            line = buffer.readline().decode('ascii').strip()
+            if not line:
+                continue
+            header_lines.append(line)
+            if line.startswith("DATA"):
+                data_line = line
+                break
+
+        if not data_line.lower().startswith("data binary"):
+            raise NotImplementedError("Only binary PCD supported")
+
+        num_points = 0
+        for line in header_lines:
+            if line.startswith("POINTS"):
+                num_points = int(line.split()[1])
+        if num_points == 0:
+            raise ValueError("PCD header missing point count")
+
+        dtype = np.dtype([
+            ('x', 'f4'), ('y', 'f4'), ('z', 'f4'),
+            ('intensity', 'f4'), ('rgb', 'u4')
+        ])
+
+        data = buffer.read(num_points * dtype.itemsize)
+        points_array = np.frombuffer(data, dtype=dtype, count=num_points)
+
+        pcd = o3d.geometry.PointCloud()
+        xyz = np.stack([points_array['x'], points_array['y'], points_array['z']], axis=-1)
+        pcd.points = o3d.utility.Vector3dVector(xyz)
+        return pcd
+
+    def compute_transform(self):
+        voxel_size = 0.2  # coarse-to-fine pyramid base
+
+        # --- Feature extraction ---
+        def preprocess(pcd, voxel):
+            pcd_down = pcd.voxel_down_sample(voxel)
+            pcd_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=voxel*2, max_nn=30))
+            fpfh = o3d.pipelines.registration.compute_fpfh_feature(
+                pcd_down, o3d.geometry.KDTreeSearchParamHybrid(radius=voxel*5, max_nn=100))
+            return pcd_down, fpfh
+
+        src_down, src_fpfh = preprocess(self.pcd_2, voxel_size)
+        tgt_down, tgt_fpfh = preprocess(self.pcd_1, voxel_size)
+
+        # --- Global alignment with RANSAC ---
+        result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
+            src_down, tgt_down, src_fpfh, tgt_fpfh, True,
+            1.5,  # distance threshold
+            o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
+            4,
+            [
+                o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
+                o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(1.5)
+            ],
+            o3d.pipelines.registration.RANSACConvergenceCriteria(4000000, 500)
+        )
+
+        trans_init = result_ransac.transformation
+
+        # --- Multi-scale ICP refinement ---
+        voxel_radii = [0.2, 0.1, 0.05]
+        max_iters = [50, 30, 14]
+        transformation = trans_init
+        for radius, iters in zip(voxel_radii, max_iters):
+            src_down = self.pcd_2.voxel_down_sample(radius)
+            tgt_down = self.pcd_1.voxel_down_sample(radius)
+            src_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius*2, max_nn=30))
+            tgt_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius*2, max_nn=30))
+
+            result_icp = o3d.pipelines.registration.registration_icp(
+                src_down, tgt_down, radius*2, transformation,
+                o3d.pipelines.registration.TransformationEstimationPointToPlane()
+            )
+            transformation = result_icp.transformation
+
+        return transformation
+
+
+def monitor_nodes(nodes, publisher):
+    while rclpy.ok():
+        if all(node.finished for node in nodes):
+            print("All pointclouds captured")
+            publisher.publish()
+            return
+        time.sleep(0.1)
+
+def main():
+    rclpy.init()
+    buffer_velodyne = BytesIO()
+    buffer_livox = BytesIO()
+
+    executor = MultiThreadedExecutor()
+    nodes = [
+        PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', buffer_velodyne, 350),
+        PointCloudSaver('livox_pcd_saver', '/livox/lidar', buffer_livox, 1000),
+    ]
+    publisher = LidarTransformPublisher(buffer_velodyne, 'velodyne', buffer_livox, 'frame_default')
+
+    monitor_thread = threading.Thread(target=monitor_nodes, args=(nodes,publisher), daemon=True)
+    monitor_thread.start()
+
+    for node in nodes:
+        executor.add_node(node)
+    try:
+        executor.spin()
+    finally:
+        monitor_thread.join()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

scripts/publish_lidar_offset.py

@@ -0,0 +1,87 @@
+#!/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

@@ -0,0 +1,113 @@
+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/CMakeLists.txt

@@ -31,3 +31,4 @@ install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/)
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/)
 install(DIRECTORY urdf   DESTINATION share/${PROJECT_NAME}/)
 install(DIRECTORY worlds DESTINATION share/${PROJECT_NAME}/)
+install(DIRECTORY materials DESTINATION share/${PROJECT_NAME}/)

src/sherpa/config/ackermann_controller.yaml

@@ -0,0 +1,34 @@
+
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+
+    ackermann_controller:
+      type: ackermann_steering_controller/AckermannSteeringController
+  
+    joint_state_broadcaster:
+        type: "joint_state_broadcaster/JointStateBroadcaster"
+
+
+ackermann_controller:  
+  ros__parameters:
+    position_feedback: false
+
+    # Interfaces
+    rear_wheels_names: ["wheel_br_joint", "wheel_bl_joint"]
+    front_wheels_names:  ["steer_fr_joint", "steer_fl_joint"]
+
+    # Physical parameters
+    wheelbase: 0.6                # Distance between front and rear axles [m]
+    front_wheel_track: 0.4        # Distance between front wheels [m]
+    rear_wheel_track: 0.5         # Distance between rear wheels [m]
+    front_wheels_radius: 0.15  # Radius of wheels [m]
+    rear_wheels_radius: 0.15  # Radius of wheels [m]
+
+    # Limits
+    max_steering_angle: 1.0  # [rad] ~45 degrees
+    max_speed: 10.0            # [m/s]
+    min_speed: -5.0            # [m/s]
+
+    # Frame config
+    reference_frame: base_link

src/sherpa/config/asd.rviz

@@ -0,0 +1,278 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      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: PointCloud2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: false
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: false
+      Visual Enabled: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /lane_detection/left
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz_default_plugins/Path
+      Color: 255; 0; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /lane_detection/right
+      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: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      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: /lane_detection/highlights
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        front_camera_link:
+          Value: true
+        imu_link:
+          Value: true
+        lidar_link:
+          Value: true
+        steer_fl_link:
+          Value: true
+        steer_fr_link:
+          Value: true
+        velodyne:
+          Value: true
+        wheel_bl_link:
+          Value: true
+        wheel_br_link:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        base_link:
+          front_camera_link:
+            {}
+          imu_link:
+            {}
+          lidar_link:
+            {}
+          steer_fl_link:
+            {}
+          steer_fr_link:
+            {}
+          wheel_bl_link:
+            {}
+          wheel_br_link:
+            {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 0; 0; 0
+    Fixed Frame: base_link
+    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.448129653930664
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.17294226586818695
+        Y: -0.44216388463974
+        Z: 0.46615225076675415
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.8847970962524414
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz_default_plugins)
+      Yaw: 3.110405445098877
+    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

src/sherpa/config/fastlio2_mid40.yaml

@@ -0,0 +1,48 @@
+/**:
+    ros__parameters:
+        feature_extract_enable: false
+        point_filter_num: 1
+        max_iteration: 25
+        filter_size_surf: 0.5
+        filter_size_map: 0.5
+        cube_side_length: 1000.0
+        runtime_pos_log_enable: false
+        map_file_path: "/home/timo/Downloads/scan.pcd"
+
+        common:
+            lid_topic:  "/livox/lidar"
+            imu_topic:  "/imu"
+            time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+            time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
+                                        # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
+
+        preprocess:
+            lidar_type: 0                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
+            scan_line: 1
+            timestamp_unit: 3            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+            blind: 0.35
+
+        mapping:
+            acc_cov: 0.01
+            gyr_cov: 0.025
+            b_acc_cov: 0.001
+            b_gyr_cov: 0.00025
+            fov_degree:    38.4
+            det_range:     260.0
+            extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
+            extrinsic_T: [ 0., 0., 0.05]
+            extrinsic_R: [ 1., 0., 0., 
+                        0., 1., 0., 
+                        0., 0., -1.]
+
+        publish:
+            path_en:  true
+            map_en: true
+            scan_publish_en:  true       # false: close all the point cloud output
+            dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
+            scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
+
+        pcd_save:
+            pcd_save_en: true
+            interval: -1                 # how many LiDAR frames saved in each pcd file; 
+                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

src/sherpa/config/fastlio2_vlp16.yaml

@@ -21,7 +21,7 @@
             scan_line: 16
             scan_rate: 10                # only need to be set for velodyne, unit: Hz,
             timestamp_unit: 3            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
-            blind: 1.05
+            blind: 0.7
 
         mapping:
             acc_cov: 0.01
@@ -30,7 +30,7 @@
             b_gyr_cov: 0.00025
             fov_degree:    360.0
             det_range:     100.0
-            extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic,
+            extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
             extrinsic_T: [ 0., 0., -0.05]
             extrinsic_R: [ 1., 0., 0., 
                         0., 1., 0., 

src/sherpa/config/gz_bridge.yaml

@@ -0,0 +1,53 @@
+- ros_topic_name: "/clock"
+  gz_topic_name: "clock"
+  ros_type_name: "rosgraph_msgs/msg/Clock"
+  gz_type_name: "gz.msgs.Clock"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "camera/camera_info"
+  gz_topic_name: "camera/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/tf"
+  gz_topic_name: "tf"
+  ros_type_name: "tf2_msgs/msg/TFMessage"
+  gz_type_name: "gz.msgs.Pose_V"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/joint_states"
+  gz_topic_name: "model/sherpa/joint_states"
+  ros_type_name: "sensor_msgs/msg/JointState"
+  gz_type_name: "gz.msgs.Model"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/velodyne_points"
+  gz_topic_name: "velodyne_points/points"
+  ros_type_name: "sensor_msgs/msg/PointCloud2"
+  gz_type_name: "gz.msgs.PointCloudPacked"
+  direction: GZ_TO_ROS  
+  
+- ros_topic_name: "/imu"
+  gz_topic_name: "imu"
+  ros_type_name: "sensor_msgs/msg/Imu"
+  gz_type_name: "gz.msgs.IMU"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/cmd_vel_in"
+  gz_topic_name: "cmd_vel"
+  ros_type_name: "geometry_msgs/msg/Twist"
+  gz_type_name: "gz.msgs.Twist"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/camera/front"
+  gz_topic_name: "front_camera"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/camera/camera_info"
+  gz_topic_name: "camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS

src/sherpa/config/mecanum_controller.yaml

@@ -0,0 +1,32 @@
+
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+    # use_sim_time: true
+
+    mecanum_controller:
+      type: "mecanum_drive_controller/MecanumDriveController"
+
+    joint_state_broadcaster:
+      type: "joint_state_broadcaster/JointStateBroadcaster"
+
+
+mecanum_controller:
+  ros__parameters:
+
+    front_left_wheel_command_joint_name: "wheel_fl_joint"
+    front_right_wheel_command_joint_name: "wheel_fr_joint"
+    rear_left_wheel_command_joint_name: "wheel_bl_joint"
+    rear_right_wheel_command_joint_name: "wheel_br_joint"
+
+    kinematics:
+      base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 }
+      wheels_radius: 0.4
+      sum_of_robot_center_projection_on_X_Y_axis: 1.0
+
+    base_frame_id: "base_link"
+    odom_frame_id: "map"
+    enable_odom_tf: false
+    twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+    pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+    use_sim_time: True

src/sherpa/launch/asd.launch.py

@@ -0,0 +1,123 @@
+import os.path
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+from launch.substitutions import Command
+
+def generate_launch_description():
+
+    ld = LaunchDescription()  
+
+    pkg_name = 'sherpa'
+    pkg_share = get_package_share_directory(pkg_name)
+    pkg_prefix = os.path.dirname(pkg_share) 
+
+    world_path = os.path.join(pkg_share, 'worlds', 'asd_course.world')
+    xacro_file = os.path.join(pkg_share, 'urdf', 'sherpa.xacro')
+
+    # create nodes
+
+    gz_env = SetEnvironmentVariable(
+        name='GZ_SIM_RESOURCE_PATH',
+        value=[pkg_prefix]
+    )
+
+    gz = ExecuteProcess(
+        cmd=[
+            'ros2', 'launch', 'ros_gz_sim', 'gz_sim.launch.py',
+            f'gz_args:=-r {world_path}'
+        ],
+        output='screen'
+    )
+
+    state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'robot_description': Command(['xacro ', xacro_file])
+        }]
+    )
+
+    controller = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['ackermann_controller'],
+        #output='screen'
+    )
+
+    joint_state_broadcaster = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+        parameters=[{'use_sim_time': True}],
+        #output='screen'
+    )
+
+    static_tf = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='static_tf_base_to_lidar',
+        arguments=[
+            '0', '0', '0',
+            '0', '0', '0',
+            'velodyne',
+            'lidar_link',
+        ]          
+    )
+
+    robot = ExecuteProcess(
+        cmd=[
+            'ros2', 'run', 'ros_gz_sim', 'create',
+            '-name', 'sherpa',
+            '-topic', 'robot_description',
+            '-x', '1.5', '-y', '9.5', '-z', '0.2'
+        ],
+        output='screen'
+    )
+
+    twist_stamper = Node(
+        package="twist_stamper",
+        executable="twist_stamper",
+        remappings=[
+            ('/cmd_vel_out', '/ackermann_controller/reference'),
+        ],
+        output='screen'
+    )
+
+    gz_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=[
+            "--ros-args",
+            "-p",
+            f"config_file:={os.path.join(pkg_share, 'config', 'gz_bridge.yaml')}",
+        ]
+    )
+
+
+    rviz2 = Node(
+        package='rviz2',
+        executable='rviz2',
+        arguments=['-d', f"{os.path.join(get_package_share_directory('sherpa'), 'config', 'asd.rviz')}"]
+    )
+
+    ld.add_action(gz_env)
+    ld.add_action(gz)
+    ld.add_action(state_publisher)
+    ld.add_action(static_tf)
+    ld.add_action(robot)
+    ld.add_action(gz_bridge)
+    ld.add_action(joint_state_broadcaster)
+    ld.add_action(controller)
+    ld.add_action(twist_stamper)
+    ld.add_action(rviz2)
+
+    return ld

src/sherpa/launch/odometry_mid40.launch.py

@@ -0,0 +1,53 @@
+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
+
+def generate_launch_description():
+
+    ld = LaunchDescription()   
+
+    livox_driver = Node(
+        package='livox_ros2_driver',
+        executable='livox_ros2_driver_node',
+        arguments=[]
+    )
+
+    imu = Node(
+        package = 'witmotion_ros',
+        executable = 'witmotion_ros_node',
+        parameters = [os.path.join(get_package_share_directory('sherpa'), 'config', 'wt931.yml')]
+    )
+
+    fast_lio = Node(
+        package='fast_lio',
+        executable='fastlio_mapping',
+        output='screen',
+        parameters=[PathJoinSubstitution([os.path.join(get_package_share_directory('sherpa'), 'config'), 'fastlio2_mid40.yaml'])]
+    )
+
+    tf_laserscan = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=['0.0', '0', '0.0', '0', '0', '0', '1', 'body', 'frame_default']
+    )
+
+    tf_map = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=['0.0', '0', '0.0', '0', '0', '0', '1', 'camera_init', 'map']
+    )
+
+
+    ld.add_action(livox_driver)
+    ld.add_action(imu)
+    ld.add_action(fast_lio)
+    ld.add_action(tf_laserscan)
+    ld.add_action(tf_map)
+
+    return ld

src/sherpa/launch/pointcloud-mid40.py

@@ -0,0 +1,23 @@
+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
+
+def generate_launch_description():
+
+    ld = LaunchDescription()   
+
+    livox_driver = Node(
+        package='livox_ros2_driver',
+        executable='livox_ros2_driver_node',
+        arguments=[]
+    )
+
+    ld.add_action(livox_driver)
+
+    return ld

src/sherpa/launch/pointcloud-vlp16.py

@@ -0,0 +1,30 @@
+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
+
+def generate_launch_description():
+
+    ld = LaunchDescription()   
+
+    velodyne_raw = Node(
+        package='velodyne_driver',
+        executable='velodyne_driver_node',
+        arguments=["--ros-args", "-p", "model:=VLP16", "-p", "rpm:=600.0", "-p", "device_ip:=10.42.30.200"]
+    )
+
+    velodyne_pointcloud = 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"]
+    )
+
+    ld.add_action(velodyne_raw)
+    ld.add_action(velodyne_pointcloud)
+
+    return ld

src/sherpa/launch/simulator.launch.py

@@ -18,7 +18,7 @@ def generate_launch_description():
     pkg_share = get_package_share_directory(pkg_name)
     pkg_prefix = os.path.dirname(pkg_share) 
 
-    world_path = os.path.join(pkg_share, 'worlds', 'empty.world')
+    world_path = os.path.join(pkg_share, 'worlds', 'raceway.world')
     xacro_file = os.path.join(pkg_share, 'urdf', 'sherpa.xacro')
 
     # create nodes
@@ -33,7 +33,7 @@ def generate_launch_description():
             'ros2', 'launch', 'ros_gz_sim', 'gz_sim.launch.py',
             f'gz_args:=-r {world_path}'
         ],
-        output='screen'
+        #output='screen'
     )
 
     state_publisher = Node(
@@ -46,11 +46,19 @@ def generate_launch_description():
         }]
     )
 
-    joint_pub = Node(
-        package='joint_state_publisher',
-        executable='joint_state_publisher',
-        name='joint_state_publisher',
-        output='screen'
+    controller = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['ackermann_controller'],
+        #output='screen'
+    )
+
+    joint_state_broadcaster = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+        parameters=[{'use_sim_time': True}],
+        #output='screen'
     )
 
     static_tf = Node(
@@ -60,8 +68,8 @@ def generate_launch_description():
         arguments=[
             '0', '0', '0',
             '0', '0', '0',
+            'velodyne',
             'lidar_link',
-            'camera_init',
         ]          
     )
 
@@ -70,23 +78,28 @@ def generate_launch_description():
             'ros2', 'run', 'ros_gz_sim', 'create',
             '-name', 'sherpa',
             '-topic', 'robot_description',
-            '-x', '0', '-y', '0', '-z', '0.5'
+            '-x', '72', '-y', '-72', '-z', '8.5'
+        ],
+        output='screen'
+    )
+
+    twist_stamper = Node(
+        package="twist_stamper",
+        executable="twist_stamper",
+        remappings=[
+            ('/cmd_vel_out', '/ackermann_controller/reference'),
         ],
         output='screen'
     )
 
     gz_bridge = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
         arguments=[
-            '/velodyne_points/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked',
-            '/model/sherpa/joint_states@gz.msgs.Model_JointState@ros2/sensor_msgs/msg/JointState',
-        ],
-        remappings=[
-            ('/velodyne_points/points', '/velodyne_points'),
-            ('/model/sherpa/joint_states', '/joint_states'),
-        ],
-        output='screen'
+            "--ros-args",
+            "-p",
+            f"config_file:={os.path.join(pkg_share, 'config', 'gz_bridge.yaml')}",
+        ]
     )
 
 
@@ -99,10 +112,12 @@ def generate_launch_description():
     ld.add_action(gz_env)
     ld.add_action(gz)
     ld.add_action(state_publisher)
-    ld.add_action(joint_pub)
     ld.add_action(static_tf)
     ld.add_action(robot)
     ld.add_action(gz_bridge)
+    ld.add_action(joint_state_broadcaster)
+    ld.add_action(controller)
+    ld.add_action(twist_stamper)
     ld.add_action(rviz2)
 
     return ld

src/sherpa/package.xml

@@ -12,6 +12,25 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
+  <depend>xacro</depend>
+
+  <!-- Dependencies for the robot control system -->
+  <depend>rclcpp</depend>
+  <depend>ros2_control</depend>
+  <depend>controller_manager</depend>
+  <depend>hardware_interface</depend>
+
+  <!-- Dependencies for gazebo -->
+  <depend>gazebo_ros2_control</depend>
+  <depend>gazebo_msgs</depend>
+  <depend>ros_gz_bridge</depend>
+
+  <!-- Dependencies for slam -->
+  <depend>velodyne_driver</depend>
+  <depend>velodyne_pointcloud</depend>
+  <depend>witmotion_ros</depend>
+  <depend>fast_lio</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

src/sherpa/urdf/components/camera.xacro

@@ -0,0 +1,49 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="camera" params="name parent xyz rpy frequency fov topic">
+
+        <link name="${name}_link">
+            <inertial>
+                <mass value="0.01"/>
+                <inertia 
+                    ixx="0.001"
+                    ixy="0.0"
+                    ixz="0.0"
+                    iyy="0.001"
+                    iyz="0.0"
+                    izz="0.001"
+                />
+            </inertial>
+        </link>
+        <joint name="${name}_joint" type="fixed">
+            <parent link="${parent}_link"/>
+            <child link="${name}_link"/>
+            <origin xyz="${xyz}" rpy="${rpy}"/>
+        </joint>
+        <gazebo reference="${name}_link">
+            <sensor name="${name}" type="camera">
+                <update_rate>${frequency}</update_rate>
+                <topic>${topic}</topic>
+                <gz_frame_id>${name}_link</gz_frame_id>
+                <camera name="${name}">
+                    <horizontal_fov>${(fov/360) * (2*3.1415)}</horizontal_fov>
+                    <image>
+                        <width>800</width>
+                        <height>800</height>
+                        <format>L8</format>
+                    </image>
+                    <clip>
+                        <near>0.02</near>
+                        <far>300</far>
+                    </clip>
+                    <noise>
+                        <type>gaussian</type>
+                        <mean>0</mean>
+                        <stddev>0.007</stddev>
+                    </noise>
+                </camera>
+            </sensor>
+        </gazebo>
+    </xacro:macro>
+</robot>

src/sherpa/urdf/components/imu.xacro

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="imu" params="name parent xyz rpy frequency topic">
+
+        <link name="${name}_link">
+            <inertial>
+                <mass value="0.01"/>
+                <inertia 
+                    ixx="0.001828"
+                    ixy="0.0"
+                    ixz="0.0"
+                    iyy="0.001828"
+                    iyz="0.0"
+                    izz="0.003528"
+                />
+            </inertial>
+        </link>
+        <joint name="${name}_joint" type="fixed">
+            <parent link="${parent}_link"/>
+            <child link="${name}_link"/>
+            <origin xyz="${xyz}" rpy="${rpy}"/>
+        </joint>
+        <gazebo reference="${name}_link">
+            <sensor name="${name}" type="imu">
+                <always_on>1</always_on>
+                <update_rate>${frequency}</update_rate>
+                <gz_frame_id>${name}_link</gz_frame_id>
+                <visualize>true</visualize>
+                <topic>${topic}</topic>
+            </sensor>
+        </gazebo>
+    </xacro:macro>
+</robot>

src/sherpa/urdf/components/mecanum_wheel.xacro

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:gz="http://gazebosim.org">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
     <material name="mecanum_wheel_material">
         <ambient>0.2 0.2 0.2 1</ambient>
@@ -13,11 +13,11 @@
 
         <!-- get left or right wheel -->
         <xacro:if value="${direction == 'l'}">
-            <xacro:property name="mesh" value="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL"/>
+            <xacro:property name="mesh" value="package://sherpa/urdf/meshes//mecanum_wheel_left.STL"/>
             <xacro:property name="friction_dir" value="-1"/>
         </xacro:if>
         <xacro:if value="${direction == 'r'}">
-            <xacro:property name="mesh" value="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL"/>
+            <xacro:property name="mesh" value="package://sherpa/urdf/meshes//mecanum_wheel_right.STL"/>
             <xacro:property name="friction_dir" value="1"/>
         </xacro:if>
 
@@ -27,16 +27,15 @@
             <inertial>
                 <mass value="${mass}"/>
                 <inertia 
-                    ixx="${0.5 * mass * radius * radius}"
+                    ixx="${(13.0/48.0) * mass * radius * radius}"
                     ixy="0.0"
                     ixz="0.0"
                     iyy="${0.5 * mass * radius * radius}"
                     iyz="0.0"
-                    izz="${0.25 * mass * radius * radius}"
+                    izz="${(13.0/48.0) * mass * radius * radius}"
                 />
             </inertial>
             <visual name='visual'>
-                <pose>0 0 0 0 0 0</pose>
                 <geometry>
                     <mesh
                         filename="${mesh}"
@@ -46,27 +45,30 @@
                 <material name="mecanum_wheel_material"/>
             </visual>
             <collision name='collision'>
-            <geometry>
-                <sphere radius="${radius}"/>
-            </geometry>
+                <origin xyz="0 0 0" rpy="-1.5707 0 0"/>
+                <geometry>
+                    <cylinder radius="${radius}" length="${radius}"/>
+                </geometry>
+            </collision>
+        </link>
+        <xacro:element xacro:name="gazebo" reference="${name}_link">
             <surface>
                 <friction>
                 <ode>
-                    <mu>1.2</mu>
-                    <mu2>0</mu2>
-                    <fdir1 gz:expressed_in="${parent}">1 ${friction_dir} 0</fdir1>
+                    <mu>1.0</mu>
+                    <mu2>0.0</mu2>
+                    <fdir1>1 ${friction_dir} 0</fdir1>
                 </ode>
                 </friction>
             </surface>
-            </collision>
-        </link>
+        </xacro:element>
         <joint name='${name}_joint' type='continuous'>
             <parent link="${parent}_link"/>
             <child link="${name}_link"/>
             <axis xyz="0 1 0"/>
             <limit 
-                velocity="5"
-                effort="100000"
+                velocity="35"
+                effort="350"
             />
             <origin xyz="${xyz}" rpy="${rpy}"/>
         </joint>

src/sherpa/urdf/components/ouster_lidar.xacro

@@ -140,13 +140,8 @@
         </joint>
         <gazebo reference="${name}_link">
             <sensor name="${name}" type="gpu_lidar">
-                <plugin
-                    filename="gz-sim-sensors-system"
-                    name="gz::sim::systems::Sensors">
-                    <render_engine>ogre2</render_engine>
-                </plugin>
                 <update_rate>${frequency}</update_rate>
-                <topic>/velodyne_points</topic>
+                <topic>${topic}</topic>
                 <gz_frame_id>${name}_link</gz_frame_id>
                 <lidar>
                     <scan>
@@ -172,4 +167,4 @@
             </sensor>
         </gazebo>
     </xacro:macro>
-</robot>
+</robot>

src/sherpa/urdf/components/wheel.xacro

@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <material name="wheel_rim_material">
+        <ambient>0.05 0.05 0.2 05</ambient>
+        <diffuse>0.9 0.9 0.9 1</diffuse>
+        <specular>0.1 0.1 0.1 1</specular>
+        <emissive>0 0 0 0</emissive>
+        <color rgba="0.025 0.025 0.025 1"/> 
+    </material> 
+
+    <material name="wheel_spoke_material">
+        <ambient>0.2 0.2 0.2 1</ambient>
+        <diffuse>0.25 0.25 0.25 1</diffuse>
+        <specular>0.2 0.2 0.2 1</specular>
+        <emissive>0 0 0 0 1</emissive>
+        <color rgba="0.8 0.0 0.0 1"/> 
+    </material>
+
+    <xacro:macro name="wheel" params="name parent xyz rpy radius mass">
+
+        <xacro:property name="mesh_scale" value="${0.0078 * radius}"/>
+
+        <link name='${name}_link'>
+            <inertial>
+                <mass value="${mass}"/>
+                <inertia 
+                    ixx="${(13.0/48.0) * mass * radius * radius}"
+                    ixy="0.0"
+                    ixz="0.0"
+                    iyy="${0.5 * mass * radius * radius}"
+                    iyz="0.0"
+                    izz="${(13.0/48.0) * mass * radius * radius}"
+                />
+            </inertial>
+            <visual name='visual'>
+                <geometry>
+                    <mesh
+                        filename="package://sherpa/urdf/meshes/wheel_cape.stl"
+                        scale="${mesh_scale} ${mesh_scale} ${mesh_scale}"
+                    />
+                </geometry>
+                <material name="wheel_rim_material"/>
+            </visual>
+            <visual name='visual'>
+                <geometry>
+                    <mesh
+                        filename="package://sherpa/urdf/meshes/wheel_spoke.stl"
+                        scale="${mesh_scale} ${mesh_scale} ${mesh_scale}"
+                    />
+                </geometry>
+                <material name="wheel_spoke_material"/>
+            </visual>
+            <collision name='collision'>
+                <origin xyz="0 0 0" rpy="-1.5707 0 0"/>
+                <geometry>
+                    <cylinder radius="${radius}" length="${radius/1.5}"/>
+                </geometry>
+            </collision>
+        </link>
+        <xacro:element xacro:name="gazebo" reference="${name}_link">
+            <surface>
+                <friction>
+                    <ode>
+                        <mu>1.5</mu>
+                        <mu2>1.2</mu2>
+                        <kp>1000000</kp>
+	                    <kd>100</kd>
+                    </ode>
+                </friction>
+                <soft_cfm>0.1</soft_cfm>  <!-- Soft contact to prevent rigid collisions -->
+                <soft_erp>0.1</soft_erp>   <!-- Error reduction parameter (makes contact softer) -->
+            </surface>
+        </xacro:element>
+        <joint name='${name}_joint' type='continuous'>
+            <parent link="${parent}_link"/>
+            <child link="${name}_link"/>
+            <axis xyz="0 1 0"/>
+            <limit 
+                velocity="5"
+                effort="30"
+            />
+            <origin xyz="${xyz}" rpy="${rpy}"/>
+        </joint>
+    </xacro:macro>
+</robot>

src/sherpa/urdf/mecanum.sdf

@@ -1,367 +0,0 @@
-<?xml version="1.0" ?>
-<!--
-  Gazebo Mecanum drive plugin demo
-
-  Try sending commands:
-
-    gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "linear: {x: 0.5, y: 0.5}"
-
-    gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "linear: {y: 0.5}, angular: {z: 0.2}"
-
--->
-<sdf version="1.6">
-  <world name="mecanum_drive">
-
-    <physics name="1ms" type="ignored">
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1.0</real_time_factor>
-    </physics>
-    <plugin
-      filename="gz-sim-physics-system"
-      name="gz::sim::systems::Physics">
-    </plugin>
-    <plugin
-      filename="gz-sim-user-commands-system"
-      name="gz::sim::systems::UserCommands">
-    </plugin>
-    <plugin
-      filename="gz-sim-scene-broadcaster-system"
-      name="gz::sim::systems::SceneBroadcaster">
-    </plugin>
-
-    <light type="directional" name="sun">
-      <cast_shadows>true</cast_shadows>
-      <pose>0 0 10 0 0 0</pose>
-      <diffuse>1 1 1 1</diffuse>
-      <specular>0.5 0.5 0.5 1</specular>
-      <attenuation>
-        <range>1000</range>
-        <constant>0.9</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <direction>-0.5 0.1 -0.9</direction>
-    </light>
-
-    <model name="ground_plane">
-      <static>true</static>
-      <link name="link">
-        <collision name="collision">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>50</mu>
-              </ode>
-            </friction>
-          </surface>
-        </collision>
-        <visual name="visual">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-          <material>
-            <ambient>0.8 0.8 0.8 1</ambient>
-            <diffuse>0.8 0.8 0.8 1</diffuse>
-            <specular>0.8 0.8 0.8 1</specular>
-          </material>
-        </visual>
-      </link>
-    </model>
-
-    <model name='vehicle_blue' xmlns:gz="http://gazebosim.org/schema">
-      <pose>0 2 0.325 0 -0 0</pose>
-
-      <link name='chassis'>
-        <pose>-0.151427 -0 0.175 0 -0 0</pose>
-        <inertial>
-          <mass>1.14395</mass>
-          <inertia>
-            <ixx>0.126164</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.416519</iyy>
-            <iyz>0</iyz>
-            <izz>0.481014</izz>
-          </inertia>
-        </inertial>
-        <visual name='visual'>
-          <geometry>
-            <box>
-              <size>2.01142 1 0.568726</size>
-            </box>
-          </geometry>
-          <material>
-            <ambient>0.5 0.5 1.0 1</ambient>
-            <diffuse>0.5 0.5 1.0 1</diffuse>
-            <specular>0.0 0.0 1.0 1</specular>
-          </material>
-        </visual>
-        <collision name='collision'>
-          <geometry>
-            <box>
-              <size>2.01142 1 0.568726</size>
-            </box>
-          </geometry>
-        </collision>
-      </link>
-
-      <link name='front_left_wheel'>
-        <pose>0.554283 0.625029 -0.025 -1.5707 0 0</pose>
-        <inertial>
-          <mass>2</mass>
-          <inertia>
-            <ixx>0.145833</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.145833</iyy>
-            <iyz>0</iyz>
-            <izz>0.125</izz>
-          </inertia>
-        </inertial>
-        <visual name='visual'>
-          <pose>0 0 0 1.5707 0 0</pose>
-          <geometry>
-            <!-- scale mesh to radius == 0.3 -->
-            <mesh>
-              <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL</uri>
-              <scale>0.006 0.006 0.006</scale>
-            </mesh>
-          </geometry>
-          <material>
-            <ambient>0.2 0.2 0.2 1</ambient>
-            <diffuse>0.2 0.2 0.2 1</diffuse>
-            <specular>0.2 0.2 0.2 1</specular>
-          </material>
-        </visual>
-        <collision name='collision'>
-          <geometry>
-            <sphere>
-              <radius>0.3</radius>
-            </sphere>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1.0</mu>
-                <mu2>0.0</mu2>
-                <fdir1 gz:expressed_in="chassis">1 -1 0</fdir1>
-              </ode>
-            </friction>
-          </surface>
-        </collision>
-      </link>
-
-      <link name='rear_left_wheel'>
-        <pose>-0.957138 0.625029 -0.025 -1.5707 0 0</pose>
-        <inertial>
-          <mass>2</mass>
-          <inertia>
-            <ixx>0.145833</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.145833</iyy>
-            <iyz>0</iyz>
-            <izz>0.125</izz>
-          </inertia>
-        </inertial>
-        <visual name='visual'>
-          <pose>0 0 0 1.5707 0 0</pose>
-          <geometry>
-            <!-- scale mesh to radius == 0.3 -->
-            <mesh>
-              <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL</uri>
-              <scale>0.006 0.006 0.006</scale>
-            </mesh>
-          </geometry>
-          <material>
-            <ambient>0.2 0.2 0.2 1</ambient>
-            <diffuse>0.2 0.2 0.2 1</diffuse>
-            <specular>0.2 0.2 0.2 1</specular>
-          </material>
-        </visual>
-        <collision name='collision'>
-          <geometry>
-            <sphere>
-              <radius>0.3</radius>
-            </sphere>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1.0</mu>
-                <mu2>0.0</mu2>
-                <fdir1 gz:expressed_in="chassis">1 1 0</fdir1>
-              </ode>
-            </friction>
-          </surface>
-        </collision>
-      </link>
-
-      <link name='front_right_wheel'>
-        <pose>0.554282 -0.625029 -0.025 -1.5707 0 0</pose>
-        <inertial>
-          <mass>2</mass>
-          <inertia>
-            <ixx>0.145833</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.145833</iyy>
-            <iyz>0</iyz>
-            <izz>0.125</izz>
-          </inertia>
-        </inertial>
-        <visual name='visual'>
-          <pose>0 0 0 1.5707 0 0</pose>
-          <geometry>
-            <!-- scale mesh to radius == 0.3 -->
-            <mesh>
-              <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL</uri>
-              <scale>0.006 0.006 0.006</scale>
-            </mesh>
-          </geometry>
-          <material>
-            <ambient>0.2 0.2 0.2 1</ambient>
-            <diffuse>0.2 0.2 0.2 1</diffuse>
-            <specular>0.2 0.2 0.2 1</specular>
-          </material>
-        </visual>
-        <collision name='collision'>
-          <geometry>
-            <sphere>
-              <radius>0.3</radius>
-            </sphere>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1.0</mu>
-                <mu2>0.0</mu2>
-                <fdir1 gz:expressed_in="chassis">1 1 0</fdir1>
-              </ode>
-            </friction>
-          </surface>
-        </collision>
-      </link>
-
-      <link name='rear_right_wheel'>
-        <pose>-0.957138 -0.625029 -0.025 -1.5707 0 0</pose>
-        <inertial>
-          <mass>2</mass>
-          <inertia>
-            <ixx>0.145833</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.145833</iyy>
-            <iyz>0</iyz>
-            <izz>0.125</izz>
-          </inertia>
-        </inertial>
-        <visual name='visual'>
-          <pose>0 0 0 1.5707 0 0</pose>
-          <geometry>
-            <!-- scale mesh to radius == 0.3 -->
-            <mesh>
-              <uri>https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL</uri>
-              <scale>0.006 0.006 0.006</scale>
-            </mesh>
-          </geometry>
-          <material>
-            <ambient>0.2 0.2 0.2 1</ambient>
-            <diffuse>0.2 0.2 0.2 1</diffuse>
-            <specular>0.2 0.2 0.2 1</specular>
-          </material>
-        </visual>
-        <collision name='collision'>
-          <geometry>
-            <sphere>
-              <radius>0.3</radius>
-            </sphere>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1.0</mu>
-                <mu2>0.0</mu2>
-                <fdir1 gz:expressed_in="chassis">1 -1 0</fdir1>
-              </ode>
-            </friction>
-          </surface>
-        </collision>
-      </link>
-
-
-      <joint name='front_left_wheel_joint' type='revolute'>
-        <parent>chassis</parent>
-        <child>front_left_wheel</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-1.79769e+308</lower>
-            <upper>1.79769e+308</upper>
-          </limit>
-        </axis>
-      </joint>
-
-      <joint name='front_right_wheel_joint' type='revolute'>
-        <parent>chassis</parent>
-        <child>front_right_wheel</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-1.79769e+308</lower>
-            <upper>1.79769e+308</upper>
-          </limit>
-        </axis>
-      </joint>
-
-      <joint name='rear_left_wheel_joint' type='revolute'>
-        <parent>chassis</parent>
-        <child>rear_left_wheel</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-1.79769e+308</lower>
-            <upper>1.79769e+308</upper>
-          </limit>
-        </axis>
-      </joint>
-
-      <joint name='rear_right_wheel_joint' type='revolute'>
-        <parent>chassis</parent>
-        <child>rear_right_wheel</child>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <limit>
-            <lower>-1.79769e+308</lower>
-            <upper>1.79769e+308</upper>
-          </limit>
-        </axis>
-      </joint>
-
-      <plugin
-        filename="gz-sim-mecanum-drive-system"
-        name="gz::sim::systems::MecanumDrive">
-        <front_left_joint>front_left_wheel_joint</front_left_joint>
-        <front_right_joint>front_right_wheel_joint</front_right_joint>
-        <back_left_joint>rear_left_wheel_joint</back_left_joint>
-        <back_right_joint>rear_right_wheel_joint</back_right_joint>
-        <wheel_separation>1.25</wheel_separation>
-        <wheelbase>1.511</wheelbase>
-        <wheel_radius>0.3</wheel_radius>
-        <min_acceleration>-5</min_acceleration>
-        <max_acceleration>5</max_acceleration>
-      </plugin>
-
-    </model>
-
-  </world>
-</sdf>

src/sherpa/urdf/sherpa.urdf

@@ -1,262 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from /ros2_ws/src/sherpa/urdf/sherpa.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="sherpa" xmlns:gz="http://gazebosim.org">
-  <link name="base_link">
-    <inertial>
-      <mass value="15"/>
-      <inertia ixx="0.001828" ixy="0.0" ixz="0.0" iyy="0.001828" iyz="0.0" izz="0.003528"/>
-    </inertial>
-  </link>
-  <material name="mecanum_wheel_material">
-    <ambient>0.2 0.2 0.2 1</ambient>
-    <diffuse>0.2 0.2 0.2 1</diffuse>
-    <specular>0.2 0.2 0.2 1</specular>
-    <emissive>0 0 0 0</emissive>
-    <color rgba="0.2 0.2 0.2 1"/>
-  </material>
-  <link name="lidar_link">
-    <visual name="base">
-      <pose>0 0 -0.035 0 0 0</pose>
-      <geometry>
-        <cylinder length="0.0015" radius="0.06"/>
-      </geometry>
-    </visual>
-    <collision name="base_collision">
-      <pose>0 0 -0.035 0 0 0</pose>
-      <geometry>
-        <cylinder length="0.01" radius="0.06"/>
-      </geometry>
-    </collision>
-    <visual name="suport_1">
-      <pose>-0.035 0.035 -0.08 0 0 0</pose>
-      <geometry>
-        <cylinder length="0.09" radius="0.005"/>
-      </geometry>
-    </visual>
-    <visual name="suport_2">
-      <pose>-0.035 -0.035 -0.08 0 0 0</pose>
-      <geometry>
-        <cylinder length="0.09" radius="0.005"/>
-      </geometry>
-    </visual>
-    <visual name="suport_3">
-      <pose>0.0495 0 -0.08 0 0 0</pose>
-      <geometry>
-        <cylinder length="0.09" radius="0.005"/>
-      </geometry>
-    </visual>
-    <!-- Bottom cylinder of Ouster/Velodyne 3D Lidar -->
-    <visual name="bot_cylinder">
-      <pose>0 0 -0.035 0 0 0</pose>
-      <geometry>
-        <mesh filename="package://sherpa/urdf/meshes/VLP16_base_1.dae" scale="1 1 1"/>
-      </geometry>
-    </visual>
-    <!-- Top cylinder of Ouster/Velodyne 3D Lidar -->
-    <visual name="top_cylinder">
-      <pose>0 0 -0.035 0 0 0</pose>
-      <geometry>
-        <mesh filename="package://sherpa/urdf/meshes/VLP16_base_2.dae" scale="1 1 1"/>
-      </geometry>
-    </visual>
-    <!-- Main cylinder of Ouster/Velodyne 3D Lidar -->
-    <visual name="main_cylinder">
-      <pose>0 0 -0.035 0 0 0</pose>
-      <geometry>
-        <mesh filename="package://sherpa/urdf/meshes/VLP16_scan.dae" scale="1 1 1"/>
-      </geometry>
-    </visual>
-    <inertial>
-      <mass value="0.83"/>
-      <inertia ixx="0.001828" ixy="0.0" ixz="0.0" iyy="0.001828" iyz="0.0" izz="0.003528"/>
-    </inertial>
-  </link>
-  <joint name="lidar_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="lidar_link"/>
-    <origin rpy="0 0 0" xyz="0 0 0.15"/>
-  </joint>
-  <gazebo reference="lidar_link">
-    <sensor name="lidar" type="gpu_lidar">
-      <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
-        <render_engine>ogre2</render_engine>
-      </plugin>
-      <update_rate>10</update_rate>
-      <topic>/velodyne_points</topic>
-      <gz_frame_id>lidar_link</gz_frame_id>
-      <lidar>
-        <scan>
-          <horizontal>
-            <samples>1800.0</samples>
-            <resolution>1</resolution>
-            <min_angle>-3.141592654</min_angle>
-            <max_angle>3.141592654</max_angle>
-          </horizontal>
-          <vertical>
-            <samples>16</samples>
-            <resolution>1</resolution>
-            <min_angle>-0.261799389</min_angle>
-            <max_angle>0.261799389</max_angle>
-          </vertical>
-        </scan>
-        <range>
-          <min>0.01</min>
-          <max>100.0</max>
-          <resolution>0.03</resolution>
-        </range>
-      </lidar>
-    </sensor>
-  </gazebo>
-  <link name="wheel_fr_link">
-    <inertial>
-      <mass value="0.25"/>
-      <inertia ixx="0.0002" ixy="0.0" ixz="0.0" iyy="0.0002" iyz="0.0" izz="0.0001"/>
-    </inertial>
-    <visual name="visual">
-      <pose>0 0 0 0 0 0</pose>
-      <geometry>
-        <mesh filename="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL" scale="0.0008 0.0008 0.0008"/>
-      </geometry>
-      <material name="mecanum_wheel_material"/>
-    </visual>
-    <collision name="collision">
-      <geometry>
-        <sphere radius="0.04"/>
-      </geometry>
-      <surface>
-        <friction>
-          <ode>
-            <mu>1.2</mu>
-            <mu2>0</mu2>
-            <fdir1 gz:expressed_in="base">1 1 0</fdir1>
-          </ode>
-        </friction>
-      </surface>
-    </collision>
-  </link>
-  <joint name="wheel_fr_joint" type="continuous">
-    <parent link="base_link"/>
-    <child link="wheel_fr_link"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="100000" velocity="5"/>
-    <origin rpy="0 0 0" xyz=" 0.11 -0.11 0"/>
-  </joint>
-  <link name="wheel_fl_link">
-    <inertial>
-      <mass value="0.25"/>
-      <inertia ixx="0.0002" ixy="0.0" ixz="0.0" iyy="0.0002" iyz="0.0" izz="0.0001"/>
-    </inertial>
-    <visual name="visual">
-      <pose>0 0 0 0 0 0</pose>
-      <geometry>
-        <mesh filename="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL" scale="0.0008 0.0008 0.0008"/>
-      </geometry>
-      <material name="mecanum_wheel_material"/>
-    </visual>
-    <collision name="collision">
-      <geometry>
-        <sphere radius="0.04"/>
-      </geometry>
-      <surface>
-        <friction>
-          <ode>
-            <mu>1.2</mu>
-            <mu2>0</mu2>
-            <fdir1 gz:expressed_in="base">1 -1 0</fdir1>
-          </ode>
-        </friction>
-      </surface>
-    </collision>
-  </link>
-  <joint name="wheel_fl_joint" type="continuous">
-    <parent link="base_link"/>
-    <child link="wheel_fl_link"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="100000" velocity="5"/>
-    <origin rpy="0 0 0" xyz=" 0.11  0.11 0"/>
-  </joint>
-  <link name="wheel_br_link">
-    <inertial>
-      <mass value="0.25"/>
-      <inertia ixx="0.0002" ixy="0.0" ixz="0.0" iyy="0.0002" iyz="0.0" izz="0.0001"/>
-    </inertial>
-    <visual name="visual">
-      <pose>0 0 0 0 0 0</pose>
-      <geometry>
-        <mesh filename="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL" scale="0.0008 0.0008 0.0008"/>
-      </geometry>
-      <material name="mecanum_wheel_material"/>
-    </visual>
-    <collision name="collision">
-      <geometry>
-        <sphere radius="0.04"/>
-      </geometry>
-      <surface>
-        <friction>
-          <ode>
-            <mu>1.2</mu>
-            <mu2>0</mu2>
-            <fdir1 gz:expressed_in="base">1 -1 0</fdir1>
-          </ode>
-        </friction>
-      </surface>
-    </collision>
-  </link>
-  <joint name="wheel_br_joint" type="continuous">
-    <parent link="base_link"/>
-    <child link="wheel_br_link"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="100000" velocity="5"/>
-    <origin rpy="0 0 0" xyz="-0.11 -0.11 0"/>
-  </joint>
-  <link name="wheel_bl_link">
-    <inertial>
-      <mass value="0.25"/>
-      <inertia ixx="0.0002" ixy="0.0" ixz="0.0" iyy="0.0002" iyz="0.0" izz="0.0001"/>
-    </inertial>
-    <visual name="visual">
-      <pose>0 0 0 0 0 0</pose>
-      <geometry>
-        <mesh filename="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL" scale="0.0008 0.0008 0.0008"/>
-      </geometry>
-      <material name="mecanum_wheel_material"/>
-    </visual>
-    <collision name="collision">
-      <geometry>
-        <sphere radius="0.04"/>
-      </geometry>
-      <surface>
-        <friction>
-          <ode>
-            <mu>1.2</mu>
-            <mu2>0</mu2>
-            <fdir1 gz:expressed_in="base">1 1 0</fdir1>
-          </ode>
-        </friction>
-      </surface>
-    </collision>
-  </link>
-  <joint name="wheel_bl_joint" type="continuous">
-    <parent link="base_link"/>
-    <child link="wheel_bl_link"/>
-    <axis xyz="0 1 0"/>
-    <limit effort="100000" velocity="5"/>
-    <origin rpy="0 0 0" xyz="-0.11  0.11 0"/>
-  </joint>
-  <gazebo>
-    <plugin filename="gz-sim-mecanum-drive-system" name="gz::sim::systems::MecanumDrive">
-      <front_left_joint>wheel_fl_joint</front_left_joint>
-      <front_right_joint>wheel_fr_joint</front_right_joint>
-      <back_left_joint>wheel_bl_joint</back_left_joint>
-      <back_right_joint>wheel_br_joint</back_right_joint>
-      <wheel_separation>0.22</wheel_separation>
-      <wheelbase>0.22</wheelbase>
-      <wheel_radius>0.04</wheel_radius>
-      <min_acceleration>-5</min_acceleration>
-      <max_acceleration>5</max_acceleration>
-    </plugin>
-  </gazebo>
-</robot>

src/sherpa/urdf/sherpa.xacro

@@ -1,5 +1,11 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="sherpa">   
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="sherpa">  
+    <gazebo>
+        <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system.so">
+            <parameters>$(find sherpa)/config/ackermann_controller.yaml</parameters>
+        </plugin>
+    </gazebo>
+
     <link name="base_link">
         <inertial>
             <mass value="15"/>
@@ -14,27 +20,89 @@
         </inertial>
     </link>
 
-    <xacro:include filename="components/mecanum_wheel.xacro"/>
+    <xacro:include filename="components/camera.xacro"/>
+    <xacro:include filename="components/imu.xacro"/>
+    <xacro:include filename="components/wheel.xacro"/>
     <xacro:include filename="components/ouster_lidar.xacro"/>
 
-    <xacro:ouster_lidar name="lidar" parent="base" xyz="0 0 0.15" rpy="0 0 0" model="vlp16" frequency="10" topic="/points"/>
-    
-    <xacro:mecanum_wheel name="wheel_fr" parent="base" xyz=" 0.11 -0.11 0" rpy="0 0 0" direction="r" radius="0.04" mass="0.25"/>
-    <xacro:mecanum_wheel name="wheel_fl" parent="base" xyz=" 0.11  0.11 0" rpy="0 0 0" direction="l" radius="0.04" mass="0.25"/>
-    <xacro:mecanum_wheel name="wheel_br" parent="base" xyz="-0.11 -0.11 0" rpy="0 0 0" direction="l" radius="0.04" mass="0.25"/>
-    <xacro:mecanum_wheel name="wheel_bl" parent="base" xyz="-0.11  0.11 0" rpy="0 0 0" direction="r" radius="0.04" mass="0.25"/>
+    <xacro:camera name="front_camera" parent="base" xyz="0 0 0.75" rpy="0 0 0" frequency="30" fov="120" topic="/front_camera"/>
+    <xacro:imu name="imu" parent="base" xyz="0 0 0.35" rpy="0 0.05 0" frequency="300" topic="/imu"/>
+    <xacro:ouster_lidar name="lidar" parent="base" xyz="0 0 0.35" rpy="0 0.05 0" model="vlp16" frequency="10" topic="/velodyne_points"/>
 
-    <gazebo>
-        <plugin filename="gz-sim-mecanum-drive-system" name="gz::sim::systems::MecanumDrive">
-            <front_left_joint>wheel_fl_joint</front_left_joint>
-            <front_right_joint>wheel_fr_joint</front_right_joint>
-            <back_left_joint>wheel_bl_joint</back_left_joint>
-            <back_right_joint>wheel_br_joint</back_right_joint>
-            <wheel_separation>0.22</wheel_separation>
-            <wheelbase>0.22</wheelbase>
-            <wheel_radius>0.04</wheel_radius>
-            <min_acceleration>-5</min_acceleration>
-            <max_acceleration>5</max_acceleration>
-        </plugin>
-    </gazebo>
-</robot>
+    <link name="steer_fl_link">
+        <inertial>
+            <mass value="0.1"/>
+            <inertia 
+                ixx="0.0"
+                ixy="0.0"
+                ixz="0.0"
+                iyy="0.0"
+                iyz="0.0"
+                izz="0.0"
+            />
+        </inertial>
+    </link>
+    <link name="steer_fr_link">
+    <inertial>
+            <mass value="0.1"/>
+            <inertia 
+                ixx="0.0"
+                ixy="0.0"
+                ixz="0.0"
+                iyy="0.0"
+                iyz="0.0"
+                izz="0.0"
+            />
+        </inertial>
+    </link>
+    
+    <xacro:wheel name="wheel_fl" parent="steer_fl" xyz="0  0.05 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
+    <xacro:wheel name="wheel_fr" parent="steer_fr" xyz="0 -0.05 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
+    <xacro:wheel name="wheel_bl" parent="base" xyz="-0.3  0.25 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
+    <xacro:wheel name="wheel_br" parent="base" xyz="-0.3 -0.25 0" rpy="0 0 0" radius="0.15" mass="2.5"/>    
+
+    <joint name="steer_fl_joint" type="revolute">
+        <parent link="base_link"/>
+        <child link="steer_fl_link"/>
+        <origin xyz="0.3 0.20 0" rpy="0 0 0"/>
+        <axis xyz="0 0 1"/>
+        <limit upper="0.3" lower="-0.3" effort="-1" velocity="-1"/>
+    </joint>
+
+    <joint name="steer_fr_joint" type="revolute">
+        <parent link="base_link"/>
+        <child link="steer_fr_link"/>
+        <origin xyz="0.3 -0.20 0" rpy="0 0 0"/>
+        <axis xyz="0 0 1"/>
+        <limit upper="0.3" lower="-0.3" effort="-1" velocity="-1"/>
+    </joint>
+
+    
+    <ros2_control name="SherpaSystem" type="system">
+        <hardware>
+            <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+        </hardware>
+    
+        <joint name="wheel_bl_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+            <state_interface name="position"/>
+        </joint>
+
+        <joint name="wheel_br_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+            <state_interface name="position"/>
+        </joint>
+
+        <joint name="steer_fl_joint">
+            <command_interface name="position"/>
+            <state_interface name="position"/>
+        </joint>
+
+        <joint name="steer_fr_joint">
+            <command_interface name="position"/>
+            <state_interface name="position"/>
+        </joint>
+    </ros2_control>
+</robot>

src/sherpa/worlds/asd_course.world

@@ -0,0 +1,170 @@
+<?xml version="1.0"?>
+<sdf version="1.7">
+    <world name="default">
+
+        <!-- A simple light -->
+        <light name="sun" type="directional">
+            <pose>0 0 20 0 0 0</pose>
+            <diffuse>0.8 0.8 0.8 1.0</diffuse>
+            <specular>0.8 0.8 0.8 1.0</specular>
+            <attenuation>
+                <range>1000</range>
+                <constant>0.9</constant>
+                <linear>0.05</linear>
+                <quadratic>0.01</quadratic>
+            </attenuation>
+            <cast_shadows>false</cast_shadows>
+        </light>
+        <scene>
+            <grid>false</grid>
+        </scene>
+        <plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
+        </plugin>
+        <plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
+            <render_engine>ogre2</render_engine>
+        </plugin>
+        <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu">
+        </plugin>
+        <plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
+        </plugin>
+        <plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
+        </plugin>
+
+        <gui fullscreen="0">
+
+            <!-- 3D scene -->
+            <plugin filename="GzScene3D" name="3D View">
+                <ignition-gui>
+                    <title>3D View</title>
+                    <property type="bool" key="showTitleBar">false</property>
+                    <property type="string" key="state">docked</property>
+                </ignition-gui>
+
+                <engine>ogre2</engine>
+                <scene>scene</scene>
+                <ambient_light>0.4 0.4 0.4</ambient_light>
+                <background_color>0.5 0.8 0.95</background_color>
+                <!--camera_pose>276.46 -130.36 5.27 0 0.43 -1.24</camera_pose-->
+                <camera_follow>
+                    <target>prius_hybrid</target>
+                    <p_gain>0.5</p_gain>
+                    <offset>0.5 5 2</offset>
+                </camera_follow>
+            </plugin>
+
+            <!-- World control -->
+            <plugin filename="WorldControl" name="World control">
+                <ignition-gui>
+                    <title>World control</title>
+                    <property type="bool" key="showTitleBar">false</property>
+                    <property type="bool" key="resizable">false</property>
+                    <property type="double" key="height">72</property>
+                    <property type="double" key="width">121</property>
+                    <property type="double" key="z">1</property>
+
+                    <property type="string" key="state">floating</property>
+                    <anchors target="3D View">
+                        <line own="left" target="left"/>
+                        <line own="bottom" target="bottom"/>
+                    </anchors>
+                </ignition-gui>
+
+                <play_pause>true</play_pause>
+                <step>true</step>
+                <start_paused>true</start_paused>
+
+            </plugin>
+
+            <!-- World statistics -->
+            <plugin filename="WorldStats" name="World stats">
+                <ignition-gui>
+                    <title>World stats</title>
+                    <property type="bool" key="showTitleBar">false</property>
+                    <property type="bool" key="resizable">false</property>
+                    <property type="double" key="height">110</property>
+                    <property type="double" key="width">290</property>
+                    <property type="double" key="z">1</property>
+
+                    <property type="string" key="state">floating</property>
+                    <anchors target="3D View">
+                        <line own="right" target="right"/>
+                        <line own="bottom" target="bottom"/>
+                    </anchors>
+                </ignition-gui>
+
+                <sim_time>true</sim_time>
+                <real_time>true</real_time>
+                <real_time_factor>true</real_time_factor>
+                <iterations>true</iterations>
+            </plugin>
+
+            <plugin filename="ImageDisplay" name="dis">
+                <ignition-gui>
+                    <property key="state" type="string">docked</property>
+                </ignition-gui>
+                <topic>front_camera</topic>
+            </plugin>
+
+            <plugin filename="Teleop" name="Teleop">
+                <ignition-gui>
+                    <property type="string" key="state">docked</property>
+                </ignition-gui>
+                <topic>/cmd_vel</topic>
+            </plugin>
+        </gui>
+
+        <model name="carolo_track">
+            <static>true</static>
+            <link name="link">
+                <collision name="collision">
+                    <geometry>
+                        <plane>
+                            <normal>0 0 1</normal>
+                            <size>200 200</size>
+                        </plane>
+                    </geometry>
+                    <surface>
+                        <friction>
+                            <ode>
+                                <mu>50</mu>
+                            </ode>
+                        </friction>
+                    </surface>
+                </collision>
+                <visual name="visual">
+                    <cast_shadows>false</cast_shadows>
+                    <geometry>
+                        <plane>
+                            <normal>0 0 1</normal>
+                            <size>35 35</size>
+                        </plane>
+                    </geometry>
+                    <material>
+                        <pbr>
+                            <metal>
+                                <albedo_map>file://sherpa/materials/textures/track.png</albedo_map>
+                            </metal>
+                        </pbr>
+                        <ambient>1 1 1 1</ambient>
+                        <diffuse>1 1 1 1</diffuse>
+                    </material>
+                </visual>
+                <pose>0 0 0 0 0 0</pose>
+                <inertial>
+                    <pose>0 0 0 0 0 0</pose>
+                    <mass>1</mass>
+                    <inertia>
+                        <ixx>1</ixx>
+                        <ixy>0</ixy>
+                        <ixz>0</ixz>
+                        <iyy>1</iyy>
+                        <iyz>0</iyz>
+                        <izz>1</izz>
+                    </inertia>
+                </inertial>
+            </link>
+            <pose>0 0 0 0 0 0</pose>
+            <self_collide>false</self_collide>
+        </model>
+    </world>
+</sdf>

src/sherpa/worlds/empty.world

@@ -1,6 +1,31 @@
 <?xml version="1.0" ?>
 <sdf version="1.5">
   <world name="empty">
+    <physics type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_update_rate>100</real_time_update_rate>
+    </physics>
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre</render_engine>
+    </plugin>
+    <plugin filename="gz-sim-imu-system"
+        name="gz::sim::systems::Imu">
+    </plugin>
+
     <light name="sun" type="directional">
       <pose>0 0 20 0 0 0</pose>
       <diffuse>0.8 0.8 0.8 1.0</diffuse>
@@ -11,7 +36,7 @@
         <linear>0.05</linear>
         <quadratic>0.01</quadratic>
       </attenuation>
-      <cast_shadows>true</cast_shadows>
+      <cast_shadows>false</cast_shadows>
     </light>
 
     <model name="ground_plane">
@@ -27,10 +52,11 @@
           <surface>
             <friction>
               <ode>
-                <mu>0.8</mu>
-                <mu2>0.6</mu2>
+                <mu>50</mu>
               </ode>
             </friction>
+            <bounce/>
+            <contact/>
           </surface>
         </collision>
         <visual name="visual">
@@ -48,7 +74,22 @@
             </script>
           </material>
         </visual>
+        <pose>0 0 0 0 0 0</pose>
+        <inertial>
+          <pose>0 0 0 0 0 0</pose>
+          <mass>1</mass>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+        </inertial>
       </link>
+      <pose>0 0 0 0 0 0</pose>
+      <self_collide>false</self_collide>
     </model>
   </world>
-</sdf>
+</sdf>

src/sherpa/worlds/raceway.world

@@ -0,0 +1,115 @@
+<?xml version="1.0" ?>
+<sdf version='1.7'>
+  <world name='sonoma'>
+    <scene>
+      <grid>false</grid>
+    </scene>
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-sensors-system"
+      name="ignition::gazebo::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin filename="gz-sim-imu-system"
+        name="gz::sim::systems::Imu">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="ignition::gazebo::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+
+    <gui fullscreen="0">
+
+      <!-- 3D scene -->
+      <plugin filename="GzScene3D" name="3D View">
+        <ignition-gui>
+          <title>3D View</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+
+        <engine>ogre2</engine>
+        <scene>scene</scene>
+        <ambient_light>0.4 0.4 0.4</ambient_light>
+        <background_color>0.5 0.8 0.95</background_color>
+        <!--camera_pose>276.46 -130.36 5.27 0 0.43 -1.24</camera_pose-->
+        <camera_follow>
+          <target>prius_hybrid</target>
+          <p_gain>0.5</p_gain>
+          <offset>0.5 5 2</offset>
+        </camera_follow>
+      </plugin>
+
+      <!-- World control -->
+      <plugin filename="WorldControl" name="World control">
+        <ignition-gui>
+          <title>World control</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">72</property>
+          <property type="double" key="width">121</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="left" target="left"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </ignition-gui>
+
+        <play_pause>true</play_pause>
+        <step>true</step>
+        <start_paused>true</start_paused>
+
+      </plugin>
+
+      <!-- World statistics -->
+      <plugin filename="WorldStats" name="World stats">
+        <ignition-gui>
+          <title>World stats</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">110</property>
+          <property type="double" key="width">290</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="right" target="right"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </ignition-gui>
+
+        <sim_time>true</sim_time>
+        <real_time>true</real_time>
+        <real_time_factor>true</real_time_factor>
+        <iterations>true</iterations>
+      </plugin>
+
+      <plugin filename="ImageDisplay" name="dis">
+        <ignition-gui>
+          <property key="state" type="string">docked</property>
+        </ignition-gui>
+        <topic>front_camera</topic>
+      </plugin>
+
+      <plugin filename="Teleop" name="Teleop">
+        <ignition-gui>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+        <topic>/cmd_vel</topic>
+      </plugin>
+    </gui>
+    <include>
+      <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Sonoma Raceway</uri>
+    </include>
+
+  </world>
+</sdf>

src/target_tracking/CMakeLists.txt

@@ -0,0 +1,85 @@
+cmake_minimum_required(VERSION 3.8)
+project(target_tracking)
+
+# Required packages
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(PCL REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_sensor_msgs REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(visualization_msgs REQUIRED)
+
+
+# Include paths
+include_directories(
+  include
+  ${PCL_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+# Node for preprocessing
+add_executable(cloud_preprocessing_node 
+      src/cloud_preprocessing.cpp
+)
+
+# Dependencies for preprocessing
+ament_target_dependencies(cloud_preprocessing_node
+  rclcpp
+  sensor_msgs
+  pcl_conversions
+  tf2
+  tf2_ros
+  tf2_sensor_msgs
+)
+
+# Node for clustering
+add_executable(cloud_clustering_node
+      src/cloud_clustering_node.cpp
+)
+
+add_library(cloud_clustering SHARED
+  src/cloud_clustering.cpp)
+target_compile_definitions(cloud_clustering
+  PRIVATE "COMPOSITION_BUILDING_DLL")
+
+
+# Dependencies for clustering
+ament_target_dependencies(cloud_clustering
+  rclcpp
+  sensor_msgs
+  visualization_msgs 
+  PCL
+  pcl_conversions
+  OpenCV
+)
+
+rclcpp_components_register_nodes(cloud_clustering "cloud_clustering::CloudClustering")
+set(node_plugins "${node_plugins}cloud_clustering::CloudClustering;$<TARGET_FILE:cloud_clustering>\n")
+
+target_link_libraries(cloud_clustering_node cloud_clustering)
+
+# Install target
+install(TARGETS
+  cloud_preprocessing_node  
+  cloud_clustering_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(TARGETS
+    cloud_clustering
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION lib/${PACKAGE_NAME}
+)
+
+# Install launchfile
+install(
+  DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
+ament_package()

src/target_tracking/README.md

@@ -0,0 +1,3 @@
+# Target tracking
+
+Ros2 implementation of a filtering node in combination with a clustering node to find a target with lidar data

src/target_tracking/include/cloud_clustering.hpp

@@ -0,0 +1,106 @@
+#ifndef CLOUD_CLUSTERING_HPP
+#define CLOUD_CLUSTERING_HPP
+
+#include "rclcpp/rclcpp.hpp"
+
+#include <geometry_msgs/msg/point.hpp>
+#include "visualization_msgs/msg/marker_array.hpp"
+
+#include "opencv2/video/tracking.hpp"
+
+#include "std_msgs/msg/float32_multi_array.hpp"
+#include "std_msgs/msg/int32_multi_array.hpp"
+
+#include "sensor_msgs/msg/point_cloud2.hpp"
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+namespace cloud_clustering
+{
+    // KF init
+    int stateDim = 4; // [x,y,v_x,v_y]//,w,h]
+    int measDim = 2;  // [z_x,z_y,z_w,z_h]
+    int ctrlDim = 0;
+
+    std::string frame_id;
+    std::string topic_in;
+
+    float z_dim_scale;
+    float cluster_tolerance;
+    int min_cluster_size;
+    int max_cluster_size;
+    float min_width;
+    float min_height;
+    float min_length;
+    float max_width;
+    float max_height;
+    float max_length;
+
+    cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F);
+    cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F);
+    cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F);
+    cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F);
+    cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F);
+    cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F);
+
+    std::vector<geometry_msgs::msg::Point> prevClusterCenters;
+
+    cv::Mat state(stateDim, 1, CV_32F);
+    cv::Mat_<float> measurement(2, 1);
+
+    std::vector<int> objID; // Output of the data association using KF
+                            // measurement.setTo(Scalar(0));
+
+    bool firstFrame = true;
+    /*
+    cv::Mat state(stateDim, 1, CV_32F);
+    cv::Mat_<float> measurement(2, 1);
+    */
+
+    class CloudClustering : public rclcpp::Node
+    {
+    public:
+        CloudClustering(
+            const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
+        CloudClustering(
+            const std::string &name_space,
+            const rclcpp::NodeOptions &options = rclcpp::NodeOptions());
+
+    private:
+        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub;
+        rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr markerPub;
+        rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr objID_pub;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster0;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster1;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster2;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster3;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster4;
+        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cluster5;
+
+        std::vector<geometry_msgs::msg::Point> prevClusterCenters;
+
+        std::vector<int> objID; // Output of the data association using KF
+                                // measurement.setTo(Scalar(0));
+
+        bool firstFrame = true;
+        rclcpp::Clock::SharedPtr clock_;
+        std::string frame_id;
+        std::string filtered_cloud;
+
+        double euclidean_distance(geometry_msgs::msg::Point &p1, geometry_msgs::msg::Point &p2);
+        std::pair<int, int> findIndexOfMin(std::vector<std::vector<float>> distMat);
+        void KFT(const std_msgs::msg::Float32MultiArray ccs);
+        void publish_cloud(rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub,
+                           pcl::PointCloud<pcl::PointXYZI>::Ptr cluster);
+
+        void parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
+                         std::vector<std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, float>> &cluster_vec,
+                         std::vector<pcl::PointXYZI> &clusterCentroids);
+        void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input);
+    };
+
+}
+#endif // CLOUD_CLUSTERING_HPP

src/target_tracking/include/hungarian.hpp

@@ -0,0 +1,66 @@
+#include <vector>
+#include <limits>
+#include <algorithm>
+
+class Hungarian {
+public:
+    Hungarian(const std::vector<std::vector<float>>& costMatrix)
+        : n(costMatrix.size()), m(costMatrix[0].size()),
+          cost(costMatrix), u(n + 1), v(m + 1),
+          p(m + 1), way(m + 1) {}
+
+    // Returns assignment vector: assignment[i] = j
+    std::vector<int> solve() {
+        for (int i = 1; i <= n; i++) {
+            p[0] = i;
+            int j0 = 0;
+            std::vector<float> minv(m + 1, std::numeric_limits<float>::max());
+            std::vector<char> used(m + 1, false);
+            do {
+                used[j0] = true;
+                int i0 = p[j0], j1 = 0;
+                float delta = std::numeric_limits<float>::max();
+                for (int j = 1; j <= m; j++) {
+                    if (!used[j]) {
+                        float cur = cost[i0 - 1][j - 1] - u[i0] - v[j];
+                        if (cur < minv[j]) {
+                            minv[j] = cur;
+                            way[j] = j0;
+                        }
+                        if (minv[j] < delta) {
+                            delta = minv[j];
+                            j1 = j;
+                        }
+                    }
+                }
+                for (int j = 0; j <= m; j++) {
+                    if (used[j]) {
+                        u[p[j]] += delta;
+                        v[j] -= delta;
+                    } else {
+                        minv[j] -= delta;
+                    }
+                }
+                j0 = j1;
+            } while (p[j0] != 0);
+            do {
+                int j1 = way[j0];
+                p[j0] = p[j1];
+                j0 = j1;
+            } while (j0);
+        }
+        std::vector<int> assignment(n, -1);
+        for (int j = 1; j <= m; j++) {
+            if (p[j] != 0) {
+                assignment[p[j] - 1] = j - 1;
+            }
+        }
+        return assignment;
+    }
+
+private:
+    int n, m;
+    std::vector<std::vector<float>> cost;
+    std::vector<float> u, v;
+    std::vector<int> p, way;
+};

src/target_tracking/launch/target_tracking_launch.py

@@ -0,0 +1,81 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import math
+
+################### user configure parameters for ros2 start ###################
+# Topics/Frames
+frame_id        = 'velodyne'
+topic_preprocessing_in  = 'filtered_points'
+topic_preprocessing_out = 'new_filtered'
+
+# Preprocessing
+x_min           = 0.0
+x_max           = 20.0
+z_min           = -0.5
+z_max           = 1.5
+tan_h_fov       = math.pi / 4 # ±45°
+tan_v_fov       = math.pi / 6 # ±30°
+
+# Clustering
+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
+
+################### user configure parameters for ros2 end ##################### 
+
+cloud_preprocessing_params = [
+    {"topic_in": topic_preprocessing_in},
+    {"topic_out": topic_preprocessing_out},
+    {"x_min": x_min},
+    {"x_max": x_max},
+    {"z_min": z_min},
+    {"z_max": z_max},
+    {"tan_h_fov": tan_h_fov},
+    {"tan_v_fov": tan_v_fov}  
+]
+
+cloud_clustering_params = [
+    {"topic_in": topic_preprocessing_out},
+    {"frame_id": frame_id},
+    {"z_dim_scale": z_dim_scale},
+    {"cluster_tolerance": cluster_tolerance},
+    {"min_cluster_size": min_cluster_size},
+    {"max_cluster_size": max_cluster_size},
+    {"min_width": min_width},
+    {"min_height": min_height},
+    {"min_length": min_length},
+    {"max_width": max_width},
+    {"max_height": max_height},
+    {"max_length": max_length}
+]
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='target_tracking',
+            executable='cloud_preprocessing_node',
+            name='cloud_preprocessing',
+            parameters=cloud_preprocessing_params,
+            output={
+                'stdout': 'screen',
+                'stderr': 'screen',
+            }
+        ),
+         Node(
+            package='target_tracking',
+            executable='cloud_clustering_node',
+            name='cloud_clustering',
+            parameters=cloud_clustering_params,
+            output={
+                'stdout': 'screen',
+                'stderr': 'screen',
+            }
+        )
+    ])

src/target_tracking/package.xml

@@ -0,0 +1,27 @@
+<?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>target_tracking</name>
+  <version>1.0.1</version>
+  <description>Studienprojekt</description>
+  <maintainer email="liam20030625@gmail.com">Liam</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 voxel -->
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>PCL</depend>
+  <depend>cv_bridge</depend>
+  <depend>OpenCV</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/target_tracking/src/cloud_clustering.cpp

@@ -0,0 +1,691 @@
+// Default C++ imports
+#include <limits>
+#include <utility>
+#include <string.h>
+#include <iostream>
+#include <iterator>
+#include <algorithm>
+#include <fstream>
+
+// Headerfiles of project
+#include "cloud_clustering.hpp"
+#include <hungarian.hpp>
+
+// Ros2 import
+#include <rclcpp/rclcpp.hpp>
+
+#include <opencv2/video/video.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+
+// PCL Types
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+// Ros2 messages
+#include <std_msgs/msg/float32_multi_array.hpp>
+#include <std_msgs/msg/int32_multi_array.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/centroid.h>
+#include <pcl/common/geometry.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+namespace cloud_clustering
+{
+  static const rclcpp::Logger LOGGER = rclcpp::get_logger("cloud_clustering");
+
+  CloudClustering::CloudClustering(
+      const rclcpp::NodeOptions &options) : CloudClustering("", options)
+  {
+  }
+
+  CloudClustering::CloudClustering(
+      const std::string &name_space,
+      const rclcpp::NodeOptions &options) : Node("CloudClustering", name_space, options)
+  {
+    // Topic parameters
+    this->declare_parameter<std::string>("topic_in", "filtered_points");
+    this->declare_parameter<std::string>("frame_id", "velodyne");
+
+    // Cluster parameters
+    this->declare_parameter<float>("z_dim_scale", 0);
+    this->declare_parameter<float>("cluster_tolerance", 0.0f);
+    this->declare_parameter<int>("min_cluster_size", 0);
+    this->declare_parameter<int>("max_cluster_size", 0);
+    this->declare_parameter<float>("min_width", 0.0f);
+    this->declare_parameter<float>("min_height", 0.0f);
+    this->declare_parameter<float>("min_length", 0.0f); 
+    this->declare_parameter<float>("max_width", 0.0f);
+    this->declare_parameter<float>("max_height", 0.0f);
+    this->declare_parameter<float>("max_length", 0.0f);    
+
+    this->get_parameter("topic_in", topic_in);
+    this->get_parameter("frame_id", frame_id);
+    this->get_parameter("z_dim_scale", z_dim_scale);
+    this->get_parameter("cluster_tolerance", cluster_tolerance);
+    this->get_parameter("min_cluster_size", min_cluster_size);
+    this->get_parameter("max_cluster_size", max_cluster_size);
+    this->get_parameter("min_width", min_width);
+    this->get_parameter("min_height", min_height);
+    this->get_parameter("min_length", min_length);
+    this->get_parameter("max_width", max_width);
+    this->get_parameter("max_height", max_height);
+    this->get_parameter("max_length", max_length);
+
+    // Initialize publishers
+    objID_pub = this->create_publisher<std_msgs::msg::Int32MultiArray>("object_ids", 10);
+    pub_cluster0 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster0", 10);
+    pub_cluster1 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster1", 10);
+    pub_cluster2 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster2", 10);
+    pub_cluster3 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster3", 10);
+    pub_cluster4 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster4", 10);
+    pub_cluster5 = this->create_publisher<sensor_msgs::msg::PointCloud2>("cluster5", 10);
+    markerPub = this->create_publisher<visualization_msgs::msg::MarkerArray>("markers", 10);
+
+    RCLCPP_INFO(this->get_logger(), "About to setup callback");
+    clock_ = this->get_clock();
+
+    // Create a ROS subscriber for the input point cloud
+    sub = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+        topic_in,
+        10, // queue size
+        std::bind(&CloudClustering::cloud_cb, this, std::placeholders::_1));
+
+    std::cout << "Started clustering node with parameters:\n"
+         << "topic_in: " << topic_in << "\n"
+         << "frame_id: " << frame_id << "\n"
+         << "z_dim_scale: " << z_dim_scale << "\n"
+         << "cluster_tolerance: " << cluster_tolerance << "\n"
+         << "min_cluster_size: " << min_cluster_size << "\n"
+         << "max_cluster_size: " << max_cluster_size << "\n";
+  }
+
+  // calculate euclidean distance of two points
+  double CloudClustering::euclidean_distance(geometry_msgs::msg::Point &p1, geometry_msgs::msg::Point &p2)
+  {
+    return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) +
+                (p1.z - p2.z) * (p1.z - p2.z) * z_dim_scale);
+  }
+  /*
+  //Count unique object IDs. just to make sure same ID has not been assigned to
+  two KF_Trackers. int countIDs(vector<int> v)
+  {
+      transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n =
+  distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log
+  n), worst case O(n^2) (usually implemented as quicksort.
+      // To guarantee worst case O(n log n) replace with make_heap, then
+  sort_heap.
+
+      // Unique will take a sorted range, and move things around to get duplicated
+      // items to the back and returns an iterator to the end of the unique
+  section of the range auto unique_end = unique(v.begin(), v.end()); // Again n
+  comparisons return distance(unique_end, v.begin()); // Constant time for random
+  access iterators (like vector's)
+  }
+  */
+
+  /*
+
+  objID: vector containing the IDs of the clusters that should be associated with
+  each KF_Tracker objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc.
+  */
+
+  std::pair<int, int> CloudClustering::findIndexOfMin(std::vector<std::vector<float>> distMat)
+  {
+    std::pair<int, int> minIndex;
+    float minEl = std::numeric_limits<float>::max();
+    for (int i = 0; i < distMat.size(); i++)
+      for (int j = 0; j < distMat.at(0).size(); j++)
+      {
+        if (distMat[i][j] < minEl)
+        {
+          minEl = distMat[i][j];
+          minIndex = std::make_pair(i, j);
+        }
+      }
+    return minIndex;
+  }
+
+  void CloudClustering::KFT(const std_msgs::msg::Float32MultiArray ccs)
+  {
+
+    // First predict, to update the internal statePre variable
+
+    std::vector<cv::Mat> pred{KF0.predict(), KF1.predict(), KF2.predict(),
+                              KF3.predict(), KF4.predict(), KF5.predict()};
+
+    // cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
+    // cout<<"Prediction 1
+    // ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
+
+    // Get measurements
+    // Extract the position of the clusters forom the multiArray. To check if the
+    // data coming in, check the .z (every third) coordinate and that will be 0.0
+    std::vector<geometry_msgs::msg::Point> clusterCenters; // clusterCenters
+
+    int i = 0;
+    for (std::vector<float>::const_iterator it = ccs.data.begin();
+         it != ccs.data.end(); it += 3)
+    {
+      geometry_msgs::msg::Point pt;
+      pt.x = *it;
+      pt.y = *(it + 1);
+      pt.z = *(it + 2);
+
+      clusterCenters.push_back(pt);
+    }
+
+    std::vector<geometry_msgs::msg::Point> KFpredictions;
+    i = 0;
+    for (auto it = pred.begin(); it != pred.end(); it++)
+    {
+      geometry_msgs::msg::Point pt;
+      pt.x = (*it).at<float>(0);
+      pt.y = (*it).at<float>(1);
+      pt.z = (*it).at<float>(2);
+
+      KFpredictions.push_back(pt);
+    }
+
+    // Find the cluster that is more probable to be belonging to a given KF.
+    objID.clear();   // Clear the objID vector
+    objID.resize(6); // Allocate default elements so that [i] doesnt segfault.
+                     // Should be done better
+    // Copy clusterCentres for modifying it and preventing multiple assignments of
+    // the same ID
+    std::vector<geometry_msgs::msg::Point> copyOfClusterCenters(clusterCenters);
+    std::vector<std::vector<float>> distMat;
+
+    for (int filterN = 0; filterN < 6; filterN++)
+    {
+      std::vector<float> distVec;
+      for (int n = 0; n < 6; n++)
+      {
+        distVec.push_back(
+            euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[n]));
+      }
+
+      distMat.push_back(distVec);
+      /*// Based on distVec instead of distMat (global min). Has problems with the
+      person's leg going out of scope int
+      ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end()));
+       //cout<<"finterlN="<<filterN<<"   minID="<<ID
+       objID.push_back(ID);
+      // Prevent assignment of the same object ID to multiple clusters
+       copyOfClusterCenters[ID].x=100000;// A large value so that this center is
+      not assigned to another cluster copyOfClusterCenters[ID].y=10000;
+       copyOfClusterCenters[ID].z=10000;
+      */
+      // cout << "filterN=" << filterN << "\n";
+    }
+
+    Hungarian hungarian(distMat);
+    std::vector<int> assignment = hungarian.solve();
+
+    for (int i = 0; i < assignment.size(); i++) {
+        objID[i] = assignment[i];
+    }
+
+    /*
+    for (int clusterCount = 0; clusterCount < 6; clusterCount++)
+    {
+      // 1. Find min(distMax)==> (i,j);
+      std::pair<int, int> minIndex(findIndexOfMin(distMat));
+      // 2. objID[i]=clusterCenters[j]; counter++
+      objID[minIndex.first] = minIndex.second;
+
+      // 3. distMat[i,:]=10000; distMat[:,j]=10000
+      distMat[minIndex.first] =
+          std::vector<float>(6, 10000.0); // Set the row to a high number.
+      for (int row = 0; row < distMat.size();
+           row++) // set the column to a high number
+      {
+        distMat[row][minIndex.second] = 10000.0;
+      }
+      // 4. if(counter<6) got to 1.
+      // cout << "clusterCount=" << clusterCount << "\n";
+    }
+    */
+    
+
+    // cout<<"Got object IDs"<<"\n";
+    // countIDs(objID);// for verif/corner cases
+
+    // display objIDs
+    /* DEBUG
+      cout<<"objID= ";
+      for(auto it=objID.begin();it!=objID.end();it++)
+          cout<<*it<<" ,";
+      cout<<"\n";
+      */
+
+    visualization_msgs::msg::MarkerArray clusterMarkers;
+
+    for (int i = 0; i < 6; i++)
+    {
+      visualization_msgs::msg::Marker m;
+
+      m.id = i;
+      m.type = visualization_msgs::msg::Marker::CUBE;
+      m.header.frame_id = frame_id;
+      m.scale.x = 0.3;
+      m.scale.y = 0.3;
+      m.scale.z = 0.3;
+      m.action = visualization_msgs::msg::Marker::ADD;
+      m.color.a = 1.0;
+      m.color.r = i % 2 ? 1 : 0;
+      m.color.g = i % 3 ? 1 : 0;
+      m.color.b = i % 4 ? 1 : 0;
+
+      // geometry_msgs::msg::Point clusterC(clusterCenters.at(objID[i]));
+      geometry_msgs::msg::Point clusterC(KFpredictions[i]);
+      m.pose.position.x = clusterC.x;
+      m.pose.position.y = clusterC.y;
+      m.pose.position.z = clusterC.z;
+
+      clusterMarkers.markers.push_back(m);
+    }
+
+    prevClusterCenters = clusterCenters;
+
+    markerPub->publish(clusterMarkers);
+
+    std_msgs::msg::Int32MultiArray obj_id;
+    for (auto it = objID.begin(); it != objID.end(); it++)
+      obj_id.data.push_back(*it);
+    // Publish the object IDs
+    objID_pub->publish(obj_id);
+    // convert clusterCenters from geometry_msgs::msg::Point to floats
+    std::vector<std::vector<float>> cc;
+    for (int i = 0; i < 6; i++)
+    {
+      std::vector<float> pt;
+      pt.push_back(clusterCenters[objID[i]].x);
+      pt.push_back(clusterCenters[objID[i]].y);
+      pt.push_back(clusterCenters[objID[i]].z);
+
+      cc.push_back(pt);
+    }
+    // cout<<"cc[5][0]="<<cc[5].at(0)<<"cc[5][1]="<<cc[5].at(1)<<"cc[5][2]="<<cc[5].at(2)<<"\n";
+    float meas0[2] = {cc[0].at(0), cc[0].at(1)};
+    float meas1[2] = {cc[1].at(0), cc[1].at(1)};
+    float meas2[2] = {cc[2].at(0), cc[2].at(1)};
+    float meas3[2] = {cc[3].at(0), cc[3].at(1)};
+    float meas4[2] = {cc[4].at(0), cc[4].at(1)};
+    float meas5[2] = {cc[5].at(0), cc[5].at(1)};
+
+    // The update phase
+    cv::Mat meas0Mat = cv::Mat(2, 1, CV_32F, meas0);
+    cv::Mat meas1Mat = cv::Mat(2, 1, CV_32F, meas1);
+    cv::Mat meas2Mat = cv::Mat(2, 1, CV_32F, meas2);
+    cv::Mat meas3Mat = cv::Mat(2, 1, CV_32F, meas3);
+    cv::Mat meas4Mat = cv::Mat(2, 1, CV_32F, meas4);
+    cv::Mat meas5Mat = cv::Mat(2, 1, CV_32F, meas5);
+
+    // cout<<"meas0Mat"<<meas0Mat<<"\n";
+    if (!(meas0Mat.at<float>(0, 0) == 0.0f || meas0Mat.at<float>(1, 0) == 0.0f))
+      cv::Mat estimated0 = KF0.correct(meas0Mat);
+    if (!(meas1[0] == 0.0f || meas1[1] == 0.0f))
+      cv::Mat estimated1 = KF1.correct(meas1Mat);
+    if (!(meas2[0] == 0.0f || meas2[1] == 0.0f))
+      cv::Mat estimated2 = KF2.correct(meas2Mat);
+    if (!(meas3[0] == 0.0f || meas3[1] == 0.0f))
+      cv::Mat estimated3 = KF3.correct(meas3Mat);
+    if (!(meas4[0] == 0.0f || meas4[1] == 0.0f))
+      cv::Mat estimated4 = KF4.correct(meas4Mat);
+    if (!(meas5[0] == 0.0f || meas5[1] == 0.0f))
+      cv::Mat estimated5 = KF5.correct(meas5Mat);
+
+    // Publish the point clouds belonging to each clusters
+
+    // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
+    // Point statePt(estimated.at<float>(0),estimated.at<float>(1));
+    // cout<<"DONE KF_TRACKER\n";
+  }
+  void CloudClustering::publish_cloud(
+      rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub,
+      pcl::PointCloud<pcl::PointXYZI>::Ptr cluster)
+  {
+    // Create a PointCloud2 message
+    auto clustermsg = std::make_shared<sensor_msgs::msg::PointCloud2>();
+
+    // Convert PCL cloud to ROS 2 message
+    pcl::toROSMsg(*cluster, *clustermsg);
+
+    // Set header info
+    clustermsg->header.frame_id = frame_id;
+    clustermsg->header.stamp = clock_->now();
+
+    // Publish the message
+    pub->publish(*clustermsg);
+  }
+
+  void CloudClustering::parse_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr input,
+                                    std::vector<std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, float>> &cluster_vec,
+                                    std::vector<pcl::PointXYZI> &clusterCentroids)
+  {
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZI>);
+    pcl::fromROSMsg(*input, *input_cloud);
+
+    // Setup KdTree
+    pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
+    tree->setInputCloud(input_cloud);
+
+    std::vector<pcl::PointIndices> cluster_indices;
+    pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
+    ec.setClusterTolerance(cluster_tolerance);
+    ec.setMinClusterSize(min_cluster_size);
+    ec.setMaxClusterSize(max_cluster_size);
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(input_cloud);
+    /* Extract the clusters out of pc and save indices in cluster_indices.*/
+    ec.extract(cluster_indices);
+
+    std::vector<pcl::PointIndices>::const_iterator it;
+    std::vector<int>::const_iterator pit;
+
+    for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
+    {
+
+      pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_cluster(
+          new pcl::PointCloud<pcl::PointXYZI>);
+      float x = 0.0;
+      float y = 0.0;
+      int numPts = 0;
+      float intensity_sum = 0.0;
+
+      float min_x = std::numeric_limits<float>::max();
+      float min_y = std::numeric_limits<float>::max();
+      float min_z = std::numeric_limits<float>::max();
+      float max_x = -std::numeric_limits<float>::max();
+      float max_y = -std::numeric_limits<float>::max();
+      float max_z = -std::numeric_limits<float>::max();
+
+      for (pit = it->indices.begin(); pit != it->indices.end(); pit++)
+      {
+        auto &pt = input_cloud->points[*pit];
+        cloud_cluster->points.push_back(pt);
+        x += pt.x;
+        y += pt.y;
+        numPts++;
+
+        intensity_sum += input_cloud->points[*pit].intensity;
+        // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
+        //                                          origin);
+        // mindist_this_cluster = std::min(dist_this_point,
+        // mindist_this_cluster);
+
+        min_x = std::min(min_x, pt.x);
+        min_y = std::min(min_y, pt.y);
+        min_z = std::min(min_z, pt.z);
+        max_x = std::max(max_x, pt.x);
+        max_y = std::max(max_y, pt.y);
+        max_z = std::max(max_z, pt.z);
+      }
+
+      if (numPts == 0)
+        continue;
+
+
+      float width  = max_x - min_x;
+      float length = max_y - min_y;
+      float height = max_z - min_z;
+    
+      // Reject clusters outside size limits
+      if (width < min_width || width > max_width ||
+          height < min_height || height > max_height ||
+          length < min_length || length > max_length)
+      {
+          continue;
+      }
+      
+
+
+      pcl::PointXYZI centroid;
+      centroid.x = x / numPts;
+      centroid.y = y / numPts;
+      centroid.z = 0.0;
+
+      cluster_vec.push_back(std::make_pair(cloud_cluster, intensity_sum / numPts));
+
+      // Get the centroid of the cluster
+      clusterCentroids.push_back(centroid);
+    }
+
+    // Ensure at least 6 clusters exist to publish (later clusters may be empty)
+    while (cluster_vec.size() < 6)
+    {
+      pcl::PointCloud<pcl::PointXYZI>::Ptr empty_cluster(
+          new pcl::PointCloud<pcl::PointXYZI>);
+
+      pcl::PointXYZI pt;
+      pt.x = pt.y = pt.z = pt.intensity = 0.0f;
+
+      empty_cluster->points.push_back(pt);
+
+      pcl::PointXYZI centroid;
+      centroid.x = 0.0;
+      centroid.y = 0.0;
+      centroid.z = 0.0;
+      centroid.intensity = 0.0;
+
+      cluster_vec.push_back(std::make_pair(empty_cluster, 0.0f));
+    }
+
+    while (clusterCentroids.size() < 6)
+    {
+      pcl::PointXYZI centroid;
+      centroid.x = 0.0;
+      centroid.y = 0.0;
+      centroid.z = 0.0;
+      centroid.intensity = 0.0;
+
+      clusterCentroids.push_back(centroid);
+    }
+  }
+
+  void CloudClustering::cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr input)
+
+  {
+    // Vector of cluster pointclouds
+    std::vector<std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, float>> cluster_vec;
+    // Cluster centroids
+    std::vector<pcl::PointXYZI> clusterCentroids;
+
+    if (firstFrame)
+    {
+      // If this is the first frame, initialize kalman filters for the clustered objects
+      // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset.
+      // Could be made generic by creating a Kalman Filter only when a new object
+      // is detected
+
+      float dvx = 0.01f; // 1.0
+      float dvy = 0.01f; // 1.0
+      float dx = 1.0f;
+      float dy = 1.0f;
+      KF0.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
+                              dvx, 0, 0, 0, 0, dvy);
+      KF1.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
+                              dvx, 0, 0, 0, 0, dvy);
+      KF2.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
+                              dvx, 0, 0, 0, 0, dvy);
+      KF3.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
+                              dvx, 0, 0, 0, 0, dvy);
+      KF4.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
+                              dvx, 0, 0, 0, 0, dvy);
+      KF5.transitionMatrix = (cv::Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
+                              dvx, 0, 0, 0, 0, dvy);
+
+      cv::setIdentity(KF0.measurementMatrix);
+      cv::setIdentity(KF1.measurementMatrix);
+      cv::setIdentity(KF2.measurementMatrix);
+      cv::setIdentity(KF3.measurementMatrix);
+      cv::setIdentity(KF4.measurementMatrix);
+      cv::setIdentity(KF5.measurementMatrix);
+      // Process Noise Covariance Matrix Q
+      // [ Ex 0  0    0 0    0 ]
+      // [ 0  Ey 0    0 0    0 ]
+      // [ 0  0  Ev_x 0 0    0 ]
+      // [ 0  0  0    1 Ev_y 0 ]
+      //// [ 0  0  0    0 1    Ew ]
+      //// [ 0  0  0    0 0    Eh ]
+      float sigmaP = 0.01;
+      float sigmaQ = 0.1;
+      setIdentity(KF0.processNoiseCov, cv::Scalar::all(sigmaP));
+      setIdentity(KF1.processNoiseCov, cv::Scalar::all(sigmaP));
+      setIdentity(KF2.processNoiseCov, cv::Scalar::all(sigmaP));
+      setIdentity(KF3.processNoiseCov, cv::Scalar::all(sigmaP));
+      setIdentity(KF4.processNoiseCov, cv::Scalar::all(sigmaP));
+      setIdentity(KF5.processNoiseCov, cv::Scalar::all(sigmaP));
+      // Meas noise cov matrix R
+      cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ)); // 1e-1
+      cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(sigmaQ));
+      cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(sigmaQ));
+      cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(sigmaQ));
+      cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(sigmaQ));
+      cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ));
+
+      // Process the point cloud
+      parse_cloud(input, cluster_vec, clusterCentroids);
+
+      // Set initial state
+      KF0.statePre.at<float>(0) = clusterCentroids.at(0).x;
+      KF0.statePre.at<float>(1) = clusterCentroids.at(0).y;
+      KF0.statePre.at<float>(2) = 0; // initial v_x
+      KF0.statePre.at<float>(3) = 0; // initial v_y
+
+      // Set initial state
+      KF1.statePre.at<float>(0) = clusterCentroids.at(1).x;
+      KF1.statePre.at<float>(1) = clusterCentroids.at(1).y;
+      KF1.statePre.at<float>(2) = 0; // initial v_x
+      KF1.statePre.at<float>(3) = 0; // initial v_y
+
+      // Set initial state
+      KF2.statePre.at<float>(0) = clusterCentroids.at(2).x;
+      KF2.statePre.at<float>(1) = clusterCentroids.at(2).y;
+      KF2.statePre.at<float>(2) = 0; // initial v_x
+      KF2.statePre.at<float>(3) = 0; // initial v_y
+
+      // Set initial state
+      KF3.statePre.at<float>(0) = clusterCentroids.at(3).x;
+      KF3.statePre.at<float>(1) = clusterCentroids.at(3).y;
+      KF3.statePre.at<float>(2) = 0; // initial v_x
+      KF3.statePre.at<float>(3) = 0; // initial v_y
+
+      // Set initial state
+      KF4.statePre.at<float>(0) = clusterCentroids.at(4).x;
+      KF4.statePre.at<float>(1) = clusterCentroids.at(4).y;
+      KF4.statePre.at<float>(2) = 0; // initial v_x
+      KF4.statePre.at<float>(3) = 0; // initial v_y
+
+      // Set initial state
+      KF5.statePre.at<float>(0) = clusterCentroids.at(5).x;
+      KF5.statePre.at<float>(1) = clusterCentroids.at(5).y;
+      KF5.statePre.at<float>(2) = 0; // initial v_x
+      KF5.statePre.at<float>(3) = 0; // initial v_y
+
+      firstFrame = false;
+
+      for (int i = 0; i < 6; i++)
+      {
+        geometry_msgs::msg::Point pt;
+        pt.x = clusterCentroids.at(i).x;
+        pt.y = clusterCentroids.at(i).y;
+        prevClusterCenters.push_back(pt);
+      }
+    }
+
+    else
+    {
+      parse_cloud(input, cluster_vec, clusterCentroids);
+
+      std_msgs::msg::Float32MultiArray cc;
+      for (int i = 0; i < 6; i++)
+      {
+        cc.data.push_back(clusterCentroids.at(i).x);
+        cc.data.push_back(clusterCentroids.at(i).y);
+        cc.data.push_back(clusterCentroids.at(i).z);
+      }
+
+      // cc_pos->publish(cc);// Publish cluster mid-points.
+      KFT(cc);
+      int i = 0;
+
+      //std::sort(objID.begin(), objID.end(), [cluster_vec](const int &a, const int &b)
+      //          { return cluster_vec[a].second < cluster_vec[b].second; });
+
+      std::vector<std::pair<int, float>> kf_intensity;
+
+      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));
+      }
+
+      std::sort(kf_intensity.begin(), kf_intensity.end(),
+                [](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)
+        {
+        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:
+        {
+          publish_cloud(pub_cluster5, cluster_vec[cluster_index].first);
+          i++;
+          break;
+        }
+        default:
+          break;
+        }
+      }
+    }
+  }
+}

src/target_tracking/src/cloud_clustering_node.cpp

@@ -0,0 +1,17 @@
+#include "cloud_clustering.hpp"
+
+int main(int argc, char * argv[])
+{
+    // Force flush of the stdout buffer.
+    // This ensures a correct sync of all prints
+    // even when executed simultaneously within a launch file.
+    setvbuf(stdout, NULL, _IONBF, BUFSIZ);
+    
+    rclcpp::init(argc, argv);
+    const rclcpp::NodeOptions options;
+
+    rclcpp::spin(std::make_shared<cloud_clustering::CloudClustering>(options));
+    
+    rclcpp::shutdown();
+    return 0;
+}

src/target_tracking/src/cloud_preprocessing.cpp

@@ -0,0 +1,186 @@
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
+#include <cmath>
+
+#include <random>
+#include <chrono>
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/io/pcd_io.h>
+
+class CloudFilterNode : public rclcpp::Node
+{
+  //Topic parameters
+  std::string topic_in;
+  std::string topic_out;
+
+  // Filter parameters
+  float x_min;
+  float x_max;
+  float z_min;
+  float z_max;
+  float tan_h_fov;
+  float tan_v_fov;
+
+  //Preallocate pointclouds
+  pcl::PointCloud<pcl::PointXYZI>::Ptr input {new pcl::PointCloud<pcl::PointXYZI>};
+  pcl::PointCloud<pcl::PointXYZI>::Ptr fov_filtered {new pcl::PointCloud<pcl::PointXYZI>};
+  pcl::PointCloud<pcl::PointXYZI>::Ptr voxel_filtered {new pcl::PointCloud<pcl::PointXYZI>};
+
+  //Preallocate filter
+  pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;
+
+public:
+  CloudFilterNode()
+  : Node("cloud_preprocessing"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_)
+  {
+
+    //Topic parameters
+    this->declare_parameter<std::string>("topic_in", "test");
+    this->declare_parameter<std::string>("topic_out", "test2");
+
+    //Filter parameters
+    this->declare_parameter<float>("x_min", 0.0f);
+    this->declare_parameter<float>("x_max", 0.0f);
+    this->declare_parameter<float>("z_min", 0.0f);
+    this->declare_parameter<float>("z_max", 0.0f);
+    this->declare_parameter<float>("tan_h_fov", 0.0f);  // ±45°
+    this->declare_parameter<float>("tan_v_fov", 0.0f);  // ±30°
+
+    this->get_parameter("topic_in", topic_in);
+    this->get_parameter("topic_out", topic_out);
+
+    this->get_parameter("x_min", x_min);
+    this->get_parameter("x_max", x_max);
+    this->get_parameter("z_min", z_min);
+    this->get_parameter("z_max", z_max);
+    this->get_parameter("tan_h_fov", tan_h_fov);
+    this->get_parameter("tan_v_fov", tan_v_fov);
+
+    voxel_filter.setLeafSize(0.03f, 0.03f, 0.03f);  // Adjust as needed
+
+    RCLCPP_INFO(this->get_logger(), "Starting filter node with parameters:\n"
+      "topic_in: %s\n"
+      "topic_out: %s\n"
+      "x_min: %f\n"
+      "x_max: %f\n"
+      "z_min: %f\n"
+      "z_max: %f\n"
+      "tan_h_fov: %f\n"
+      "tan_v_fov: %f"
+    , topic_in.c_str(), topic_out.c_str(), x_min, x_max, z_min, z_max, tan_h_fov, tan_v_fov
+  );
+
+    subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+      topic_in, rclcpp::SensorDataQoS(),
+      std::bind(&CloudFilterNode::cloud_callback, this, std::placeholders::_1));
+
+    filtered_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(topic_out, 10);
+  }
+
+private:
+  void cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
+  {
+    //auto start = std::chrono::high_resolution_clock::now();
+    //Clear all pointclouds
+    input->clear();
+    fov_filtered->clear();
+    voxel_filtered->clear();
+
+    // Convert to PCL
+    pcl::fromROSMsg(*msg, *input);
+
+    fov_filtered->reserve(input->size());
+
+    for (const auto& point : input->points)
+    {
+      if (point.x < x_min || point.x > x_max) continue;
+      if (std::isnan(point.x) || std::isnan(point.y) || std::isnan(point.z)) continue;
+      if (point.z < z_min || point.z > z_max) continue;
+
+      float inv_x = 1.0f / point.x;
+      if (fabsf(point.y * inv_x) > tan_h_fov) continue;
+      if (fabsf(point.z * inv_x) > tan_v_fov) continue;
+
+      fov_filtered->push_back(point);
+    }
+
+    // Apply voxel grid filter
+    voxel_filter.setInputCloud(fov_filtered);
+    voxel_filter.filter(*voxel_filtered);
+
+    //Apply ransac
+    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
+    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+    // Create the segmentation object
+    pcl::SACSegmentation<pcl::PointXYZI> seg;
+    // Optional
+    seg.setOptimizeCoefficients(true);
+    // Mandatory
+    seg.setModelType(pcl::SACMODEL_PLANE);
+    seg.setMethodType(pcl::SAC_RANSAC);
+    seg.setDistanceThreshold(0.02);
+    seg.setMaxIterations(200);
+  
+    seg.setInputCloud(voxel_filtered);
+    seg.segment (*inliers, *coefficients);
+  
+    if (inliers->indices.size () == 0)
+    {
+      RCLCPP_INFO(this->get_logger(), "Oh oh. No inliers");
+      return;
+    }
+
+    //Select remaining points
+    pcl::ExtractIndices<pcl::PointXYZI> extract;
+    extract.setInputCloud(voxel_filtered);
+    extract.setIndices(inliers);
+    extract.setNegative(true);
+    extract.filter(*voxel_filtered);
+
+    // 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.frame_id = msg->header.frame_id;
+    filtered_publisher_->publish(out_msg);
+
+    //RCLCPP_INFO(this->get_logger(), "Filtered %zu -> %zu points", input->points.size(), voxel_filtered->points.size());
+    //auto stop = std::chrono::high_resolution_clock::now();
+    //auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
+
+    // To get the value of duration use the count()
+    // member function on the duration object
+    //std::cout << duration.count() << std::endl;
+
+  }
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
+
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr filtered_publisher_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr original_publisher_;
+};
+
+int main(int argc, char * argv[])
+{
+  srand((unsigned int) time (NULL));
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<CloudFilterNode>());
+  rclcpp::shutdown();
+  return 0;
+}