diff --git a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/moveit_stop.py b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/moveit_stop.py index 5d94cbb331a0ea6fcff667b96a3c2a8c15402483..31fd198578a6a087df825f79805eaee21447eeaa 100644 --- a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/moveit_stop.py +++ b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/moveit_stop.py @@ -6,6 +6,8 @@ import numpy as np import trimesh from std_msgs.msg import String import sensor_msgs_py.point_cloud2 as pc2 +from std_msgs.msg import Header +from sensor_msgs.msg import PointField class PointCloudProcessor(Node): def __init__(self): @@ -20,13 +22,20 @@ class PointCloudProcessor(Node): self.cancellation_pub = self.create_publisher( String, "/trajectory_execution_event", 1 ) - + self.out_zone_pub = self.create_publisher( + PointCloud2, + '/out_safe_zone', + 10) + self.in_zone_pub = self.create_publisher( + PointCloud2, + '/in_safe_zone', + 10) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) # Create meshes 5cm larger than the original example self.meshes_static = [ - trimesh.creation.cylinder(radius=0.2, height=1.0), # Cylinder for upper_arm_link + trimesh.creation.cylinder(radius=0.2, height=0.8), # Cylinder for upper_arm_link trimesh.creation.cylinder(radius=0.2, height=1.05) # Cylinder for forearm_link ] @@ -44,7 +53,7 @@ class PointCloudProcessor(Node): mesh.apply_transform(translation_matrix_x) # Move the upper_arm_link cylinder 10cm in the positive x direction - translation_matrix_upper_arm = trimesh.transformations.translation_matrix([0.1, 0, 0]) + translation_matrix_upper_arm = trimesh.transformations.translation_matrix([0, 0, 0]) self.meshes_static[0].apply_transform(translation_matrix_upper_arm) # Move the forearm_link cylinder 5cm in the negative x direction @@ -57,7 +66,7 @@ class PointCloudProcessor(Node): mesh.apply_transform(translation_matrix_z) # Threshold for stopping the trajectory - self.point_threshold = 33 # Adjust as needed + self.point_threshold = 2 # Adjust as needed def pointcloud_callback(self, msg): self.perspective_frame = 'vl53l7cx_link' @@ -80,10 +89,13 @@ class PointCloudProcessor(Node): inside_robot = np.logical_or.reduce([mesh.contains(points) for mesh in transformed_meshes]) points_inside_robot = points[inside_robot] - self.get_logger().info(f'Number of points inside robot: {len(points_inside_robot)}') + self.get_logger().info(f'Number of points to close to robot: {len(points_inside_robot)}') - if len(points_inside_robot) > self.point_threshold: - self.stop_trajectory() + # Publish the point clouds + self.out_zone_pub.publish(self.create_point_cloud(points[~inside_robot], self.perspective_frame)) + self.in_zone_pub.publish(self.create_point_cloud(points_inside_robot, self.perspective_frame)) + #if len(points_inside_robot) > self.point_threshold: + # self.stop_trajectory() def transform_mesh(self, transform, mesh_static): translation = transform.transform.translation @@ -98,10 +110,36 @@ class PointCloudProcessor(Node): return transformed_mesh def stop_trajectory(self): + self.get_logger().info("Stopping trajectory execution due to point cloud threshold.") cancel_string = String() cancel_string.data = "stop" self.cancellation_pub.publish(cancel_string) - + + def create_point_cloud(self, points, parent_frame): + """ + Create a PointCloud2 message from the 3D points. + """ + ros_dtype = PointField.FLOAT32 # ROS 2 data type for floating-point numbers + dtype = np.float32 # NumPy data type for floating-point numbers + itemsize = np.dtype(dtype).itemsize # Size of each data item in bytes + + data = points.astype(dtype).tobytes() # Convert points to binary data + + fields = [PointField(name=n, offset=i * itemsize, datatype=ros_dtype, count=1) + for i, n in enumerate('xyz')] # Define the fields for x, y, and z + + header = Header(frame_id=parent_frame) # Create a header with the parent frame + return PointCloud2( + header=header, + height=1, # Single row of points + width=points.shape[0], # Number of points + is_dense=True, # Allow for invalid points + is_bigendian=False, # Data is in little-endian format + fields=fields, # Fields for x, y, and z + point_step=(itemsize * 3), # Size of each point in bytes + row_step=(itemsize * 3 * points.shape[0]), # Size of each row in bytes + data=data # Binary data for the points + ) def main(args=None): rclpy.init(args=args) pointcloud_processor = PointCloudProcessor() diff --git a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/pcl_rob_v2_node.py b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/pcl_rob_v2_node.py index 0421526458b439093fd14567ed44efb44d8eb74e..ec16085e92765a6a0e53fcbb2fcd8b3ab77f4ed5 100644 --- a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/pcl_rob_v2_node.py +++ b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/pcl_rob_v2_node.py @@ -10,7 +10,6 @@ import sensor_msgs_py.point_cloud2 as pc2 from std_msgs.msg import Header from scipy.spatial.transform import Rotation as R import os -from geometry_msgs.msg import TransformStamped class PointCloudMeshChecker(Node): def __init__(self):