diff --git a/workspaces/sensor_ws/src/ser_test/ser_test/pcl_rob_node.py b/workspaces/sensor_ws/src/ser_test/ser_test/pcl_rob_node.py index 95a46a5df9fe0d70556078d9b621186fb07af4d4..39562a06aed1475aa5404544e56aeeda7cac4809 100644 --- a/workspaces/sensor_ws/src/ser_test/ser_test/pcl_rob_node.py +++ b/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