checked out right branch

.gitmodules

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

src/sherpa/launch/tracking.launch.py

@@ -64,71 +64,62 @@ def generate_launch_description():
         ],
     )
 
-    # ----- pointcloud downsampling -----
-    downsample_vlp16_node = Node(
-        package='target_tracking',
-        executable='cloud_preprocessing_node',
-        parameters=[
-            {"topic_in": "/lidar/vlp16/points"},
-            {"topic_out": "/lidar/vlp16/downsampled"},
-            {"x_min": 0.0},
-            {"x_max": 100.0},
-            {"z_min": -5.0},
-            {"z_max": 10.0},
-            {"tan_h_fov": math.pi}, # 180°  
-            {"tan_v_fov": math.pi / 5}, # ~36°  
-        ],
-    )
-
-    downsample_mid40_node = Node(
-        package='target_tracking',
-        executable='cloud_preprocessing_node',
-        parameters=[
-            {"topic_in": "/lidar/mid40/points"},
-            {"topic_out": "/lidar/mid40/downsampled"},
-            {"x_min": 0.0},
-            {"x_max": 100.0},
-            {"z_min": -5.0},
-            {"z_max": 10.0},
-            {"tan_h_fov": math.pi / 4}, # 45°  
-            {"tan_v_fov": math.pi / 4}, # 45°  
-        ],
-    )
-
     # ----- pointcloud merging -----
     pointcloud_merger_node = Node(
         package='scans_merger',
         executable='scans_merger',
         parameters=[
             {"destination_frame": "lidar_base"},
-            {"input_cloud_1": "/lidar/vlp16/downsampled"},
+            {"input_cloud_1": "/lidar/vlp16/points"},
-            {"input_cloud_2": "/lidar/mid40/downsampled"},
+            {"input_cloud_2": "/lidar/mid40/points"},
             {"merged_cloud": "/lidar/merged/points"},
         ],
     )
 
+     # ----- pointcloud downsampling -----
+    downsample_node = Node(
+        package='target_tracking',
+        executable='cloud_preprocessing_node',
+        parameters=[
+            {"topic_in": "/lidar/merged/points"},
+            {"topic_out": "/lidar/merged/downsampled"},
+            {"x_min": 0.0},
+            {"x_max": 100.0},
+            {"z_min": -5.0},
+            {"z_max": 10.0},
+            {"tan_h_fov": math.pi}, # 180°  
+            {"tan_v_fov": math.pi / 4}, # ~45°  
+        ],
+    )
+
     # ----- tracking -----
     cloud_clustering_node = Node(
         package='target_tracking',
         executable='cloud_clustering_node',
         parameters=[
-            {"topic_in": "/lidar/merged/points"},
+            {"topic_in": "/lidar/merged/downsampled"},
             {"frame_id": 'lidar_base'},
             {"z_dim_scale": 1.0},
             #{"cluster_tolerance": 0.065},
             #{"min_cluster_size": 450},
             #{"max_cluster_size": 1650},
-            {"min_cluster_size": 125},
+            {"min_cluster_size": 50},
-            {"max_cluster_size": 2250},
+            {"max_cluster_size": 7500},
-            {"cluster_tolerance": 0.15},
+            {"cluster_tolerance": 0.065},
+            {"min_width": 0.3},
+            {"min_height": 1.0},
+            {"min_length": 0.3},
+            {"max_width": 2.0},
+            {"max_height": 2.5},
+            {"max_length": 2.0},
         ],
         remappings=[
-            ('/cluster5', '/tracking/cluster0'),
+            ('/cluster0', '/tracking/cluster0'),
-            ('/cluster4', '/tracking/cluster1'),
+            ('/cluster1', '/tracking/cluster1'),
-            ('/cluster3', '/tracking/cluster2'),
+            ('/cluster2', '/tracking/cluster2'),
-            ('/cluster2', '/tracking/cluster3'),
+            ('/cluster3', '/tracking/cluster3'),
-            ('/cluster1', '/tracking/cluster4'),
+            ('/cluster4', '/tracking/cluster4'),
-            ('/cluster0', '/tracking/cluster5'),
+            ('/cluster5', '/tracking/cluster5'),
         ],
     )
 
@@ -150,9 +141,8 @@ def generate_launch_description():
     ld.add_action(vlp16_driver_node)
     ld.add_action(vlp16_pointcloud_node)
     ld.add_action(mid40_driver_node)
