Merge branch 'master' of https://git.anxietyprime.de/SHERPA/sherpa_ros2_ws

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;
-        }
       }
     }
   }