Update mesh dimensions and publisher topic for point cloud processing

workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/moveit_stop.py

@@ -35,8 +35,8 @@ class PointCloudProcessor(Node):
 
         # Create meshes 5cm larger than the original example
         self.meshes_static = [
-            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
+            trimesh.creation.cylinder(radius=0.3, height=0.8),  # Cylinder for upper_arm_link
+            trimesh.creation.cylinder(radius=0.3, height=1.05)  # Cylinder for forearm_link
         ]
 
         # Rotate cylinders 90 degrees around the y-axis
@@ -53,11 +53,11 @@ 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, 0, 0])
+        translation_matrix_upper_arm = trimesh.transformations.translation_matrix([-0.15, 0, 0])
         self.meshes_static[0].apply_transform(translation_matrix_upper_arm)
 
         # Move the forearm_link cylinder 5cm in the negative x direction
-        translation_matrix_forearm = trimesh.transformations.translation_matrix([-0.05, 0, 0])
+        translation_matrix_forearm = trimesh.transformations.translation_matrix([0.05, 0, 0])
         self.meshes_static[1].apply_transform(translation_matrix_forearm)
 
         # Move cylinders 10cm in the positive z direction
@@ -94,8 +94,8 @@ class PointCloudProcessor(Node):
         # 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()
+        if len(points_inside_robot) > self.point_threshold:
+            self.stop_trajectory()
 
     def transform_mesh(self, transform, mesh_static):
         translation = transform.transform.translation

workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/serial_to_pcl_node.py

@@ -33,7 +33,7 @@ class SerialListPublisher(Node):
         self.parent_frame = self.get_parameter('parent_frame').get_parameter_value().string_value  # Get the parameter value
 
         # Create a publisher for PointCloud2 messages
-        self.pcd_publisher = self.create_publisher(PointCloud2, 'pcd', 10)
+        self.pcd_publisher = self.create_publisher(PointCloud2, 'pcl', 10)
 
         # Open the serial port for communication
         self.serial_port = serial.Serial(serial_port, baudrate, timeout=None)

workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/test_transform.py

@@ -24,8 +24,8 @@ class PointCloudProcessor(Node):
 
         # Create cylinders
         self.meshes_static = [
-            trimesh.creation.cylinder(radius=0.18, height=0.9),  # Cylinder for upper_arm_link (10 cm shorter)
-            trimesh.creation.cylinder(radius=0.18, height=1.0)   # Cylinder for forearm_link
+            trimesh.creation.cylinder(radius=0.25, height=0.9),  # Cylinder for upper_arm_link (10 cm shorter)
+            trimesh.creation.cylinder(radius=0.25, height=1.0)   # Cylinder for forearm_link
         ]
 
         # Rotate cylinders 90 degrees around the y-axis
@@ -46,8 +46,8 @@ class PointCloudProcessor(Node):
         self.meshes_static[0].apply_transform(translation_matrix_upper_arm)
 
         # Move the upper_arm_link cylinder 10cm in the positive x direction
-        translation_matrix_upper_arm = trimesh.transformations.translation_matrix([-0.05, 0, 0])
-        self.meshes_static[1].apply_transform(translation_matrix_upper_arm)
+        translation_forearm = trimesh.transformations.translation_matrix([0, 0, 0])
+        self.meshes_static[1].apply_transform(translation_forearm)
 
         # Move cylinders 10cm in the positive z direction
         translation_matrix_z = trimesh.transformations.translation_matrix([0, 0, 0.1])