Merge pull request #279 from compas-dev/ros-noetic-support

Add support for ROS Noetic

Author: gonzalocasas

CHANGELOG.rst

@@ -12,11 +12,14 @@ Unreleased
 
 **Added**
 
+* Added support for MoveIt on ROS Noetic
+
 **Changed**
 
 **Fixed**
 
 * Fix ``repr()`` of ``ROSmsg`` class
+* Fix data type of secs and nsecs in ``Time`` ROS message
 
 **Deprecated**
 

README.rst

@@ -30,6 +30,10 @@ COMPAS FAB: Robotic Fabrication for COMPAS
 .. image:: https://zenodo.org/badge/107952684.svg
    :target: https://zenodo.org/badge/latestdoi/107952684
 
+.. image:: https://img.shields.io/twitter/follow/compas_dev?style=social
+   :target: https://twitter.com/compas_dev
+   :alt: Twitter Follow
+
 .. end-badges
 
 **Robotic fabrication package for the COMPAS Framework** that facilitates the

setup.cfg

@@ -41,7 +41,6 @@ line_length = 180
 known_first_party = compas_fab
 default_section = THIRDPARTY
 forced_separate = test_compas_fab
-not_skip = __init__.py
 skip = migrations
 
 [coverage:run]

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

@@ -5,8 +5,9 @@
 from compas.utilities import await_callback
 
 from compas_fab.backends.interfaces import InverseKinematics
-from compas_fab.backends.ros.backend_features.helpers import validate_response
 from compas_fab.backends.ros.backend_features.helpers import convert_constraints_to_rosmsg
+from compas_fab.backends.ros.backend_features.helpers import validate_response
+from compas_fab.backends.ros.messages import RosDistro
 from compas_fab.backends.ros.messages import AttachedCollisionObject
 from compas_fab.backends.ros.messages import GetPositionIKRequest
 from compas_fab.backends.ros.messages import GetPositionIKResponse
@@ -18,6 +19,7 @@
 from compas_fab.backends.ros.messages import PositionIKRequest
 from compas_fab.backends.ros.messages import RobotState
 from compas_fab.backends.ros.service_description import ServiceDescription
+from compas_fab.robots import Duration
 
 __all__ = [
     'MoveItInverseKinematics',
@@ -61,10 +63,12 @@ def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=N
             - ``"constraints"``: (:obj:`list` of :class:`compas_fab.robots.Constraint`, optional)
               A set of constraints that the request must obey.
               Defaults to ``None``.
-            - ``"attempts"``: (:obj:`int`, optional) The maximum number of inverse kinematic attempts.
-              Defaults to ``8``.
+            - ``"timeout"``: (:obj:`int`, optional) Maximum allowed time for inverse kinematic calculation in seconds.
+              Defaults to ``2``. This value supersedes the ``"attempts"`` argument used before ROS Noetic.
             - ``"attached_collision_meshes"``: (:obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh`, optional)
               Defaults to ``None``.
+            - ``"attempts"``: (:obj:`int`, optional) The maximum number of inverse kinematic attempts.
+              Defaults to ``8``. This value is ignored on ROS Noetic and newer, use ``"timeout"`` instead.
 
         Raises
         ------
@@ -108,12 +112,22 @@ def inverse_kinematics_async(self, callback, errback,
 
         constraints = convert_constraints_to_rosmsg(options.get('constraints'), header)
 
+        timeout_in_secs = options.get('timeout', 2)
+        timeout_duration = Duration(timeout_in_secs, 0).to_data()
+
         ik_request = PositionIKRequest(group_name=group,
                                        robot_state=start_state,
                                        constraints=constraints,
                                        pose_stamped=pose_stamped,
                                        avoid_collisions=options.get('avoid_collisions', True),
-                                       attempts=options.get('attempts', 8))
+                                       attempts=options.get('attempts', 8),
+                                       timeout=timeout_duration)
+
+        # The field `attempts` was removed in Noetic (and higher)
+        # so it needs to be removed from the message otherwise it causes a serialization error
+        # https://github.com/ros-planning/moveit/pull/1288
+        if self.ros_client.ros_distro not in (RosDistro.KINETIC, RosDistro.MELODIC):
+            del ik_request.attempts
 
         def convert_to_positions(response):
             callback((response.solution.joint_state.position, response.solution.joint_state.name))

src/compas_fab/backends/ros/client.py

@@ -4,6 +4,7 @@
 
 from compas.robots import RobotModel
 from roslibpy import Message
+from roslibpy import Param
 from roslibpy import Ros
 from roslibpy.actionlib import ActionClient
 from roslibpy.actionlib import Goal
@@ -142,6 +143,7 @@ def __init__(self, host='localhost', port=9090, is_secure=False, planner_backend
 
         planner_backend_type = PLANNER_BACKENDS[planner_backend]
         self.planner = planner_backend_type(self)
+        self._ros_distro = None
 
     def __enter__(self):
         self.run()
@@ -151,6 +153,14 @@ def __enter__(self):
     def __exit__(self, *args):
         self.close()
 
+    @property
+    def ros_distro(self):
+        """Retrieves the ROS version to which the client is connected (eg. kinetic)"""
+        if not self._ros_distro:
+            self._ros_distro = Param(self, '/rosdistro').get(timeout=1)
+
+        return self._ros_distro
+
     def load_robot(self, load_geometry=False, urdf_param_name='/robot_description', srdf_param_name='/robot_description_semantic', precision=None, local_cache_directory=None):
         """Load an entire robot instance -including model and semantics- directly from ROS.
 

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

@@ -6,6 +6,7 @@
 from .moveit_msgs import *              # noqa: F401,F403
 from .object_recognition_msgs import *  # noqa: F401,F403
 from .octomap_msgs import *             # noqa: F401,F403
+from .ros_releases import *             # noqa: F401,F403
 from .sensor_msgs import *              # noqa: F401,F403
 from .services import *                 # noqa: F401,F403
 from .shape_msgs import *               # noqa: F401,F403

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

@@ -185,7 +185,7 @@ class PositionIKRequest(ROSmsg):
     """
 
     def __init__(self, group_name="robot", robot_state=None, constraints=None,
-                 pose_stamped=None, timeout=1.0, attempts=8,
+                 pose_stamped=None, timeout=None, attempts=8,
                  avoid_collisions=True):
         self.group_name = group_name
         self.robot_state = robot_state if robot_state else RobotState()

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

@@ -0,0 +1,10 @@
+from __future__ import absolute_import
+
+
+class RosDistro(object):
+    """Supported/recognized ROS distros"""
+    KINETIC = 'kinetic'
+    MELODIC = 'melodic'
+    NOETIC = 'noetic'
+
+    SUPPORTED_DISTROS = tuple([KINETIC, MELODIC, NOETIC])

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

@@ -43,7 +43,7 @@ class Time(ROSmsg):
     """https://docs.ros.org/kinetic/api/std_msgs/html/msg/Time.html
     """
 
-    def __init__(self, secs=0., nsecs=0.):
+    def __init__(self, secs=0, nsecs=0):
         self.secs = secs
         self.nsecs = nsecs