Merge pull request #87 from compas-dev/simplify-ros-load

Simplify load robot from ROS

Author: gonzalocasas

CHANGELOG.rst

@@ -12,6 +12,7 @@ Unreleased
 
 **Added**
 
+* Added ``load_robot`` method to ROS client to simplify loading robots from running ROS setup.
 * Added ``compas_fab.robots.Wrench``: a Wrench class representing force in free space, separated into its linear (force) and angular (torque) parts.
 * Added ``compas_fab.robots.Inertia``: a Inertia class representing spatial distribution of mass in a rigid body
 

docs/examples/03_backends_ros/files/02_forward_kinematics_ros_loader.py

@@ -1,21 +1,8 @@
-from compas.robots import RobotModel
-
 from compas_fab.backends import RosClient
-from compas_fab.backends import RosFileServerLoader
 from compas_fab.robots import Configuration
-from compas_fab.robots import Robot
-from compas_fab.robots import RobotSemantics
 
 with RosClient() as client:
-    loader = RosFileServerLoader(client)
-
-    urdf = loader.load_urdf()
-    srdf = loader.load_srdf()
-
-    model = RobotModel.from_urdf_string(urdf)
-    semantics = RobotSemantics.from_srdf_string(srdf, model)
-
-    robot = Robot(model, semantics=semantics, client=client)
+    robot = client.load_robot()
 
     configuration = Configuration.from_prismatic_and_revolute_values([0.], [-2.238, -1.153, -2.174, 0.185, 0.667, 0.])
 

docs/examples/03_backends_ros/files/02_inverse_kinematics_ros_loader.py

@@ -1,21 +1,8 @@
 from compas.geometry import Frame
-from compas.robots import RobotModel
-
 from compas_fab.backends import RosClient
-from compas_fab.backends import RosFileServerLoader
-from compas_fab.robots import Robot
-from compas_fab.robots import RobotSemantics
 
 with RosClient() as client:
-    loader = RosFileServerLoader(client)
-
-    urdf = loader.load_urdf()
-    srdf = loader.load_srdf()
-
-    model = RobotModel.from_urdf_string(urdf)
-    semantics = RobotSemantics.from_srdf_string(srdf, model)
-
-    robot = Robot(model, semantics=semantics, client=client)
+    robot = client.load_robot()
 
     frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
     start_configuration = robot.init_configuration()

docs/examples/03_backends_ros/files/04_plan_cartesian_motion_ros_loader.py

@@ -1,22 +1,9 @@
 from compas.geometry import Frame
-from compas.robots import RobotModel
-
 from compas_fab.backends import RosClient
-from compas_fab.backends import RosFileServerLoader
 from compas_fab.robots import Configuration
-from compas_fab.robots import Robot
-from compas_fab.robots import RobotSemantics
 
 with RosClient() as client:
-    loader = RosFileServerLoader(client)
-
-    urdf = loader.load_urdf()
-    srdf = loader.load_srdf()
-
-    model = RobotModel.from_urdf_string(urdf)
-    semantics = RobotSemantics.from_srdf_string(srdf, model)
-
-    robot = Robot(model, semantics=semantics, client=client)
+    robot = client.load_robot()
     group = robot.main_group_name
 
     frames = []

docs/examples/03_backends_ros/files/04_plan_motion_ros_loader.py

@@ -1,23 +1,11 @@
 import math
 from compas.geometry import Frame
-from compas.robots import RobotModel
 
 from compas_fab.backends import RosClient
-from compas_fab.backends import RosFileServerLoader
 from compas_fab.robots import Configuration
-from compas_fab.robots import Robot
-from compas_fab.robots import RobotSemantics
 
 with RosClient() as client:
-    loader = RosFileServerLoader(client)
-
-    urdf = loader.load_urdf()
-    srdf = loader.load_srdf()
-
-    model = RobotModel.from_urdf_string(urdf)
-    semantics = RobotSemantics.from_srdf_string(srdf, model)
-
-    robot = Robot(model, semantics=semantics, client=client)
+    robot = client.load_robot()
     group = robot.main_group_name
 
     frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])

docs/examples/03_backends_ros/files/robot-playground.ghx

@@ -4220,11 +4220,7 @@ if sc.sticky[response_key]:
 from scriptcontext import sticky as st
 
 import compas
-from compas.robots import RobotModel
 from compas_fab.backends import RosClient
-from compas_fab.backends import RosFileServerLoader
-from compas_fab.robots import Robot
-from compas_fab.robots import RobotSemantics
 from compas_fab.ghpython import RobotArtist
 
 compas.PRECISION = '12f'
@@ -4237,21 +4233,11 @@ if robot_key not in st:
     st[robot_key] = None
 
 if load:
-    with RosClient() as ros:
-        # Load URDF from ROS with local cache enabled
-        local_directory = os.path.join(os.path.expanduser('~'), 'robot_description')
-        loader = RosFileServerLoader(ros, local_cache=True, local_cache_directory=local_directory)
-        loader.robot_name = 'panda-0001'
-    
-        urdf = loader.load_urdf()
-        srdf = loader.load_srdf()
+    local_directory = os.path.join(os.path.expanduser('~'), 'robot_description', 'panda')
 
-        # Create robot model from URDF and load geometry
-        model = RobotModel.from_urdf_string(urdf)
-        model.load_geometry(loader)
-        robot = Robot(model)
+    with RosClient() as ros:
+        robot = ros.load_robot(load_geometry=True, local_cache_directory=local_directory)
         robot.artist = RobotArtist(robot.model)
