diff --git a/Progress_Documentation/rosgraph.png b/Progress_Documentation/rosgraph.png
new file mode 100644
index 0000000000000000000000000000000000000000..ff23c1e3b964eda6a6300045131a5f90dabb3da8
Binary files /dev/null and b/Progress_Documentation/rosgraph.png differ
diff --git a/workspaces/COLCON_WS/src/Universal_Robots_ROS2_Gazebo_Simulation b/workspaces/COLCON_WS/src/Universal_Robots_ROS2_Gazebo_Simulation
new file mode 160000
index 0000000000000000000000000000000000000000..93b25c6aae7efd4845633cd01f45403a2cc592e6
--- /dev/null
+++ b/workspaces/COLCON_WS/src/Universal_Robots_ROS2_Gazebo_Simulation
@@ -0,0 +1 @@
+Subproject commit 93b25c6aae7efd4845633cd01f45403a2cc592e6
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
new file mode 100644
index 0000000000000000000000000000000000000000..5d94cbb331a0ea6fcff667b96a3c2a8c15402483
--- /dev/null
+++ b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/moveit_stop.py
@@ -0,0 +1,113 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2
+from tf2_ros import TransformListener, Buffer
+import numpy as np
+import trimesh
+from std_msgs.msg import String
+import sensor_msgs_py.point_cloud2 as pc2
+
+class PointCloudProcessor(Node):
+ def __init__(self):
+ super().__init__('pointcloud_processor')
+
+ self.subscription = self.create_subscription(
+ PointCloud2,
+ 'valid_from_perspective',
+ self.pointcloud_callback,
+ 10)
+
+ self.cancellation_pub = self.create_publisher(
+ String, "/trajectory_execution_event", 1
+ )
+
+ 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=1.05) # Cylinder for forearm_link
+ ]
+
+ # Rotate cylinders 90 degrees around the y-axis
+ rotation_matrix = trimesh.transformations.rotation_matrix(
+ angle=np.pi / 2, # 90 degrees in radians
+ direction=[0, 1, 0] # Rotation around the y-axis
+ )
+ for mesh in self.meshes_static:
+ mesh.apply_transform(rotation_matrix)
+
+ # Translate cylinders 35cm in the negative x direction
+ translation_matrix_x = trimesh.transformations.translation_matrix([-0.35, 0, 0])
+ for mesh in self.meshes_static:
+ 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])
+ 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])
+ self.meshes_static[1].apply_transform(translation_matrix_forearm)
+
+ # Move cylinders 10cm in the positive z direction
+ translation_matrix_z = trimesh.transformations.translation_matrix([0, 0, 0.1])
+ for mesh in self.meshes_static:
+ mesh.apply_transform(translation_matrix_z)
+
+ # Threshold for stopping the trajectory
+ self.point_threshold = 33 # Adjust as needed
+
+ def pointcloud_callback(self, msg):
+ self.perspective_frame = 'vl53l7cx_link'
+ try:
+ now = rclpy.time.Time()
+ transforms = {
+ 'shoulder': self.tf_buffer.lookup_transform(self.perspective_frame, 'upper_arm_link', now),
+ 'forearm': self.tf_buffer.lookup_transform(self.perspective_frame, 'forearm_link', now)
+ }
+ except Exception as e:
+ self.get_logger().error(f'Error in looking up transform: {e}')
+ return
+
+ transformed_meshes = []
+ for i, transform in enumerate(transforms.values()):
+ transformed_meshes.append(self.transform_mesh(transform, self.meshes_static[i]))
+
+ points = np.array([(point['x'], point['y'], point['z']) for point in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)], dtype=np.float64)
+
+ 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)}')
+
+ if len(points_inside_robot) > self.point_threshold:
+ self.stop_trajectory()
+
+ def transform_mesh(self, transform, mesh_static):
+ translation = transform.transform.translation
+ rotation = transform.transform.rotation
+
+ translation_matrix = trimesh.transformations.translation_matrix([translation.x, translation.y, translation.z])
+ rotation_matrix = trimesh.transformations.quaternion_matrix([rotation.w, rotation.x, rotation.y, rotation.z])
+ transformation_matrix = np.dot(translation_matrix, rotation_matrix)
+
+ transformed_mesh = mesh_static.copy()
+ transformed_mesh.apply_transform(transformation_matrix)
+ return transformed_mesh
+
+ def stop_trajectory(self):
+ cancel_string = String()
+ cancel_string.data = "stop"
+ self.cancellation_pub.publish(cancel_string)
+
+def main(args=None):
+ rclpy.init(args=args)
+ pointcloud_processor = PointCloudProcessor()
+ rclpy.spin(pointcloud_processor)
+ pointcloud_processor.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/random_pcl.py b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/random_pcl.py
new file mode 100644
index 0000000000000000000000000000000000000000..5f34bcd7fec830053cba991488e89cccda31d22f
--- /dev/null
+++ b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/random_pcl.py
@@ -0,0 +1,66 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import PointCloud2, PointField
+from std_msgs.msg import Header
+import numpy as np
+import random
+
+#!/usr/bin/env python3
+
+
+class RandomPointCloudPublisher(Node):
+ def __init__(self):
+ super().__init__('random_pointcloud_publisher')
+ self.publisher_ = self.create_publisher(PointCloud2, 'pcl', 10)
+ self.timer = self.create_timer(0.1, self.publish_pointcloud) # Publish every second
+ self.get_logger().info("Random PointCloud Publisher Node has been started.")
+
+ def generate_random_pointcloud(self, num_points=4800):
+ points = np.random.uniform(-1.1, 1.1, (num_points, 3)) # Generate random points as a NumPy array
+ return points
+
+ 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 publish_pointcloud(self):
+ points = self.generate_random_pointcloud()
+ pointcloud_msg = self.create_point_cloud(points, 'vl53l7cx_link')
+ self.publisher_.publish(pointcloud_msg)
+ self.get_logger().info("Published random point cloud.")
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = RandomPointCloudPublisher()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ node.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
diff --git a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/test_transform.py b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/test_transform.py
index 4277d0d8936e38e024e00e4495ebe17519932360..064c3b8fe562eeb75db8ebdc97e0ad3c317cb34b 100644
--- a/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/test_transform.py
+++ b/workspaces/COLCON_WS/src/serial_to_pcl/serial_to_pcl/test_transform.py
@@ -5,10 +5,6 @@ from tf2_ros import TransformListener, Buffer
import numpy as np
import trimesh
from std_msgs.msg import Header
-# Removed unused import
-import os
-
-# Removed unused import
import sensor_msgs.msg as sensor_msgs
import sensor_msgs_py.point_cloud2 as pc2
@@ -17,7 +13,7 @@ class PointCloudProcessor(Node):
super().__init__('pointcloud_processor')
self.subscription = self.create_subscription(
PointCloud2,
- 'pcd',
+ 'pcl',
self.pointcloud_callback,
10)
self.publisher_valid = self.create_publisher(PointCloud2, '/valid_from_perspective', 10)
@@ -26,23 +22,37 @@ class PointCloudProcessor(Node):
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
- base_path = os.path.join(os.path.expanduser('~'), 'robot-sensor', 'workspaces', 'COLCON_WS', 'src', 'ur_description', 'meshes', 'ur10', 'collision_custom')
+ # Create cylinders
self.meshes_static = [
- trimesh.load_mesh(os.path.join(base_path, 'shoulder.stl')),
- trimesh.load_mesh(os.path.join(base_path, 'forearm.stl')),
- trimesh.load_mesh(os.path.join(base_path, 'upperarm.stl')),
- trimesh.load_mesh(os.path.join(base_path, 'wrist1.stl')),
- trimesh.load_mesh(os.path.join(base_path, 'wrist2.stl')),
- trimesh.load_mesh(os.path.join(base_path, 'wrist3.stl'))
+ 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
]
- base_path_collision = os.path.join(os.path.expanduser('~'), 'robot-sensor', 'workspaces', 'COLCON_WS', 'src', 'ur_description', 'meshes', 'ur10', 'collision')
- self.meshes_static.append(trimesh.load_mesh(os.path.join(base_path_collision, 'base.stl')))
+ # Rotate cylinders 90 degrees around the y-axis
+ rotation_matrix = trimesh.transformations.rotation_matrix(
+ angle=np.pi / 2, # 90 degrees in radians
+ direction=[0, 1, 0] # Rotation around the y-axis
+ )
+ for mesh in self.meshes_static:
+ mesh.apply_transform(rotation_matrix)
+
+ # Translate cylinders 35cm in the negative x direction
+ translation_matrix_x = trimesh.transformations.translation_matrix([-0.35, 0, 0])
+ for mesh in self.meshes_static:
+ 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])
+ 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)
- scale_factor = 1.5
- scale_matrix = trimesh.transformations.scale_matrix(scale_factor)
+ # Move cylinders 10cm in the positive z direction
+ translation_matrix_z = trimesh.transformations.translation_matrix([0, 0, 0.1])
for mesh in self.meshes_static:
- mesh.apply_transform(scale_matrix)
+ mesh.apply_transform(translation_matrix_z)
def visualize_meshes(self, meshes):
if meshes: # Check if there are meshes to visualize
@@ -58,26 +68,18 @@ class PointCloudProcessor(Node):
try:
now = rclpy.time.Time()
transforms = {
- 'base': self.tf_buffer.lookup_transform(self.perspective_frame, 'world', now),
- 'shoulder': self.tf_buffer.lookup_transform(self.perspective_frame, 'shoulder_link', now),
- 'upperarm': self.tf_buffer.lookup_transform(self.perspective_frame, 'upper_arm_link', now),
- 'forearm': self.tf_buffer.lookup_transform(self.perspective_frame, 'forearm_link', now),
- 'wrist1': self.tf_buffer.lookup_transform(self.perspective_frame, 'wrist_1_link', now),
- 'wrist2': self.tf_buffer.lookup_transform(self.perspective_frame, 'wrist_2_link', now),
- 'wrist3': self.tf_buffer.lookup_transform(self.perspective_frame, 'wrist_3_link', now)
+ 'shoulder': self.tf_buffer.lookup_transform(self.perspective_frame, 'upper_arm_link', now),
+ 'forearm': self.tf_buffer.lookup_transform(self.perspective_frame, 'forearm_link', now)
}
except Exception as e:
self.get_logger().error(f'Error in looking up transform: {e}')
+ return
+
transformed_meshes = []
- for i, transform in enumerate(transforms.values()): # Removed unused 'key'
+ for i, transform in enumerate(transforms.values()):
transformed_meshes.append(self.transform_mesh(transform, self.meshes_static[i]))
- for i, (key, transform) in enumerate(transforms.items()):
- transformed_meshes.append(self.transform_mesh(transform, self.meshes_static[i]))
-
- translation_matrix_forearm = trimesh.transformations.translation_matrix([0, 0, -0.05])
- transformed_meshes[3].apply_transform(translation_matrix_forearm)
- self.visualize_meshes(transformed_meshes)
+ #self.visualize_meshes(transformed_meshes)
points = np.array([(point['x'], point['y'], point['z']) for point in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)], dtype=np.float64)
@@ -104,6 +106,7 @@ class PointCloudProcessor(Node):
return transformed_mesh
def publish_point_cloud(self, points_valid, points_invalid):
+ self.get_logger().info(f'Publishing {len(points_valid)} valid points.')
self.publisher_valid.publish(self.point_cloud(points_valid, self.perspective_frame))
self.publisher_invalid.publish(self.point_cloud(points_invalid, self.perspective_frame))
@@ -136,4 +139,4 @@ def main(args=None):
rclpy.shutdown()
if __name__ == '__main__':
- main()
\ No newline at end of file
+ main()
diff --git a/workspaces/COLCON_WS/src/serial_to_pcl/setup.py b/workspaces/COLCON_WS/src/serial_to_pcl/setup.py
index 621d049a65dc8fd17e70cfabeb1b12c54b92dc9f..5daa3e48ddd38861e5ab9398bec8ee1ca28e1efc 100755
--- a/workspaces/COLCON_WS/src/serial_to_pcl/setup.py
+++ b/workspaces/COLCON_WS/src/serial_to_pcl/setup.py
@@ -23,6 +23,7 @@ setup(
'serial_to_pcl_node = serial_to_pcl.serial_to_pcl_node:main',
'pcl_rob_node = serial_to_pcl.pcl_rob_node:main',
'pcl_rob_v2_node = serial_to_pcl.pcl_rob_v2_node:main',
+ 'random_pcl_node = serial_to_pcl.random_pcl:main',
],
},
)
diff --git a/workspaces/pymoveit2_ws/src/pymoveit2 b/workspaces/pymoveit2_ws/src/pymoveit2
new file mode 160000
index 0000000000000000000000000000000000000000..4797ed5b0b89e602f184459b99f08257f78f319d
--- /dev/null
+++ b/workspaces/pymoveit2_ws/src/pymoveit2
@@ -0,0 +1 @@
+Subproject commit 4797ed5b0b89e602f184459b99f08257f78f319d