-    ld.add_action(downsample_vlp16_node)
-    ld.add_action(downsample_mid40_node)
     ld.add_action(pointcloud_merger_node)
+    ld.add_action(downsample_node)
     ld.add_action(cloud_clustering_node)
     ld.add_action(servo_tracking_node)
 

src/sherpa_lidar_utils/src/servo_angle_tf.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 import rclpy
 from rclpy.node import Node
+from rclpy.duration import Duration
 from std_msgs.msg import Float32
 from geometry_msgs.msg import TransformStamped
 import tf2_ros
@@ -46,7 +47,7 @@ class ServoTfNode(Node):
 
         t = TransformStamped()
         #t.header.stamp = (self.get_clock().now() - rclpy.duration.Duration(seconds=0.1)).to_msg()
-        t.header.stamp = self.get_clock().now().to_msg()
+        t.header.stamp = (self.get_clock().now() + Duration(seconds=0.05)).to_msg()
         t.header.frame_id = "livox_base"
         t.child_frame_id = "frame_default"
 

src/sherpa_lidar_utils/src/track_cluster.py

@@ -42,6 +42,10 @@ class Cluster5Tracker(Node):
             self.get_logger().warn("No points in cluster5 PointCloud2")
             return
 
+        if len(points) == 1:
+            self.get_logger().warn("Only one point found. Defaulting to search target")
+            centroid = (3, 0, 0)
+        else:
             arr = np.array(points, dtype=np.float32)
             centroid = np.mean(arr, axis=0)
 
@@ -68,14 +72,15 @@ class Cluster5Tracker(Node):
 
         # Compute yaw angle in degrees
         x, y = pt_transformed.point.x, pt_transformed.point.y
-        yaw_deg = -math.degrees(math.atan2(y, x))
+        yaw_deg = math.degrees(math.atan2(y, x))
 
         self.buf.append(yaw_deg)
-        if len(self.buf) > 15:
+        if len(self.buf) > 5:
             self.buf.pop(0)
+        target_angle: float = sum(self.buf) / len(self.buf)
 
         angle_msg = Float32()
-        angle_msg.data = sum(self.buf) / len(self.buf)
+        angle_msg.data = target_angle
         self.angle_pub.publish(angle_msg)
 
         #self.get_logger().info(f"Centroid: ({x:.2f}, {y:.2f}, {pt_transformed.point.z:.2f}), Yaw: {yaw_deg:.2f}°")

src/target_tracking/src/cloud_clustering.cpp

@@ -628,63 +628,40 @@ namespace cloud_clustering
       //          { return cluster_vec[a].second < cluster_vec[b].second; });
 
       std::vector<std::pair<int, float>> kf_intensity;
+kf_intensity.reserve(objID.size());
 
-      for (int i = 0; i < objID.size(); i++) {
+for (int kf_idx = 0; kf_idx < objID.size(); kf_idx++)
-          int cluster_index = objID[i];
+{
-          float mean_intensity = cluster_vec[cluster_index].second;
+    int cluster_idx = objID[kf_idx];
-          kf_intensity.push_back(std::make_pair(i, mean_intensity));
+    float intensity = cluster_vec[cluster_idx].second; // already normalized
-      }
+    kf_intensity.emplace_back(kf_idx, intensity);
+}
 
