added everything from pc
This commit is contained in:
@@ -13,6 +13,7 @@ import threading
|
||||
import time
|
||||
import open3d as o3d
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
from datetime import datetime
|
||||
|
||||
class PointCloudSaver(Node):
|
||||
def __init__(self, node_name: str, pointcloud_topic: str, buffer, timeout_ms: int):
|
||||
@@ -111,6 +112,11 @@ class LidarTransformPublisher(Node):
|
||||
|
||||
self.br.sendTransform(t)
|
||||
self.get_logger().info("Published static transform.")
|
||||
if input("Is it acurate? [y/N] ").lower() == 'y':
|
||||
file_name = f"calibration/{datetime.now().strftime("%Y_%m_%d-%H_%M_%S")}.npy"
|
||||
np.save(file_name, self.T)
|
||||
self.get_logger().info(f"Saved {file_name}")
|
||||
|
||||
|
||||
def pcd_buffer_to_o3d(self, buffer: BytesIO):
|
||||
buffer.seek(0)
|
||||
@@ -195,7 +201,7 @@ class LidarTransformPublisher(Node):
|
||||
return transformation
|
||||
|
||||
|
||||
def monitor_nodes(nodes, publisher):
|
||||
def monitor_nodes(nodes, executor, publisher):
|
||||
while rclpy.ok():
|
||||
if all(node.finished for node in nodes):
|
||||
print("All pointclouds captured")
|
||||
@@ -210,12 +216,12 @@ def main():
|
||||
|
||||
executor = MultiThreadedExecutor()
|
||||
nodes = [
|
||||
PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', buffer_velodyne, 350),
|
||||
PointCloudSaver('livox_pcd_saver', '/livox/lidar', buffer_livox, 1000),
|
||||
PointCloudSaver('velodyne_pcd_saver', '/lidar/vlp16/points', buffer_velodyne, 3_500),
|
||||
PointCloudSaver('livox_pcd_saver', '/lidar/mid40/points', buffer_livox, 5_000),
|
||||
]
|
||||
publisher = LidarTransformPublisher(buffer_velodyne, 'velodyne', buffer_livox, 'frame_default')
|
||||
|
||||
monitor_thread = threading.Thread(target=monitor_nodes, args=(nodes,publisher), daemon=True)
|
||||
monitor_thread = threading.Thread(target=monitor_nodes, args=(nodes,executor,publisher), daemon=True)
|
||||
monitor_thread.start()
|
||||
|
||||
for node in nodes:
|
||||
|
||||
103
scripts/plot.py
Normal file
103
scripts/plot.py
Normal file
@@ -0,0 +1,103 @@
|
||||
#!/usr/bin/env python3
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Float32
|
||||
import matplotlib.pyplot as plt
|
||||
import matplotlib.animation as animation
|
||||
import threading
|
||||
|
||||
class MultiPlotter(Node):
|
||||
def __init__(self):
|
||||
super().__init__("multi_plotter")
|
||||
|
||||
# Data buffers
|
||||
self.angle = []
|
||||
self.velocity = []
|
||||
self.acceleration = []
|
||||
|
||||
# Subscriptions
|
||||
self.create_subscription(Float32, '/nucleo/stepper0/position',
|
||||
self.angle_callback, 10)
|
||||
self.create_subscription(Float32, '/nucleo/stepper0/velocity',
|
||||
self.velocity_callback, 10)
|
||||
self.create_subscription(Float32, '/nucleo/stepper0/acceleration',
|
||||
self.accel_callback, 10)
|
||||
|
||||
def angle_callback(self, msg):
|
||||
self.angle.append(msg.data)
|
||||
self._trim()
|
||||
|
||||
def velocity_callback(self, msg):
|
||||
self.velocity.append(msg.data)
|
||||
self._trim()
|
||||
|
||||
def accel_callback(self, msg):
|
||||
self.acceleration.append(msg.data)
|
||||
self._trim()
|
||||
|
||||
def _trim(self, limit=2000):
|
||||
"""Keep buffers short so memory stays small."""
|
||||
if len(self.angle) > limit:
|
||||
self.angle.pop(0)
|
||||
if len(self.velocity) > limit:
|
||||
self.velocity.pop(0)
|
||||
if len(self.acceleration) > limit:
|
||||
self.acceleration.pop(0)
|
||||
|
||||
|
||||
def animate(i, node, axes):
|
||||
# Clear each subplot
|
||||
for ax in axes:
|
||||
ax.cla()
|
||||
|
||||
# Angle
|
||||
if len(node.angle) > 0:
|
||||
axes[0].plot(node.angle, color='red')
|
||||
axes[0].set_title("Angle")
|
||||
axes[0].grid(True)
|
||||
|
||||
# Velocity
|
||||
if len(node.velocity) > 0:
|
||||
axes[1].plot(node.velocity, color='green')
|
||||
axes[1].set_title("Velocity")
|
||||
axes[1].grid(True)
|
||||
|
||||
# Acceleration
|
||||
if len(node.acceleration) > 0:
|
||||
axes[2].plot(node.acceleration, color='blue')
|
||||
axes[2].set_title("Acceleration")
|
||||
axes[2].grid(True)
|
||||
|
||||
# Shared axis labels
|
||||
axes[2].set_xlabel("Samples")
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = MultiPlotter()
|
||||
|
||||
# Start ROS spinning in the background
|
||||
thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
|
||||
thread.start()
|
||||
|
||||
# Create three subplots
|
||||
fig, axes = plt.subplots(3, 1, figsize=(8, 8), sharex=True)
|
||||
|
||||
# Persist animation object
|
||||
global ani
|
||||
ani = animation.FuncAnimation(
|
||||
fig,
|
||||
animate,
|
||||
fargs=(node, axes),
|
||||
interval=16
|
||||
)
|
||||
|
||||
plt.tight_layout()
|
||||
plt.show()
|
||||
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,87 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from tf2_ros import StaticTransformBroadcaster
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
import numpy as np
|
||||
import open3d as o3d
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
|
||||
class StaticTransformPublisher(Node):
|
||||
def __init__(self):
|
||||
super().__init__('static_transform_from_pointclouds')
|
||||
|
||||
# Static TF broadcaster
|
||||
self.br = StaticTransformBroadcaster(self)
|
||||
|
||||
# Pointcloud files
|
||||
self.velodyne_file = "/root/velodyne.pcd"
|
||||
self.livox_file = "/root/livox.pcd"
|
||||
|
||||
# Frames
|
||||
self.livox_frame = "frame_default"
|
||||
self.velodyne_frame = "velodyne"
|
||||
|
||||
# Compute transform once
|
||||
self.T = self.compute_transform()
|
||||
self.get_logger().info(f"Computed initial transform:\n{self.T}")
|
||||
|
||||
# Prepare translation and rotation
|
||||
T_copy = np.array(self.T, copy=True)
|
||||
trans = T_copy[:3, 3]
|
||||
rot_quat = R.from_matrix(T_copy[:3, :3]).as_quat() # [x, y, z, w]
|
||||
|
||||
# Create static TransformStamped
|
||||
t = TransformStamped()
|
||||
t.header.stamp.sec = 0
|
||||
t.header.stamp.nanosec = 0
|
||||
t.header.frame_id = self.velodyne_frame
|
||||
t.child_frame_id = self.livox_frame
|
||||
t.transform.translation.x = trans[0]
|
||||
t.transform.translation.y = trans[1]
|
||||
t.transform.translation.z = trans[2]
|
||||
t.transform.rotation.x = rot_quat[0]
|
||||
t.transform.rotation.y = rot_quat[1]
|
||||
t.transform.rotation.z = rot_quat[2]
|
||||
t.transform.rotation.w = rot_quat[3]
|
||||
|
||||
# Publish once
|
||||
self.br.sendTransform(t)
|
||||
self.get_logger().info("Published static transform.")
|
||||
|
||||
def compute_transform(self):
|
||||
# Load point clouds
|
||||
pcd_vel = o3d.io.read_point_cloud(self.velodyne_file)
|
||||
pcd_liv = o3d.io.read_point_cloud(self.livox_file)
|
||||
|
||||
# Downsample
|
||||
voxel_size = 0.05
|
||||
pcd_vel_ds = pcd_vel.voxel_down_sample(voxel_size)
|
||||
pcd_liv_ds = pcd_liv.voxel_down_sample(voxel_size)
|
||||
|
||||
# Estimate normals
|
||||
pcd_vel_ds.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(knn=20))
|
||||
pcd_liv_ds.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(knn=20))
|
||||
|
||||
# ICP registration
|
||||
threshold = 0.5
|
||||
reg_result = o3d.pipelines.registration.registration_icp(
|
||||
pcd_liv_ds, pcd_vel_ds, threshold,
|
||||
np.eye(4),
|
||||
o3d.pipelines.registration.TransformationEstimationPointToPoint()
|
||||
)
|
||||
return reg_result.transformation
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = StaticTransformPublisher()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,113 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
import sensor_msgs_py.point_cloud2 as pc2
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import struct
|
||||
from io import BytesIO
|
||||
import threading
|
||||
import time
|
||||
|
||||
class PointCloudSaver(Node):
|
||||
def __init__(self, node_name: str, pointcloud_topic: str, buffer, timeout_ms: int):
|
||||
super().__init__(node_name)
|
||||
self.subscription = self.create_subscription(
|
||||
PointCloud2,
|
||||
pointcloud_topic,
|
||||
self.callback,
|
||||
10
|
||||
)
|
||||
self.buffer = buffer
|
||||
self.finished = False
|
||||
self.points = []
|
||||
self.end_time = self.get_clock().now().nanoseconds + (timeout_ms * 1_000_000)
|
||||
self.cmap = plt.get_cmap('jet')
|
||||
|
||||
def callback(self, msg):
|
||||
now = self.get_clock().now().nanoseconds
|
||||
for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
|
||||
self.points.append([p[0], p[1], p[2], p[3]])
|
||||
|
||||
if now > self.end_time:
|
||||
if not self.points:
|
||||
self.get_logger().warn("No points received!")
|
||||
self.destroy_node()
|
||||
self.finished = True
|
||||
return
|
||||
|
||||
np_points = np.array(self.points, dtype=np.float32)
|
||||
intensities = np_points[:, 3]
|
||||
norm_int = (intensities - intensities.min()) / (intensities.ptp() + 1e-8)
|
||||
|
||||
# Map normalized intensity to RGB colormap
|
||||
colors = self.cmap(norm_int)[:, :3] # RGB 0-1
|
||||
colors = (colors * 255).astype(np.uint8)
|
||||
rgb_int = np.left_shift(colors[:,0].astype(np.uint32), 16) | \
|
||||
np.left_shift(colors[:,1].astype(np.uint32), 8) | \
|
||||
colors[:,2].astype(np.uint32)
|
||||
|
||||
filename = "pointcloud.pcd"
|
||||
self.write_pcd_with_intensity_rgb(filename, np_points, rgb_int)
|
||||
self.get_logger().info(f"Saved {filename}")
|
||||
self.destroy_node()
|
||||
self.finished = True
|
||||
|
||||
def write_pcd_with_intensity_rgb(self, filename, points, rgb_int):
|
||||
header = f"""# .PCD v0.7 - Point Cloud Data file format
|
||||
VERSION 0.7
|
||||
FIELDS x y z intensity rgb
|
||||
SIZE 4 4 4 4 4
|
||||
TYPE F F F F U
|
||||
COUNT 1 1 1 1 1
|
||||
WIDTH {points.shape[0]}
|
||||
HEIGHT 1
|
||||
VIEWPOINT 0 0 0 1 0 0 0
|
||||
POINTS {points.shape[0]}
|
||||
DATA binary
|
||||
"""
|
||||
self.buffer.write(header.encode('ascii'))
|
||||
for i in range(points.shape[0]):
|
||||
# x, y, z, intensity as float32, rgb as uint32
|
||||
self.buffer.write(struct.pack('ffffI', points[i,0], points[i,1], points[i,2], points[i,3], rgb_int[i]))
|
||||
|
||||
def monitor_nodes(nodes):
|
||||
"""Separate thread that monitors node status and shuts down ROS when done."""
|
||||
while rclpy.ok():
|
||||
if all(node.finished for node in nodes):
|
||||
print("All nodes finished. Shutting down ROS.")
|
||||
rclpy.shutdown()
|
||||
break
|
||||
time.sleep(0.1) # check periodically
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
file_velodyne = open('/root/velodyne.pcd', "wb+")
|
||||
file_livox = open('/root/livox.pcd', "wb+")
|
||||
|
||||
executor = MultiThreadedExecutor()
|
||||
|
||||
nodes = [
|
||||
PointCloudSaver('velodyne_pcd_saver', '/velodyne_points', file_velodyne, 5000),
|
||||
PointCloudSaver('livox_pcd_saver', '/livox/lidar', file_livox, 5000),
|
||||
]
|
||||
|
||||
monitor_thread = threading.Thread(target=monitor_nodes, args=(nodes,), daemon=True)
|
||||
monitor_thread.start()
|
||||
|
||||
for node in nodes:
|
||||
executor.add_node(node)
|
||||
try:
|
||||
executor.spin()
|
||||
finally:
|
||||
monitor_thread.join()
|
||||
print("Executor and monitor thread exited cleanly.")
|
||||
|
||||
file_velodyne.close()
|
||||
file_livox.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
Reference in New Issue
Block a user