SHERPA/sherpa_ros2_ws
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

definitly not last minute changes

Browse Source
This commit is contained in:
Timo Schneider
2026-01-11 15:49:41 +01:00
parent 3e6952cb59
commit 4217434b43
4 changed files with 77 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
src/sherpa/launch/tracking.launch.py
View File

@@ -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)

3
src/sherpa_lidar_utils/src/servo_angle_tf.py
View File

@@ -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"

15
src/sherpa_lidar_utils/src/track_cluster.py
View File

@@ -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}°")

83
src/target_tracking/src/cloud_clustering.cpp
View File

@@ -628,63 +628,40 @@ namespace cloud_clustering
// { return cluster_vec[a].second < cluster_vec[b].second; });
std::vector<std::pair<int, float>> kf_intensity;
kf_intensity.reserve(objID.size());
for (int i = 0; i < objID.size(); i++) {
int cluster_index = objID[i];
float mean_intensity = cluster_vec[cluster_index].second;
kf_intensity.push_back(std::make_pair(i, mean_intensity));
}
for (int kf_idx = 0; kf_idx < objID.size(); kf_idx++)
{
int cluster_idx = objID[kf_idx];
float intensity = cluster_vec[cluster_idx].second; // already normalized
kf_intensity.emplace_back(kf_idx, intensity);
}
std::sort(kf_intensity.begin(), kf_intensity.end(),
[](const auto &a, const auto &b) {
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++;
}
}
}