-      std::sort(kf_intensity.begin(), kf_intensity.end(),
+std::sort(kf_intensity.begin(), kf_intensity.end(),
-                [](const auto &a, const auto &b) {
+          [](const auto &a, const auto &b)
+          {
               return a.second > b.second;
           });
 
-      for (auto &[kf_idx, intensity] : kf_intensity) {
+      int pub_idx = 0;
-        int cluster_index = objID[kf_idx];
+
-        auto &cluster = cluster_vec[cluster_index].first;
+      for (const auto &[kf_idx, intensity] : kf_intensity)
-        switch (i)
       {
-        case 0:
+          int cluster_idx = objID[kf_idx];
+          auto &cluster = cluster_vec[cluster_idx].first;
+
+          switch (pub_idx)
           {
-          publish_cloud(pub_cluster0, cluster_vec[cluster_index].first);
+          case 0: publish_cloud(pub_cluster0, cluster); break;
-          i++;
+          case 1: publish_cloud(pub_cluster1, cluster); break;
-          break;
+          case 2: publish_cloud(pub_cluster2, cluster); break;
-        }
+          case 3: publish_cloud(pub_cluster3, cluster); break;
-        case 1:
+          case 4: publish_cloud(pub_cluster4, cluster); break;
-        {
+          case 5: publish_cloud(pub_cluster5, cluster); break;
-          publish_cloud(pub_cluster1, cluster_vec[cluster_index].first);
+          default: break;
-          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:
+          pub_idx++;
-        {
-          publish_cloud(pub_cluster5, cluster_vec[cluster_index].first);
-          i++;
-          break;
-        }
-        default:
-          break;
-        }
       }
     }
   }

tracking.rviz

@@ -6,11 +6,9 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
-        - /PointCloud29/Topic1
+        - /Merged1/Topic1
-        - /PointStamped1
-        - /PointStamped1/Topic1
       Splitter Ratio: 0.5
-    Tree Height: 720
+    Tree Height: 789
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -28,7 +26,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: Cluster0
 Visualization Manager:
   Class: ""
   Displays:
@@ -79,7 +77,7 @@ Visualization Manager:
       Max Intensity: 100
       Min Color: 0; 0; 0
       Min Intensity: 1
-      Name: PointCloud2
+      Name: Cluster0
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
@@ -112,7 +110,7 @@ Visualization Manager:
       Max Intensity: 0
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: PointCloud2
+      Name: Cluster1
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
@@ -145,7 +143,7 @@ Visualization Manager:
       Max Intensity: 0
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: PointCloud2
+      Name: Cluster2
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
@@ -178,7 +176,7 @@ Visualization Manager:
       Max Intensity: 0
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: PointCloud2
+      Name: Cluster3
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
@@ -211,7 +209,7 @@ Visualization Manager:
       Max Intensity: 0
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: PointCloud2
+      Name: Cluster4
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
@@ -235,8 +233,8 @@ Visualization Manager:
       Axis: Z
       Channel Name: intensity
       Class: rviz_default_plugins/PointCloud2
-      Color: 85; 0; 127
+      Color: 255; 255; 255
-      Color Transformer: FlatColor
+      Color Transformer: Intensity
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
@@ -244,7 +242,7 @@ Visualization Manager:
       Max Intensity: 0
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: PointCloud2
+      Name: Cluster5
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
@@ -274,81 +272,15 @@ Visualization Manager:
       Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 166
+      Max Intensity: 255
       Min Color: 0; 0; 0
       Min Intensity: 0
-      Name: PointCloud2
+      Name: Merged
       Position Transformer: XYZ
       Selectable: true
       Size (Pixels): 3
       Size (m): 0.009999999776482582
-      Style: Flat Squares
+      Style: Points
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /lidar/vlp16/downsampled
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 154
-      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: /lidar/mid40/downsampled
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: false
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 154
-      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
@@ -356,14 +288,47 @@ Visualization Manager:
         Reliability Policy: Best Effort
         Value: /lidar/merged/points
       Use Fixed Frame: true
-      Use rainbow: true
+      Use rainbow: false
       Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 255
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: Downsampled
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /lidar/merged/downsampled
+      Use Fixed Frame: true
+      Use rainbow: false
+      Value: true
     - Alpha: 1
       Class: rviz_default_plugins/PointStamped
       Color: 255; 255; 255
       Enabled: true
       History Length: 1
-      Name: PointStamped
+      Name: Target
       Radius: 0.10000000149011612
       Topic:
         Depth: 5
@@ -419,33 +384,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 20.069171905517578
+      Distance: 7.419111251831055
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 2.547194004058838
+        X: 3.313748836517334
-        Y: 0.8107800483703613
+        Y: -0.11149537563323975
-        Z: -0.0785689651966095
+        Z: -0.589261531829834
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.8897970914840698
+      Pitch: 0.635202944278717
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz_default_plugins)
-      Yaw: 3.055371046066284
+      Yaw: 3.1103579998016357
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1011
+  Height: 1080
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000015600000359fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000359000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003b40000003efc0100000002fb0000000800540069006d00650100000000000003b40000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000002580000035900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000039e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000006240000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -454,6 +419,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 948
+  Width: 1920
-  X: 1927
+  X: 1920
-  Y: 62
+  Y: 0