-        robot.semantics = RobotSemantics.from_srdf_string(srdf, model)
 
         st[robot_key] = robot
 
@@ -15078,4 +15064,4 @@ if robot:
       </items>
     </chunk>
   </chunks>
-</Archive>
+</Archive>

src/compas_fab/backends/ros/client.py

@@ -1,12 +1,16 @@
 from __future__ import print_function
 
+import os
+
+from compas.robots import RobotModel
 from compas.utilities import await_callback
 from roslibpy import Message
 from roslibpy import Ros
 from roslibpy.actionlib import ActionClient
 from roslibpy.actionlib import Goal
 
 from compas_fab.backends.ros.exceptions import RosError
+from compas_fab.backends.ros.fileserver_loader import RosFileServerLoader
 from compas_fab.backends.ros.messages import ExecuteTrajectoryFeedback
 from compas_fab.backends.ros.messages import ExecuteTrajectoryGoal
 from compas_fab.backends.ros.messages import ExecuteTrajectoryResult
@@ -21,6 +25,8 @@
 from compas_fab.backends.ros.messages import Time
 from compas_fab.backends.ros.planner_backend_moveit import MoveItPlanner
 from compas_fab.backends.tasks import CancellableFutureResult
+from compas_fab.robots import Robot
+from compas_fab.robots import RobotSemantics
 
 __all__ = [
     'RosClient',
@@ -110,6 +116,60 @@ def __exit__(self, *args):
 
         self.close()
 
+    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.
+
+        Parameters
+        ----------
+        load_geometry : bool, optional
+            ``True`` to load the robot's geometry, otherwise ``False`` to load only the model and semantics.
+        urdf_param_name : str, optional
+            Parameter name where the URDF is defined. If not defined, it will default to ``/robot_description``.
+        srdf_param_name : str, optional
+            Parameter name where the SRDF is defined. If not defined, it will default to ``/robot_description_semantic``.
+        precision : float
+            Defines precision for importing/loading meshes. Defaults to ``compas.PRECISION``.
+        local_cache_directory : str, optional
+            Directory where the robot description (URDF, SRDF and meshes) are stored.
+            This differs from the directory taken as parameter by the :class:`RosFileServerLoader`
+            in that it points directly to the specific robot package, not to a global workspace storage
+            for all robots. If not assigned, the robot will not be cached locally.
+
+        Examples
+        --------
+
+        >>> from compas_fab.backends import RosClient
+        >>> with RosClient() as client:
+        ...     robot = client.load_robot()
+        ...     print(robot.name)
+        ur5
+        """
+        robot_name = None
+        use_local_cache = False
+
+        if local_cache_directory is not None:
+            use_local_cache = True
+            path_parts = local_cache_directory.strip(os.path.sep).split(os.path.sep)
+            path_parts, robot_name = path_parts[:-1], path_parts[-1]
+            local_cache_directory = os.path.sep.join(path_parts)
+
+        loader = RosFileServerLoader(self, use_local_cache, local_cache_directory, precision)
+
+        if robot_name:
+            loader.robot_name = robot_name
+
+        urdf = loader.load_urdf(urdf_param_name)
+        srdf = loader.load_srdf(srdf_param_name)
+
+        model = RobotModel.from_urdf_string(urdf)
+        semantics = RobotSemantics.from_srdf_string(srdf, model)
+
+        if load_geometry:
+            model.load_geometry(loader)
+
+        return Robot(model, semantics=semantics, client=self)
+
+
     def inverse_kinematics(self, frame, base_link, group,
                            joint_names, joint_positions, avoid_collisions=True,
                            constraints=None, attempts=8,

src/compas_fab/backends/ros/fileserver_loader.py

@@ -15,10 +15,10 @@
 from compas.geometry import Transformation
 from compas.robots.resources.basic import _get_file_format
 from compas.robots.resources.basic import _mesh_import
-from compas.utilities import await_callback
 from compas.utilities import geometric_key
 
 LOGGER = logging.getLogger('compas_fab.backends.ros')
+TIMEOUT = 10
 
 __all__ = [
     'RosFileServerLoader',
@@ -119,7 +119,7 @@ def load_urdf(self, parameter_name='/robot_description'):
                 return _read_file(filename)
 
         param = roslibpy.Param(self.ros, parameter_name)
-        urdf = await_callback(param.get)
+        urdf = param.get(timeout=TIMEOUT)
 
         self.robot_name = self._read_robot_name(urdf)
 
@@ -151,7 +151,7 @@ def load_srdf(self, parameter_name='/robot_description_semantic'):
                 return _read_file(filename)
 
         param = roslibpy.Param(self.ros, parameter_name)
-        srdf = await_callback(param.get)
+        srdf = param.get(timeout=TIMEOUT)
 
         if self.local_cache_enabled:
             _write_file(self._srdf_filename, srdf)
@@ -205,7 +205,7 @@ def load_mesh(self, url):
         if not use_local_file:
             service = roslibpy.Service(self.ros, '/file_server/get_file', 'file_server/GetBinaryFile')
             request = roslibpy.ServiceRequest(dict(name=url))
-            response = await_callback(service.call, 'callback', 'errback', request)
+            response = service.call(request, timeout=TIMEOUT)
 
             file_content = binascii.a2b_base64(response.data['value'])