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):