added another video

workspaces/sensor_ws/src/ser_test/ser_test/pcl_rob_node.py

@@ -27,12 +27,12 @@ class PointCloudProcessor(Node):
         self.tf_listener = TransformListener(self.tf_buffer, self)
 
         # Load the STL file
-        self.mesh_base_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/forearm.stl')
+        self.mesh_base_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/shoulder.stl')
         self.mesh_base_link.apply_scale(2)
         self.bounding_box = self.mesh_base_link.bounding_box
 
     def pointcloud_callback(self, msg):
-        self.perspective_frame ='forearm_link'
+        self.perspective_frame ='shoulder_link'
         try:
             now = rclpy.time.Time().to_msg()
             trans = self.tf_buffer.lookup_transform(self.perspective_frame, msg.header.frame_id, now, timeout=rclpy.duration.Duration(seconds=1.0))
@@ -41,7 +41,7 @@ class PointCloudProcessor(Node):
             return
         
         self.transformed_points = self.do_transform_cloud(msg, trans.transform)
-        inside = self.bounding_box.contains(self.transformed_points)
+        inside = self.mesh_base_link.contains(self.transformed_points)
         outside = np.logical_not(inside)
         self.transformed_points =np.asanyarray(self.transformed_points)
         points_valid = np.hstack((self.transformed_points, inside.reshape(256,1)))
@@ -59,6 +59,9 @@ class PointCloudProcessor(Node):
         points = []
         for point in list_of_tuples:
             point = rotation.apply(point)
+            point[0] = point[0] + trans.translation.x
+            point[1] = point[1] + trans.translation.y
+            point[2] = point[2] + trans.translation.z
             points.append(point)
         return points