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 0000000..301a029
Binary files /dev/null and b/src/sherpa_lidar_utils/src/calibration/2025_11_30-16_55_16.npy differ
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
+}