Feature ros2

CHANGELOG.md

@@ -9,6 +9,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+* Added a `ros2` filtering for the header msg type.
 * Added `compas_fab.robots.Waypoints` class to represent a sequence of targets. It has two child classes: `FrameWaypoints` and `PointAxisWaypoints`.
 * Added `compas_fab.robots.Target` class to represent a motion planning target.
 * Added also child classes `FrameTarget`, `PointAxisTarget`, `ConfigurationTarget`, `ConstraintSetTarget`

src/compas_fab/backends/ros/backend_features/move_it_add_attached_collision_mesh.py

@@ -50,6 +50,7 @@ def add_attached_collision_mesh(self, attached_collision_mesh, options=None):
 
     def add_attached_collision_mesh_async(self, callback, errback, attached_collision_mesh):
         aco = AttachedCollisionObject.from_attached_collision_mesh(attached_collision_mesh)
+        aco.object.filter_fields_for_distro(self.client.ros_distro)
         aco.object.operation = CollisionObject.ADD
         robot_state = RobotState(attached_collision_objects=[aco], is_diff=True)
         scene = PlanningScene(robot_state=robot_state, is_diff=True)

src/compas_fab/backends/ros/backend_features/move_it_add_collision_mesh.py

@@ -11,6 +11,7 @@
 from compas_fab.backends.ros.messages import PlanningScene
 from compas_fab.backends.ros.messages import PlanningSceneWorld
 from compas_fab.backends.ros.service_description import ServiceDescription
