From ed031c9b8ddf7ecad8f0117f635d42d24c4da92c Mon Sep 17 00:00:00 2001 From: SHERPA Date: Tue, 6 Jan 2026 16:38:26 +0100 Subject: [PATCH] added local conf --- scripts/calibrate_lidar.py | 2 +- src/FAST_LIO | 1 - src/Livox-SDK2 | 1 - src/livox_ros_driver2 | 1 - src/sherpa_lidar_utils/CMakeLists.txt | 15 +++ src/sherpa_lidar_utils/package.xml | 21 +++ .../src/calibration/2025_11_30-16_55_16.npy | Bin 0 -> 256 bytes .../src/publish_calibration.py | 126 ++++++++++++++++++ src/sherpa_lidar_utils/src/servo_angle_tf.py | 84 ++++++++++++ src/sherpa_lidar_utils/src/track_cluster.py | 95 +++++++++++++ .../launch/target_tracking_launch.py | 16 ++- src/target_tracking/src/cloud_clustering.cpp | 2 +- .../src/cloud_preprocessing.cpp | 7 +- 13 files changed, 362 insertions(+), 9 deletions(-) delete mode 160000 src/FAST_LIO delete mode 160000 src/Livox-SDK2 delete mode 160000 src/livox_ros_driver2 create mode 100644 src/sherpa_lidar_utils/CMakeLists.txt create mode 100644 src/sherpa_lidar_utils/package.xml create mode 100644 src/sherpa_lidar_utils/src/calibration/2025_11_30-16_55_16.npy create mode 100644 src/sherpa_lidar_utils/src/publish_calibration.py create mode 100644 src/sherpa_lidar_utils/src/servo_angle_tf.py create mode 100644 src/sherpa_lidar_utils/src/track_cluster.py diff --git a/scripts/calibrate_lidar.py b/scripts/calibrate_lidar.py index 674e94c..95538d0 100644 --- a/scripts/calibrate_lidar.py +++ b/scripts/calibrate_lidar.py @@ -83,7 +83,7 @@ class LidarTransformPublisher(Node): self.lidar1_buffer = lidar1_buffer self.lidar2_buffer = lidar2_buffer self.lidar1_frame = lidar1_frame - self.lidar2_frame = lidar2_frame + self.lidar2_frame = "livox_base" # lidar2_frame def publish(self): self.pcd_1 = self.pcd_buffer_to_o3d(self.lidar1_buffer) diff --git a/src/FAST_LIO b/src/FAST_LIO deleted file mode 160000 index f203153..0000000 --- a/src/FAST_LIO +++ /dev/null @@ -1 +0,0 @@ -Subproject commit f20315319d194d003e3b6d4f406958f38156e812 diff --git a/src/Livox-SDK2 b/src/Livox-SDK2 deleted file mode 160000 index 9791bc3..0000000 --- a/src/Livox-SDK2 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 9791bc3d8744377dc02be7e6f40587af14add90d diff --git a/src/livox_ros_driver2 b/src/livox_ros_driver2 deleted file mode 160000 index 3f45473..0000000 --- a/src/livox_ros_driver2 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 3f454733309f04f34a3d706f03812224596deae4 diff --git a/src/sherpa_lidar_utils/CMakeLists.txt b/src/sherpa_lidar_utils/CMakeLists.txt new file mode 100644 index 0000000..03ae308 --- /dev/null +++ b/src/sherpa_lidar_utils/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.8) +project(sherpa_lidar_utils) + +find_package(ament_cmake REQUIRED) +find_package(rclpy REQUIRED) + +# Install the Python script into the lib directory of the package +install(PROGRAMS + src/publish_calibration.py + src/servo_angle_tf.py + src/track_cluster.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/src/sherpa_lidar_utils/package.xml b/src/sherpa_lidar_utils/package.xml new file mode 100644 index 0000000..873d661 --- /dev/null +++ b/src/sherpa_lidar_utils/package.xml @@ -0,0 +1,21 @@ + + + + sherpa_lidar_utils + 0.0.1 + Studienprojekt + timo + Apache 2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + rclpy + + + ament_cmake + + diff --git a/src/sherpa_lidar_utils/src/calibration/2025_11_30-16_55_16.npy b/src/sherpa_lidar_utils/src/calibration/2025_11_30-16_55_16.npy new file mode 100644 index 0000000000000000000000000000000000000000..301a0294f70c7808268849618ee334b0553f4f0b GIT binary patch literal 256 zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlV+i=qoAIaUsO_*m=~X4l#&V(4=E~51qv5u zBo?Fsxf&)q3MQI53bhL411>#htJC%O-rC=9ellC;+coz>% literal 0 HcmV?d00001 diff --git a/src/sherpa_lidar_utils/src/publish_calibration.py b/src/sherpa_lidar_utils/src/publish_calibration.py new file mode 100644 index 0000000..74bb443 --- /dev/null +++ b/src/sherpa_lidar_utils/src/publish_calibration.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 +""" +average_tf_publisher.py +Compute the mean of all 4x4 transform .npy files in a folder +and broadcast it as a static TF at 50 Hz. + +Usage: + ros2 run average_tf_publisher.py [directory] [parent_frame] [child_frame] + +Default parameters: + directory = "calibration" + parent_frame = "world" + child_frame = "average_lidar" +""" + +import os +import sys +import glob +import numpy as np +from scipy.spatial.transform import Rotation as R + +import rclpy +from rclpy.node import Node +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster +from geometry_msgs.msg import TransformStamped + + +class AverageTFPublisher(Node): + def __init__(self): + super().__init__("average_tf_publisher") + self.declare_parameter('calibration', 'calibration') + self.declare_parameter('parent_frame', 'velodyne') + self.declare_parameter('child_frame', 'livox_base') + self.declare_parameter('hz', 50.0) + + # Read the values + directory = self.get_parameter('calibration').value + self.parent_frame = self.get_parameter('parent_frame').value + self.child_frame = self.get_parameter('child_frame').value + + self.get_logger().info(f"Loading transforms from: {directory}") + transforms = self._load_transforms(directory) + if len(transforms) == 0: + raise RuntimeError(f"No valid .npy transforms found in {directory}") + + self.get_logger().info(f"Loaded {len(transforms)} transforms.") + self.mean_t, self.mean_q = self._compute_mean_transform(transforms) + self.get_logger().info( + f"Average translation: {self.mean_t}\n" + f"Average quaternion: {self.mean_q}" + ) + + self.br = StaticTransformBroadcaster(self) + + # Publish at 50 Hz (static TF can be published once, but continuous + # publishing helps late-joining nodes) + period = 1.0 / self.get_parameter('hz').value + self.timer = self.create_timer(period, self._timer_cb) + + def _load_transforms(self, directory): + """Load all 4x4 numpy transforms from directory.""" + files = sorted(glob.glob(os.path.join(directory, "*.npy"))) + mats = [] + for f in files: + try: + T = np.load(f) + if T.shape == (4, 4): + mats.append(T) + else: + self.get_logger().warn(f"Skipping {f}: wrong shape {T.shape}") + except Exception as e: + self.get_logger().warn(f"Failed loading {f}: {e}") + return mats + + def _compute_mean_transform(self, mats): + """Return (mean_translation, mean_quaternion)""" + translations = [] + quats = [] + for T in mats: + translations.append(T[:3, 3]) + q = R.from_matrix(T[:3, :3]).as_quat() # [x,y,z,w] + quats.append(q) + + # mean translation + t_mean = np.mean(np.vstack(translations), axis=0) + + # quaternion average using eigenvector method + Q = np.vstack(quats) + A = Q.T @ Q + eigvals, eigvecs = np.linalg.eigh(A) + q_mean = eigvecs[:, np.argmax(eigvals)] + if q_mean[3] < 0: # ensure w positive for consistency + q_mean = -q_mean + return t_mean, q_mean # q_mean = [x,y,z,w] + + def _timer_cb(self): + t = TransformStamped() + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = self.parent_frame + t.child_frame_id = self.child_frame + + t.transform.translation.x = float(self.mean_t[0]) + t.transform.translation.y = float(self.mean_t[1]) + t.transform.translation.z = float(self.mean_t[2]) + t.transform.rotation.x = float(self.mean_q[0]) + t.transform.rotation.y = float(self.mean_q[1]) + t.transform.rotation.z = float(self.mean_q[2]) + t.transform.rotation.w = float(self.mean_q[3]) + + self.br.sendTransform(t) + + +def main(argv=None): + rclpy.init(args=argv) + + try: + node = AverageTFPublisher() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/sherpa_lidar_utils/src/servo_angle_tf.py b/src/sherpa_lidar_utils/src/servo_angle_tf.py new file mode 100644 index 0000000..63bb10e --- /dev/null +++ b/src/sherpa_lidar_utils/src/servo_angle_tf.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32 +from geometry_msgs.msg import TransformStamped +import tf2_ros +import math + +def euler_to_quaternion(roll, pitch, yaw): + """Convert Euler angles to quaternion (x, y, z, w).""" + cr = math.cos(roll / 2) + sr = math.sin(roll / 2) + cp = math.cos(pitch / 2) + sp = math.sin(pitch / 2) + cy = math.cos(yaw / 2) + sy = math.sin(yaw / 2) + + w = cr * cp * cy + sr * sp * sy + x = sr * cp * cy - cr * sp * sy + y = cr * sp * cy + sr * cp * sy + z = cr * cp * sy - sr * sp * cy + return x, y, z, w + + +class ServoTfNode(Node): + """Publishes TF for a servo based on its current position.""" + + def __init__(self): + super().__init__("stepper_tf_node") + + # Subscribe to current servo position + self.pos_sub = self.create_subscription( + Float32, + "/nucleo/stepper0/position", + self.pos_callback, + 10 + ) + + # TF broadcaster + self.br = tf2_ros.TransformBroadcaster(self) + self.get_logger().info("Servo TF node started, subscribing to /nucleo/stepper0/position") + + def pos_callback(self, msg: Float32): + """Publish a TF corresponding to the servo position.""" + angle_deg = msg.data # get float from subscriber + + 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.frame_id = "livox_base" + t.child_frame_id = "frame_default" + + t.transform.translation.x = 0.0 + t.transform.translation.y = 0.0 + t.transform.translation.z = 0.0 + + # Convert angle to yaw (radians) and then to quaternion + yaw = angle_deg * math.pi / 180.0 + qx, qy, qz, qw = euler_to_quaternion(0, 0, yaw) + + t.transform.rotation.x = qx + t.transform.rotation.y = qy + t.transform.rotation.z = qz + t.transform.rotation.w = qw + + # Publish the transform + self.br.sendTransform(t) + + +def main(args=None): + rclpy.init(args=args) + node = ServoTfNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() + diff --git a/src/sherpa_lidar_utils/src/track_cluster.py b/src/sherpa_lidar_utils/src/track_cluster.py new file mode 100644 index 0000000..7fb3122 --- /dev/null +++ b/src/sherpa_lidar_utils/src/track_cluster.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2 +from geometry_msgs.msg import PointStamped +from std_msgs.msg import Float32 +from tf2_ros import Buffer, TransformListener +import tf2_geometry_msgs +import numpy as np +import math +import sensor_msgs_py.point_cloud2 as pc2 + +class Cluster5Tracker(Node): + def __init__(self): + super().__init__('cluster_tracker') + + self.source_frame = 'lidar_base' + self.target_frame = 'livox_base' + + self.buf = [] + + # Subscribe to point cloud of cluster 5 + self.sub = self.create_subscription( + PointCloud2, + '/tracking/target', + self.cb, + 10 + ) + + # Publishers + self.centroid_pub = self.create_publisher(PointStamped, '/tracking/target_pos', 10) + self.angle_pub = self.create_publisher(Float32, '/nucleo/stepper0/target', 10) + + # TF + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + def cb(self, msg: PointCloud2): + # Extract raw x, y, z as a regular list + points = [[p[0], p[1], p[2]] for p in pc2.read_points(msg, field_names=("x","y","z"), skip_nans=True)] + if len(points) == 0: + self.get_logger().warn("No points in cluster5 PointCloud2") + return + + arr = np.array(points, dtype=np.float32) + centroid = np.mean(arr, axis=0) + + pt = PointStamped() + pt.header.frame_id = msg.header.frame_id + pt.header.stamp = msg.header.stamp + pt.point.x, pt.point.y, pt.point.z = centroid + + try: + # Transform to livox_base + t = self.tf_buffer.lookup_transform( + self.target_frame, + pt.header.frame_id, + pt.header.stamp, + timeout=rclpy.duration.Duration(seconds=1.0) + ) + pt_transformed = tf2_geometry_msgs.do_transform_point(pt, t) + except Exception as e: + self.get_logger().warn(f"TF lookup failed: {e}") + return + + # Publish centroid + self.centroid_pub.publish(pt_transformed) + + # Compute yaw angle in degrees + x, y = pt_transformed.point.x, pt_transformed.point.y + yaw_deg = -math.degrees(math.atan2(y, x)) + + self.buf.append(yaw_deg) + if len(self.buf) > 15: + self.buf.pop(0) + + angle_msg = Float32() + angle_msg.data = sum(self.buf) / len(self.buf) + 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}°") + +def main(): + rclpy.init() + node = Cluster5Tracker() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/src/target_tracking/launch/target_tracking_launch.py b/src/target_tracking/launch/target_tracking_launch.py index af6da84..c2414c3 100644 --- a/src/target_tracking/launch/target_tracking_launch.py +++ b/src/target_tracking/launch/target_tracking_launch.py @@ -5,8 +5,13 @@ import math ################### user configure parameters for ros2 start ################### # Topics/Frames frame_id = 'velodyne' +<<<<<<< Updated upstream topic_preprocessing_in = 'filtered_points' topic_preprocessing_out = 'new_filtered' +======= +topic_preprocessing_in = 'merged_cloud' +topic_preprocessing_out = 'filtered_points' +>>>>>>> Stashed changes # Preprocessing x_min = 0.0 @@ -17,6 +22,7 @@ tan_h_fov = math.pi / 4 # ±45° tan_v_fov = math.pi / 6 # ±30° # Clustering +<<<<<<< Updated upstream z_dim_scale = 0.1 cluster_tolerance = 0.3 min_cluster_size = 10 @@ -27,6 +33,12 @@ min_length = 0.0 max_width = 1.5 max_height = 2.5 max_length = 1.5 +======= +z_dim_scale = 1.0 +cluster_tolerance = 0.1 +min_cluster_size = 350 +max_cluster_size = 1500 +>>>>>>> Stashed changes ################### user configure parameters for ros2 end ##################### @@ -68,7 +80,7 @@ def generate_launch_description(): 'stderr': 'screen', } ), - Node( + Node( package='target_tracking', executable='cloud_clustering_node', name='cloud_clustering', @@ -78,4 +90,4 @@ def generate_launch_description(): 'stderr': 'screen', } ) - ]) \ No newline at end of file + ]) diff --git a/src/target_tracking/src/cloud_clustering.cpp b/src/target_tracking/src/cloud_clustering.cpp index 1b85478..f1cd41d 100644 --- a/src/target_tracking/src/cloud_clustering.cpp +++ b/src/target_tracking/src/cloud_clustering.cpp @@ -100,7 +100,7 @@ namespace cloud_clustering // Create a ROS subscriber for the input point cloud sub = this->create_subscription( topic_in, - 10, // queue size + rclcpp::SensorDataQoS(), // queue size std::bind(&CloudClustering::cloud_cb, this, std::placeholders::_1)); std::cout << "Started clustering node with parameters:\n" diff --git a/src/target_tracking/src/cloud_preprocessing.cpp b/src/target_tracking/src/cloud_preprocessing.cpp index 29caea6..848b93c 100644 --- a/src/target_tracking/src/cloud_preprocessing.cpp +++ b/src/target_tracking/src/cloud_preprocessing.cpp @@ -153,11 +153,12 @@ private: // 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.stamp = msg->header.stamp; 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()); +<<<<<<< Updated upstream //auto stop = std::chrono::high_resolution_clock::now(); //auto duration = std::chrono::duration_cast(stop - start); @@ -165,6 +166,8 @@ private: // member function on the duration object //std::cout << duration.count() << std::endl; +======= +>>>>>>> Stashed changes } tf2_ros::Buffer tf_buffer_; @@ -183,4 +186,4 @@ int main(int argc, char * argv[]) rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; -} \ No newline at end of file +}