From 4217434b431380a8cc2585d2438bab7af0f9b178 Mon Sep 17 00:00:00 2001 From: Timo Date: Sun, 11 Jan 2026 15:49:41 +0100 Subject: [PATCH] definitly not last minute changes --- src/sherpa/launch/tracking.launch.py | 80 +++++++++---------- src/sherpa_lidar_utils/src/servo_angle_tf.py | 3 +- src/sherpa_lidar_utils/src/track_cluster.py | 15 ++-- src/target_tracking/src/cloud_clustering.cpp | 83 +++++++------------- 4 files changed, 77 insertions(+), 104 deletions(-) diff --git a/src/sherpa/launch/tracking.launch.py b/src/sherpa/launch/tracking.launch.py index 863182b..59d8378 100644 --- a/src/sherpa/launch/tracking.launch.py +++ b/src/sherpa/launch/tracking.launch.py @@ -64,71 +64,62 @@ def generate_launch_description(): ], ) - # ----- pointcloud downsampling ----- - downsample_vlp16_node = Node( - package='target_tracking', - executable='cloud_preprocessing_node', - parameters=[ - {"topic_in": "/lidar/vlp16/points"}, - {"topic_out": "/lidar/vlp16/downsampled"}, - {"x_min": 0.0}, - {"x_max": 100.0}, - {"z_min": -5.0}, - {"z_max": 10.0}, - {"tan_h_fov": math.pi}, # 180° - {"tan_v_fov": math.pi / 5}, # ~36° - ], - ) - - downsample_mid40_node = Node( - package='target_tracking', - executable='cloud_preprocessing_node', - parameters=[ - {"topic_in": "/lidar/mid40/points"}, - {"topic_out": "/lidar/mid40/downsampled"}, - {"x_min": 0.0}, - {"x_max": 100.0}, - {"z_min": -5.0}, - {"z_max": 10.0}, - {"tan_h_fov": math.pi / 4}, # 45° - {"tan_v_fov": math.pi / 4}, # 45° - ], - ) - # ----- pointcloud merging ----- pointcloud_merger_node = Node( package='scans_merger', executable='scans_merger', parameters=[ {"destination_frame": "lidar_base"}, - {"input_cloud_1": "/lidar/vlp16/downsampled"}, - {"input_cloud_2": "/lidar/mid40/downsampled"}, + {"input_cloud_1": "/lidar/vlp16/points"}, + {"input_cloud_2": "/lidar/mid40/points"}, {"merged_cloud": "/lidar/merged/points"}, ], ) + # ----- pointcloud downsampling ----- + downsample_node = Node( + package='target_tracking', + executable='cloud_preprocessing_node', + parameters=[ + {"topic_in": "/lidar/merged/points"}, + {"topic_out": "/lidar/merged/downsampled"}, + {"x_min": 0.0}, + {"x_max": 100.0}, + {"z_min": -5.0}, + {"z_max": 10.0}, + {"tan_h_fov": math.pi}, # 180° + {"tan_v_fov": math.pi / 4}, # ~45° + ], + ) + # ----- tracking ----- cloud_clustering_node = Node( package='target_tracking', executable='cloud_clustering_node', parameters=[ - {"topic_in": "/lidar/merged/points"}, + {"topic_in": "/lidar/merged/downsampled"}, {"frame_id": 'lidar_base'}, {"z_dim_scale": 1.0}, #{"cluster_tolerance": 0.065}, #{"min_cluster_size": 450}, #{"max_cluster_size": 1650}, - {"min_cluster_size": 125}, - {"max_cluster_size": 2250}, - {"cluster_tolerance": 0.15}, + {"min_cluster_size": 50}, + {"max_cluster_size": 7500}, + {"cluster_tolerance": 0.065}, + {"min_width": 0.3}, + {"min_height": 1.0}, + {"min_length": 0.3}, + {"max_width": 2.0}, + {"max_height": 2.5}, + {"max_length": 2.0}, ], remappings=[ - ('/cluster5', '/tracking/cluster0'), - ('/cluster4', '/tracking/cluster1'), - ('/cluster3', '/tracking/cluster2'), - ('/cluster2', '/tracking/cluster3'), - ('/cluster1', '/tracking/cluster4'), - ('/cluster0', '/tracking/cluster5'), + ('/cluster0', '/tracking/cluster0'), + ('/cluster1', '/tracking/cluster1'), + ('/cluster2', '/tracking/cluster2'), + ('/cluster3', '/tracking/cluster3'), + ('/cluster4', '/tracking/cluster4'), + ('/cluster5', '/tracking/cluster5'), ], ) @@ -150,9 +141,8 @@ def generate_launch_description(): ld.add_action(vlp16_driver_node) ld.add_action(vlp16_pointcloud_node) ld.add_action(mid40_driver_node) - ld.add_action(downsample_vlp16_node) - ld.add_action(downsample_mid40_node) ld.add_action(pointcloud_merger_node) + ld.add_action(downsample_node) ld.add_action(cloud_clustering_node) ld.add_action(servo_tracking_node) diff --git a/src/sherpa_lidar_utils/src/servo_angle_tf.py b/src/sherpa_lidar_utils/src/servo_angle_tf.py index 63bb10e..ac8a106 100644 --- a/src/sherpa_lidar_utils/src/servo_angle_tf.py +++ b/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" diff --git a/src/sherpa_lidar_utils/src/track_cluster.py b/src/sherpa_lidar_utils/src/track_cluster.py index 7fb3122..c0e1b8d 100644 --- a/src/sherpa_lidar_utils/src/track_cluster.py +++ b/src/sherpa_lidar_utils/src/track_cluster.py @@ -42,8 +42,12 @@ class Cluster5Tracker(Node): self.get_logger().warn("No points in cluster5 PointCloud2") return - arr = np.array(points, dtype=np.float32) - centroid = np.mean(arr, axis=0) + if len(points) == 1: + self.get_logger().warn("Only one point found. Defaulting to search target") + centroid = (3, 0, 0) + else: + arr = np.array(points, dtype=np.float32) + centroid = np.mean(arr, axis=0) pt = PointStamped() pt.header.frame_id = msg.header.frame_id @@ -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}°") diff --git a/src/target_tracking/src/cloud_clustering.cpp b/src/target_tracking/src/cloud_clustering.cpp index f1cd41d..5e596ca 100644 --- a/src/target_tracking/src/cloud_clustering.cpp +++ b/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> kf_intensity; +kf_intensity.reserve(objID.size()); - for (int i = 0; i < objID.size(); i++) { - int cluster_index = objID[i]; - float mean_intensity = cluster_vec[cluster_index].second; - kf_intensity.push_back(std::make_pair(i, mean_intensity)); - } +for (int kf_idx = 0; kf_idx < objID.size(); kf_idx++) +{ + int cluster_idx = objID[kf_idx]; + float intensity = cluster_vec[cluster_idx].second; // already normalized + kf_intensity.emplace_back(kf_idx, intensity); +} - std::sort(kf_intensity.begin(), kf_intensity.end(), - [](const auto &a, const auto &b) { - return a.second > b.second; - }); +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; - } + int pub_idx = 0; - case 5: - { - publish_cloud(pub_cluster5, cluster_vec[cluster_index].first); - i++; - break; - } - default: - break; - } + for (const auto &[kf_idx, intensity] : kf_intensity) + { + int cluster_idx = objID[kf_idx]; + auto &cluster = cluster_vec[cluster_idx].first; + + switch (pub_idx) + { + case 0: publish_cloud(pub_cluster0, cluster); break; + case 1: publish_cloud(pub_cluster1, cluster); break; + case 2: publish_cloud(pub_cluster2, cluster); break; + case 3: publish_cloud(pub_cluster3, cluster); break; + case 4: publish_cloud(pub_cluster4, cluster); break; + case 5: publish_cloud(pub_cluster5, cluster); break; + default: break; + } + + pub_idx++; } } }