+from compas_fab.backends.ros.messages import RosDistro
 
 __all__ = [
     "MoveItAddCollisionMesh",
@@ -49,8 +50,14 @@ def add_collision_mesh(self, collision_mesh, options=None):
 
     def add_collision_mesh_async(self, callback, errback, collision_mesh):
         co = CollisionObject.from_collision_mesh(collision_mesh)
+        co.filter_fields_for_distro(self.client.ros_distro)
         co.operation = CollisionObject.ADD
         world = PlanningSceneWorld(collision_objects=[co])
+        if self.client.ros_distro in (RosDistro.HUMBLE, RosDistro.JAZZY):
+            world.octomap.filter_fields_for_ros2()
         scene = PlanningScene(world=world, is_diff=True)
+        if self.client.ros_distro in (RosDistro.HUMBLE, RosDistro.JAZZY):
+            scene.robot_state.joint_state.filter_fields_for_ros2()
+            scene.robot_state.multi_dof_joint_state.filter_fields_for_ros2()
         request = scene.to_request(self.client.ros_distro)
         self.APPLY_PLANNING_SCENE(self.client, request, callback, errback)

src/compas_fab/backends/ros/backend_features/move_it_append_collision_mesh.py

@@ -49,6 +49,7 @@ def append_collision_mesh(self, collision_mesh, options=None):
 
     def append_collision_mesh_async(self, callback, errback, collision_mesh):
         co = CollisionObject.from_collision_mesh(collision_mesh)
+        co.filter_fields_for_distro(self.client.ros_distro)
         co.operation = CollisionObject.APPEND
         world = PlanningSceneWorld(collision_objects=[co])
         scene = PlanningScene(world=world, is_diff=True)

src/compas_fab/backends/ros/backend_features/move_it_forward_kinematics.py

@@ -13,6 +13,7 @@
 from compas_fab.backends.ros.messages import MultiDOFJointState
 from compas_fab.backends.ros.messages import RobotState
 from compas_fab.backends.ros.service_description import ServiceDescription
+from compas_fab.backends.ros.messages import RosDistro
 
 __all__ = [
     "MoveItForwardKinematics",
@@ -78,6 +79,9 @@ def forward_kinematics_async(self, callback, errback, configuration, options):
         fk_link_names = [options["link"]]
 
         header = Header(frame_id=base_link)
+        if self.client.ros_distro in (RosDistro.HUMBLE, RosDistro.JAZZY):
+            header = header.for_ros2()
+
         joint_state = JointState(name=configuration.joint_names, position=configuration.joint_values, header=header)
         robot_state = RobotState(joint_state, MultiDOFJointState(header=header))
         robot_state.filter_fields_for_distro(self.client.ros_distro)

src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py

@@ -99,6 +99,9 @@ def inverse_kinematics_async(
         """Asynchronous handler of MoveIt IK service."""
         base_link = options["base_link"]
         header = Header(frame_id=base_link)
+        if self.client.ros_distro in (RosDistro.HUMBLE, RosDistro.JAZZY):
+            header = header.for_ros2()
+
         pose_stamped = PoseStamped(header, Pose.from_frame(frame_WCF))
 
         joint_state = JointState(

src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py

@@ -17,6 +17,7 @@
 from compas_fab.backends.ros.messages import Pose
 from compas_fab.backends.ros.messages import RobotState
 from compas_fab.backends.ros.service_description import ServiceDescription
+from compas_fab.backends.ros.messages import RosDistro
 
 from compas_fab.robots import FrameWaypoints
 from compas_fab.robots import PointAxisWaypoints
@@ -119,7 +120,9 @@ def plan_cartesian_motion_with_frame_waypoints_async(
         joints = options["joints"]
 
         header = Header(frame_id=options["base_link"])
-
+        if self.client.ros_distro in (RosDistro.HUMBLE, RosDistro.JAZZY):
+            header = header.for_ros2()
+
         # Convert compas_fab.robots.FrameWaypoints to list of Pose for ROS
         list_of_pose = [Pose.from_frame(frame) for frame in waypoints.target_frames]
 

src/compas_fab/backends/ros/backend_features/move_it_plan_motion.py

@@ -18,6 +18,7 @@
 from compas_fab.backends.ros.messages import RobotState
 from compas_fab.backends.ros.messages import TrajectoryConstraints
 from compas_fab.backends.ros.service_description import ServiceDescription
+from compas_fab.backends.ros.messages import RosDistro
 
 __all__ = ["MoveItPlanMotion"]
 
@@ -99,6 +100,9 @@ def plan_motion_async(self, callback, errback, target, start_configuration=None,
 
         joints = options["joints"]
         header = Header(frame_id=options["base_link"])
+        if self.client.ros_distro in (RosDistro.HUMBLE, RosDistro.JAZZY):
+            header = header.for_ros2()
+
         joint_state = JointState(
             header=header, name=start_configuration.joint_names, position=start_configuration.joint_values
         )
@@ -107,6 +111,7 @@ def plan_motion_async(self, callback, errback, target, start_configuration=None,
         if options.get("attached_collision_meshes"):
             for acm in options["attached_collision_meshes"]:
                 aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
+                aco.object.filter_fields_for_distro(self.client.ros_distro)
                 start_state.attached_collision_objects.append(aco)
 
         # Filter needs to happen after all objects have been added

src/compas_fab/backends/ros/backend_features/move_it_remove_attached_collision_mesh.py

@@ -51,6 +51,7 @@ def remove_attached_collision_mesh(self, id, options=None):
     def remove_attached_collision_mesh_async(self, callback, errback, id):
         aco = AttachedCollisionObject()
         aco.object.id = id
+        aco.object.filter_fields_for_distro(self.client.ros_distro)
         aco.object.operation = CollisionObject.REMOVE
         robot_state = RobotState(attached_collision_objects=[aco], is_diff=True)
         scene = PlanningScene(robot_state=robot_state, is_diff=True)

src/compas_fab/backends/ros/backend_features/move_it_remove_collision_mesh.py

@@ -50,6 +50,7 @@ def remove_collision_mesh(self, id, options=None):
     def remove_collision_mesh_async(self, callback, errback, id):
         co = CollisionObject()
         co.id = id
+        co.filter_fields_for_distro(self.client.ros_distro)
         co.operation = CollisionObject.REMOVE
         world = PlanningSceneWorld(collision_objects=[co])
         scene = PlanningScene(world=world, is_diff=True)

src/compas_fab/backends/ros/messages/geometry_msgs.py

@@ -67,6 +67,7 @@ def from_msg(cls, msg):
         position = Point.from_msg(msg["position"])
         orientation = Quaternion.from_msg(msg["orientation"])
         return cls(position, orientation)
+        """Creates a ROS2 compatible Pose class without the orientation field"""
 
 
 class PoseStamped(ROSmsg):
@@ -83,6 +84,10 @@ def from_msg(cls, msg):
         header = Header.from_msg(msg["header"])
         pose = Pose.from_msg(msg["pose"])
         return cls(header, pose)
+
+    def filter_fields_for_ros2(self):
+        if hasattr(self, "header"):
+            self.header = self.header.filter_fields_for_ros2()
 
 
 class PoseArray(ROSmsg):
@@ -99,6 +104,10 @@ def from_msg(cls, msg):
         header = Header.from_msg(msg["header"])
         poses = [Pose.from_msg(p) for p in msg["poses"]]
         return cls(header, poses)
+
+    def filter_fields_for_ros2(self):
+        if hasattr(self, "header"):
+            self.header = self.header.filter_fields_for_ros2()
 
 
 class Vector3(ROSmsg):
@@ -194,14 +203,18 @@ def from_msg(cls, msg):
         header = Header.from_msg(msg["header"])
         wrench = Wrench.from_msg(msg["wrench"])
         return cls(header, wrench)
+
+    def filter_fields_for_ros2(self):
+        if hasattr(self, "header"):
+            self.header = self.header.filter_fields_for_ros2()
 
 
 class Inertia(ROSmsg):
     """https://docs.ros.org/api/geometry_msgs/html/msg/Inertia.html
 
     Examples
     --------
-    >>> inertia = compas_fab.robots.Inertia([[0] * 3] * 3, 1., [0.1, 3.1, 4.4])
+    >>> inertia = compas_fab.robots.Inertia([[0] * 3] * 3, 1.0, [0.1, 3.1, 4.4])
     >>> ros_inertia = Inertia.from_inertia(inertia)
     >>> ros_inertia.msg
     {'m': 1.0, 'com': {'x': 0.1, 'y': 3.1, 'z': 4.4}, 'ixx': 0.0, 'ixy': 0.0, 'ixz': 0.0, 'iyy': 0.0, 'iyz': 0.0, 'izz': 0.0}

src/compas_fab/backends/ros/messages/moveit_msgs.py

@@ -128,6 +128,10 @@ def to_collision_meshes(self):
             cm = CollisionMesh(mesh.mesh, self.id, pose.frame, root_name)
             collision_meshes.append(cm)
         return collision_meshes
+
+    def filter_fields_for_distro(self, ros_distro):
+        if ros_distro in (RosDistro.HUMBLE, RosDistro.JAZZY):
+            self.header = self.header.for_ros2()
 
 
 class AttachedCollisionObject(ROSmsg):
@@ -163,7 +167,8 @@ def to_attached_collision_meshes(self):
             acm = AttachedCollisionMesh(cm, self.link_name, self.touch_links, self.weight)
             attached_collision_meshes.append(acm)
         return attached_collision_meshes
-
+
+
 
 class Constraints(ROSmsg):
     """https://docs.ros.org/kinetic/api/moveit_msgs/html/msg/Constraints.html"""
@@ -211,7 +216,7 @@ def filter_fields_for_distro(self, ros_distro):
         # Remove the field `pose` for distros older than NOETIC
         if ros_distro in (RosDistro.KINETIC, RosDistro.MELODIC):
             for aco in self.attached_collision_objects:
-                del aco.object.pose
+                del aco.object.pose      
 
 
 class PositionIKRequest(ROSmsg):
@@ -579,6 +584,7 @@ def filter_fields_for_distro(self, ros_distro):
                 del co.pose
 
 
+
 class PlanningScene(ROSmsg):
     """https://docs.ros.org/melodic/api/moveit_msgs/html/msg/PlanningScene.html"""
 

src/compas_fab/backends/ros/messages/octomap_msgs.py

@@ -18,7 +18,10 @@ def __init__(self, header=None, binary=False, id="", resolution=0.0, data=None):
         self.id = id  # Class id of the contained octree
         self.resolution = resolution  # Resolution (in m) of the smallest octree nodes
         self.data = data or []  # binary serialization of octree, use conversions.h to read and write octrees
-
+
+    def filter_fields_for_ros2(self):
+        if hasattr(self, "header"):
+            self.header = self.header.for_ros2()
 
 class OctomapWithPose(ROSmsg):
     """https://docs.ros.org/kinetic/api/octomap_msgs/html/msg/OctomapWithPose.html"""
@@ -29,3 +32,10 @@ def __init__(self, header=None, origin=None, octomap=None):
         self.header = header or Header()  # Header
         self.origin = origin or Pose()  # geometry_msgs/Pose  The pose of the octree with respect to the header frame
         self.octomap = octomap or Octomap()  # octomap_msgs/Octomap  The actual octree msg
+
+    def filter_fields_for_ros2(self):
+        if hasattr(self, "header"):
+            self.header = self.header.for_ros2()
+        self.octomap.filter_fields_for_ros2()
+
+

src/compas_fab/backends/ros/messages/ros_releases.py

@@ -7,5 +7,7 @@ class RosDistro(object):
     KINETIC = "kinetic"
     MELODIC = "melodic"
     NOETIC = "noetic"
+    HUMBLE = "humble"
+    JAZZY = "jazzy"
 
-    SUPPORTED_DISTROS = tuple([KINETIC, MELODIC, NOETIC])
+    SUPPORTED_DISTROS = tuple([KINETIC, MELODIC, NOETIC, HUMBLE, JAZZY])

src/compas_fab/backends/ros/messages/sensor_msgs.py

@@ -36,6 +36,10 @@ def from_msg(cls, msg):
         velocity = msg["velocity"]
         effort = msg["effort"]
         return cls(header, name, position, velocity, effort)
+
+    def filter_fields_for_ros2(self):
+        if hasattr(self, "header"):
+            self.header = self.header.for_ros2()
 
 
 class MultiDOFJointState(ROSmsg):
@@ -49,3 +53,7 @@ def __init__(self, header=None, joint_names=None, transforms=None, twist=None, w
         self.transforms = transforms if transforms else []
         self.twist = twist if twist else []
         self.wrench = wrench if wrench else []
+
+    def filter_fields_for_ros2(self):
+        if hasattr(self, "header"):
+            self.header = self.header.for_ros2()

src/compas_fab/backends/ros/messages/std_msgs.py

@@ -115,6 +115,11 @@ def __init__(self, seq=0, stamp=Time(), frame_id="/world"):
         self.stamp = stamp
         self.frame_id = frame_id
 
+    def for_ros2(self):
+        """Creates a ROS2 compatible Header class without the seq field"""
+        header2 = self.msg.copy()
+        header2.pop("seq", None)
+        return header2
 
 class String(ROSmsg):
     """https://docs.ros.org/api/std_msgs/html/msg/String.html"""