add ROS feature with simulators

pyrobolearn/simulators/bullet.py

@@ -740,7 +740,7 @@ class Bullet(Simulator):
     # loading URDFs, SDFs, MJCFs, meshes #
     ######################################
 
-    def load_urdf(self, filename, position=None, orientation=None, use_maximal_coordinates=None,
+    def _load_urdf(self, filename, position=None, orientation=None, use_maximal_coordinates=None,
                   use_fixed_base=None, flags=None, scale=None):
         """Load the given URDF file.
 

pyrobolearn/simulators/middlewares/middleware.py

@@ -6,14 +6,6 @@ Dependencies in PRL:
 * NONE
 """
 
-# TODO
-import os
-import subprocess
-import psutil
-import signal
-import importlib
-import inspect
-
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2019, PyRoboLearn"
@@ -28,7 +20,7 @@ __status__ = "Development"
 class MiddleWare(object):
     r"""Middleware (abstract) class
 
-    Middlewares can be provided to simulators which can then use them to send/receive messages.
+    Middleware can be provided to simulators which can then use them to send/receive messages.
     """
 
     def __init__(self, subscribe=False, publish=False, teleoperate=False):
@@ -43,42 +35,105 @@ class MiddleWare(object):
               previous attributes :attr:`subscribe` and :attr:`publish`.
         """
         # set variables
-        self.subscribe = subscribe
-        self.publish = publish
-        self.teleoperate = teleoperate
+        self.is_subscribing = subscribe
+        self.is_publishing = publish
+        self.is_teleoperating = teleoperate
 
     ##############
     # Properties #
     ##############
 
     @property
-    def subscribe(self):
+    def is_subscribing(self):
         return self._subscribe
 
-    @subscribe.setter
-    def subscribe(self, subscribe):
+    @is_subscribing.setter
+    def is_subscribing(self, subscribe):
         self._subscribe = bool(subscribe)
 
     @property
-    def publish(self):
+    def is_publishing(self):
         return self._publish
 
-    @publish.setter
-    def publish(self, publish):
+    @is_publishing.setter
+    def is_publishing(self, publish):
         self._publish = bool(publish)
 
     @property
-    def teleoperate(self):
+    def is_teleoperating(self):
         return self._teleoperate
 
-    @teleoperate.setter
-    def teleoperate(self, teleoperate):
+    @is_teleoperating.setter
+    def is_teleoperating(self, teleoperate):
         self._teleoperate = bool(teleoperate)
 
+    #############
+    # Operators #
+    #############
+
+    def __str__(self):
+        """Return a readable string about the class."""
+        return self.__class__.__name__
+
+    def __del__(self):
+        """Close/Delete the simulator."""
+        self.close()
+
+    def __copy__(self):
+        """Return a shallow copy of the middleware. This can be overridden in the child class."""
+        return self.__class__(subscribe=self.is_subscribing, publish=self.is_publishing,
+                              teleoperate=self.is_teleoperating)
+
+    def __deepcopy__(self, memo={}):
+        """Return a deep copy of the middleware. This can be overridden in the child class.
+
+        Args:
+            memo (dict): memo dictionary of objects already copied during the current copying pass.
+        """
+        # if the object has already been copied return the reference to the copied object
+        if self in memo:
+            return memo[self]
+
+        # create a new copy of the simulator
+        middleware = self.__class__(subscribe=self.is_subscribing, publish=self.is_publishing,
+                                    teleoperate=self.is_teleoperating)
+
+        memo[self] = middleware
+        return middleware
+
     ###########
     # Methods #
     ###########
 
+    def close(self):
+        """
+        Close the middleware.
+        """
+        pass
+
+    def reset(self):
+        """
+        Reset the middleware.
+        """
+        pass
+
+    def load_urdf(self, urdf):
+        """Load the given URDF file.
+
+        The load_urdf will send a command to the physics server to load a physics model from a Universal Robot
+        Description File (URDF). The URDF file is used by the ROS project (Robot Operating System) to describe robots
+        and other objects, it was created by the WillowGarage and the Open Source Robotics Foundation (OSRF).
+        Many robots have public URDF files, you can find a description and tutorial here:
+        http://wiki.ros.org/urdf/Tutorials
+
+        Args:
+            urdf (str): a relative or absolute path to the URDF file on the file system of the physics server.
+
+        Returns:
+            int (non-negative): unique id associated to the load model.
+        """
+        pass
+
     def has_sensor(self, body_id, name):
         """
         Check if the specified robot has the given sensor.

pyrobolearn/simulators/middlewares/ros_publisher.py

@@ -3,6 +3,8 @@
 """Define the abstract robot publisher.
 """
 
+import collections
+
 import rospy
 
 # import the messages
@@ -12,7 +14,7 @@ from geometry_msgs import msg as geometry_msg
 
 
 __author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
 __license__ = "GNU GPLv3"
 __version__ = "1.0.0"
@@ -23,26 +25,167 @@ __status__ = "Development"
 
 class PublisherData(object):
     r"""Publisher data holder
+
+    This just instantiates the ROS publisher but also allows to easily access the message attributes from this class.
+    You can also publish the last message that has been set, or send a new one.
+
+    For instance, the `std_msgs.String` has one attribute called `data`, you can then instantiate a `PublisherData`
+    like `pub = PublisherData(topic_name, std_msgs.String, queue_size=10)`. You can then access to the message instance
+    with `pub.msg`, and you can directly access to the attribute using `pub.data` (you can also access it with
+    `pub.msg.data`).
     """
 
     def __init__(self, topic, data_class, queue_size=10):
-        self.__dict__['publisher'] = rospy.Publisher(topic, data_class, queue_size=queue_size)
-        self.__dict__['attributes'] = [attr for attr in [attr for attr in dir(data_class) if not attr.startswith('_')]
-                                       if not callable(getattr(data_class, attr))]
-        self.__dict__['publisher_data'] = data_class()
+        """
+        Initialize the PublisherData that publishes the given message data.
 
-    def publish(self, data=None):
+        Args:
+            topic (str, list[str]): topic name(s). If multiple topics are given, it will group them. Note that you can
+              only group topics that use the same message class.
+            data_class (class): message class for serialization.
+            queue_size (int): The queue size used for asynchronously publishing messages from different threads. A
+              size of zero means an infinite queue, which can be dangerous. When None is passed all publishing will
+              happen synchronously and a warning message will be printed.
+        """
+        # self.__dict__['publisher'] = rospy.Publisher(topic, data_class, queue_size=queue_size)
+        # # set the message attributes to be part of this class attributes
+        # self.__dict__['attributes'] = [attr for attr in [attr for attr in dir(data_class) if not attr.startswith('_')]
+        #                                if not callable(getattr(data_class, attr))]
+        # self.__dict__['msg'] = data_class()
+
+        self.topic = topic
+        self.msg_class = data_class
+        if isinstance(topic, collections.Iterable):
+            self.is_group = True
+            self.publisher = [rospy.Publisher(t, data_class, queue_size=queue_size) for t in topic]
+            self.msg = [data_class() for _ in topic]
+        else:
+            self.is_group = False
+            self.publisher = rospy.Publisher(topic, data_class, queue_size=queue_size)
+            self.msg = data_class()
+
+    def publish(self, data=None, indices=None, replace=True):
+        """
+        Publish the given data.
+
+        Args:
+            data (None, class, list[class]): message class instance(s) that holds the data. If None, it will sent the
+              last message.
+            indices (None, list[int], int): if multiple topics are defined for this class, you can specify which index
+              to use.
+            replace (bool): if True, it will replace the message by the given data.
+        """
         if data is None:
-            self.publisher.publish(self.publisher_data)
+            replace = False
+            data = self.msg
+            if isinstance(indices, int):
+                data = data[indices]
+
+        # if multiple publisher
+        if isinstance(self.publisher, collections.Iterable):
+            if indices is None:  # publish to every topics the corresponding data
+                for idx, (pub, msg) in enumerate(zip(self.publisher, data)):
+                    pub.publish(msg)
+                    if replace:
+                        self.msg[idx] = msg
+            else:  # if indices are specified, send it to the specified indices
+                if isinstance(indices, int):
+                    self.publisher[indices].publish(data)
+                    if replace:
+                        self.msg[indices] = data
+                else:
+                    for i, index in enumerate(indices):
+                        self.publisher[index].publish(data[i])
+                        if replace:
+                            self.msg[index] = data[i]
+
+        # if one publisher
         else:
             self.publisher.publish(data)
+            if replace:
+                self.msg = data
 
-    def __setattr__(self, key, value):
-        if key in self.attributes:
-            setattr(self.publisher_data, key, value)
+    # def __setattr__(self, key, value):
+    #     """Set the given attribute to the message."""
+    #     if key in self.attributes:
+    #         setattr(self.msg, key, value)
+    #
+    # def __getattr__(self, key):
+    #     """Get the specified attribute value from the message."""
+    #     return getattr(self.msg, key)
 
-    def __getattr__(self, key):
-        return getattr(self.publisher_data, key)
+    def set_attributes(self, key, values, indices=None):
+        """
+        Set the given value(s) to the message attributes.
+
+        Args:
+            key (str): message attribute name.
+            values (object): message attribute value(s).
+            indices (None, list[int], int): if multiple topics are defined for this class, you can specify which index
+              to use.
+        """
+        if self.is_group:
+            if indices is None:  # set every message attribute
+                if isinstance(values, collections.Iterable):
+                    for msg, value in zip(self.msg, values):
+                        setattr(msg, key, value)
+                else:
+                    for msg in self.msg:
+                        setattr(msg, key, values)
+            else:
+                if isinstance(indices, int):
+                    setattr(self.msg[indices], key, values)
+                else:
+                    if isinstance(values, collections.Iterable):
+                        for i, index in enumerate(indices):
+                            setattr(self.msg[index], key, values[i])
+                    else:
+                        for index in indices:
+                            setattr(self.msg[index], key, values)
+
+        else:
+            setattr(self.msg, key, values)
+
+    def get_attributes(self, key, indices=None):
+        """
+        Get the given message attribute(s) specified by the given key name.
+
+        Args:
+            key (str): message attribute name to get.
+            indices (None, list[int], int): if multiple topics are defined for this class, you can specify which index
+              to use.
+
+        Returns:
+            object: message attribute value(s).
+        """
+        if self.is_group:
+            if indices is None:
+                return [getattr(msg, key) for msg in self.msg]
+            elif isinstance(indices, int):
+                return getattr(self.msg[indices], key)
+            elif isinstance(indices, collections.Iterable):
+                return [getattr(self.msg[index], key) for index in indices]
+            else:
+                raise TypeError("Expecting the given indices to be an int, None, or list of int, but got instead: "
+                                "{}".format(type(indices)))
+        return getattr(self.msg, key)
+
+    def unregister(self):
+        """
+        Unsubscribe from a topic. Topic instance is no longer valid after this call. Additional calls to `unregister()`
+        have no effect.
+        """
+        if isinstance(self.publisher, collections.Iterable):
+            for subscriber in self.publisher:
+                subscriber.unregister()
+        else:
+            self.publisher.unregister()
+
+    def __del__(self):
+        """
+        Close all topics.
+        """
+        self.unregister()
 
 
 class Publisher(object):
@@ -50,6 +193,8 @@ class Publisher(object):
 
     This Publisher abstract class is the class from which all the other publishers inherit from. It provides the
     common functionalities between the various publishers.
+
+    It contains one or more `PublisherData` instances to send the various data.
     """
 
     def __init__(self, publisher_id=None):
@@ -58,8 +203,8 @@ class Publisher(object):
 
         Args:
             publisher_id (int, None): publisher id which is used when initializing the node. If None, a name will be
-                auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous`
-                parameter in `rospy.init_node`.
+              auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous`
+              parameter in `rospy.init_node`.
         """
 
         # initialize the node
@@ -71,33 +216,98 @@ class Publisher(object):
         # all publishers
         self.publishers = dict()
 
-    def create_publisher(self, name, topic, data_class):
+    def create_publisher(self, name, topic, data_class, queue_size=10):
         """
         Create a publisher to the specific topic.
 
         Args:
             name (str): unique name of the publisher. The name must be unique. You will be able to access to this
-            topic (str): name of the topic.
+              publisher using its name.
+            topic (str, list[str]): name of the topic(s).
             data_class (object): data type class to use for messages
+            queue_size (int): The queue size used for asynchronously publishing messages from different threads. A
+              size of zero means an infinite queue, which can be dangerous. When None is passed all publishing will
+              happen synchronously and a warning message will be printed.
 
         Returns:
             PublisherData: the publisher data holder.
         """
-        publisher = PublisherData(topic, data_class)
+        publisher = PublisherData(topic, data_class, queue_size=queue_size)
         self.publishers[name] = publisher
-        setattr(self, name, publisher)
+        # setattr(self, name, publisher)
         return publisher
 
-    def publish(self, name=None, data=None):
+    def has_publisher(self, name):
+        """
+        Return True if the given publisher name has been created.
+
+        Args:
+            name (str): unique name of the publisher.
+
+        Returns:
+            bool: True if the given publisher name exists.
+        """
+        return name in self.publishers
+
+    def get_subscriber(self, name):
+        """
+        Return the associated `PublisherData` given its unique name.
+
+        Args:
+            name (str): unique name of the publisher.
+
+        Returns:
+            PublisherData, None: the publisher data holder. None if the publisher associated with the given name
+              doesn't exist.
+        """
+        return self.publishers.get(name)
+
+    def publish(self, name=None, data=None, indices=None):
+        """
+        Publish the given data using the given publisher name.
+
+        Args:
+            name (str): unique name of the publisher.
+            data (class, None): message class instance that holds the data. If None, it will sent the last message.
+            indices (None, list[int], int): if multiple topics are defined for this class, you can specify which index
+              to use.
+        """
         if name is None and data is None:
             for publisher in self.publishers.values():
                 publisher.publish()
         elif name is not None:
-            self.__dict__[name].publish(data)
+            # self.__dict__[name].publish(data)
+            self.publishers[name].publish(data=data, indices=indices)
 
     # def __getattr__(self, name):
     #     return self.publishers[name]
 
+    def unregister(self, name=None):
+        """
+        Unsubscribe from a topic. Topic instance is no longer valid after this call. Additional calls to `unregister()`
+        have no effect.
+
+        Args:
+            name (str, None): name of the topic to unsubscribe. If None, it will unsubscribe from all topics.
+        """
+        if name is None:
+            for subscriber in self.publishers.values():
+                subscriber.unregister()
+        else:
+            self.publishers[name].unregister()
+
+    def close(self):
+        """
+        Close all topics.
+        """
+        self.unregister()
+
+    def __del__(self):
+        """
+        Close all topics.
+        """
+        self.unregister()
+
 
 class RobotPublisher(Publisher):
     r"""Robot Publisher class
@@ -112,26 +322,126 @@ class RobotPublisher(Publisher):
         Args:
             name (str): name of the robot. This will be used to create the topics.
             id_ (int, None): robot id which is used when initializing the node. If None, a name will be
-                auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous`
-                parameter in `rospy.init_node`.
+              auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous`
+              parameter in `rospy.init_node`.
         """
         super(RobotPublisher, self).__init__(publisher_id=id_)
         self.name = name.lower()
 
         # create Joint states
-        # self.create_publisher('joint_states', self.name + '/joint_states', sensor_msg.JointState)
-        self.joint_states = PublisherData(self.name + '/joint_states', sensor_msg.JointState)
-        self.publishers['joint_states'] = self.joint_states
+        # self.joint_states = PublisherData(self.name + '/joint_states', sensor_msg.JointState)
+        # self.publishers['joint_states'] = self.joint_states
 
-    def set_joint_positions(self, joint_ids, positions):
-        # self.joint_states.position[joint_ids] = positions
-        self.joint_states.position = positions
+        # self.joint_cmds = self.create_publisher('joint_cmds', self.name + '/joint_commands', sensor_msg.JointState)
 
-    def set_joint_velocities(self, joint_ids, velocities):
-        self.joint_states.velocity[joint_ids] = velocities
+        # joint position/velocity/torque commands
+        self.q_cmd, self.dq_cmd, self.tau_cmd = None, None, None
+        self.q_attr, self.dq_attr, self.tau_attr = None, None, None
 
-    def set_joint_torques(self, joint_ids, torques):
-        self.joint_states.effort[joint_ids] = torques
+    @staticmethod
+    def __check_publisher_and_attribute(publisher, attribute_name):
+        """
+        Check the type of the publisher and attribute name.
+
+        Args:
+            publisher (PublisherData): publisher.
+            attribute_name (str): message attribute name.
+        """
+        if not isinstance(publisher, PublisherData):
+            raise TypeError("Expecting the given 'publisher' to be an instance of `PublisherData`, instead got: "
+                            "{}".format(type(publisher)))
+        if not isinstance(attribute_name, str):
+            raise TypeError("Expecting the given 'attribute_name' to be a string, instead got: "
+                            "{}".format(type(attribute_name)))
+
+    def init_set_joint_positions(self, publisher, msg_attribute_name):
+        """
+        Initialize set joint positions.
+
+        Args:
+            publisher (PublisherData): publisher.
+            msg_attribute_name (str): message attribute name.
+        """
+        self.__check_publisher_and_attribute(publisher, msg_attribute_name)
+        self.q_cmd = publisher
+        self.q_attr = msg_attribute_name
+
+    def set_joint_positions(self, positions, q_indices=None):
+        """
+        Set the given joint positions.
+
+        Args:
+            positions (float, np.array[float]): joint position(s) to set.
+            q_indices (int, list[int], np.array[int], None): joint q index / indices. If None, it will consider all
+              the joints.
+        """
+        if self.q_cmd is not None:
+            self.q_cmd.set_attributes(key=self.q_attr, values=positions, indices=q_indices)
+
+    def init_set_joint_velocities(self, publisher, msg_attribute_name):
+        """
+        Initialize set joint velocities.
+
+        Args:
+            publisher (PublisherData): publisher.
+            msg_attribute_name (str): message attribute name.
+        """
+        self.__check_publisher_and_attribute(publisher, msg_attribute_name)
+        self.dq_cmd = publisher
+        self.dq_attr = msg_attribute_name
+
+    def set_joint_velocities(self, velocities, q_indices=None):
+        """
+        Set the given joint velocities.
+
+        Args:
+            velocities (float, np.array[float]): joint velocity(ies) to set.
+            q_indices (int, list[int], np.array[int], None): joint q index / indices. If None, it will consider all
+              the joints.
+        """
+        if self.dq_cmd is not None:
+            self.dq_cmd.set_attributes(key=self.dq_attr, values=velocities, indices=q_indices)
+
+    def init_set_joint_torques(self, publisher, msg_attribute_name):
+        """
+        Initialize set joint torques.
+
+        Args:
+            publisher (PublisherData): publisher.
+            msg_attribute_name (str): message attribute name.
+        """
+        self.__check_publisher_and_attribute(publisher, msg_attribute_name)
+        self.tau_cmd = publisher
+        self.tau_attr = msg_attribute_name
+
+    def set_joint_torques(self, torques, q_indices=None):
+        """
+        Set the given joint torques.
+
+        Args:
+            torques (float, np.array[float]): joint torques to set.
+            q_indices (int, list[int], np.array[int], None): joint q index / indices. If None, it will consider all
+              the joints.
+        """
+        if self.tau_cmd is not None:
+            self.tau_cmd.set_attributes(key=self.tau_attr, values=torques, indices=q_indices)
+
+    def set_pid(self, pid, q_indices=None):
+        """
+        Set the given PID coefficients to the given joint ids.
+
+        Args:
+            pid (list[np.array[float[3]]]): list of PID coefficients for each joint. If one of the value is -1, it
+              will left untouched the associated PID value to the previous one.
+            q_indices (int, list[int], np.array[int], None): joint q index / indices. If None, it will consider all
+              the joints.
+        """
+        # rospy.wait_for_service('/gazebo/reset_simulation')
+        # try:
+        #     self.reset_srv()
+        # except rospy.ServiceException as e:
+        #     print("/gazebo/reset_simulation service call failed")
+        pass  # /rrbot/joint1_position_controller/pid/set_parameters
 
 
 # Tests
@@ -143,9 +453,9 @@ if __name__ == '__main__':
 
     publisher = RobotPublisher('walter')
     print("Published topics: {}".format(rospy.get_published_topics()))
-    print("Robot joint state attributes: {}".format(publisher.joint_states.attributes))
+    print("Robot joint state attributes: {}".format(publisher.joint_cmds.msg.attributes))
 
-    publisher.joint_states.position = np.array(range(3))
+    publisher.joint_cmds.position = np.array(range(3))
 
     for t in count():
         print(t)

pyrobolearn/simulators/middlewares/ros_subscriber.py

@@ -3,6 +3,8 @@
 """Define the abstract robot subscriber.
 """
 
+import collections
+
 import numpy as np
 import rospy
 
@@ -13,7 +15,7 @@ from sensor_msgs import msg as sensor_msg
 
 
 __author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
 __license__ = "GNU GPLv3"
 __version__ = "1.0.0"
@@ -24,22 +26,94 @@ __status__ = "Development"
 
 class SubscriberData(object):
     r"""Subscriber data holder
+
+    This instantiates the ROS subscriber, save the received data, and allow to easily access the message attributes
+    from this class.
     """
 
     def __init__(self, topic, data_class):
-        self.subscriber = rospy.Subscriber(topic, data_class, callback=self.callback)
-        self.attributes = set([attr for attr in [attr for attr in dir(data_class) if not attr.startswith('_')]
-                               if not callable(getattr(data_class, attr))])
-        self.subscriber_data = data_class()
+        """
+        Initialize the SubscriberData that subscribes to the given topic.
 
-    def callback(self, data):
-        self.subscriber_data = data
+        Args:
+            topic (str, list[str]): topic name(s). If multiple topics are given, it will group them. Note that you can
+              only group topics that use the same message class.
+            data_class (class): message class for serialization.
+        """
+        # self.subscriber = rospy.Subscriber(topic, data_class, callback=self.callback)
+        # # set the message attributes to be part of this class attributes
+        # self.attributes = set([attr for attr in [attr for attr in dir(data_class) if not attr.startswith('_')]
+        #                        if not callable(getattr(data_class, attr))])
+        # self.subscriber_data = data_class()
 
-    def __getattr__(self, name):
-        return getattr(self.subscriber_data, name)
+        self.topic = topic
+        if isinstance(topic, collections.Iterable):
+            self.is_group = True
+            self.subscriber = [rospy.Subscriber(t, data_class, callback=self.callback, callback_args=idx)
+                               for idx, t in enumerate(topic)]
+            self.msg = [data_class() for _ in topic]
+        else:
+            self.is_group = False
+            self.subscriber = rospy.Subscriber(topic, data_class, callback=self.callback)
+            self.msg = data_class()
+
+    def callback(self, data, idx=None):
+        """
+        Callback function that saves the data in the current instance.
+
+        Args:
+            data (object): message class instance.
+            idx (int, None): message index.
+        """
+        if idx is None:
+            self.msg = data
+        else:
+            self.msg[idx] = data
+
+    # def __getattr__(self, name):
+    #     """Get the specified attribute value given its name."""
+    #     return getattr(self.subscriber_data, name)
+
+    def get_attributes(self, key, indices=None):
+        """
+        Get the given message attribute(s) specified by the given key name.
+
+        Args:
+            key (str): message attribute name to get.
+            indices (None, list[int], int): if multiple topics are defined for this class, you can specify which index
+              to use.
+
+        Returns:
+            object: message attribute value(s).
+        """
+        if self.is_group:
+            if indices is None:
+                return [getattr(msg, key) for msg in self.msg]
+            elif isinstance(indices, int):
+                return getattr(self.msg[indices], key)
+            elif isinstance(indices, collections.Iterable):
+                return [getattr(self.msg[index], key) for index in indices]
+            else:
+                raise TypeError("Expecting the given indices to be an int, None, or list of int, but got instead: "
+                                "{}".format(type(indices)))
+        return getattr(self.msg, key)
 
     def unregister(self):
-        self.subscriber.unregister()
+        """
+        Unsubscribe from a topic. Topic instance is no longer valid after this call. Additional calls to `unregister()`
+        have no effect.
+        """
+        if isinstance(self.subscriber, collections.Iterable):
+            for subscriber in self.subscriber:
+                subscriber.unregister()
+        else:
+            self.subscriber.unregister()
+
+    def __del__(self):
+        """
+        Close all topics.
+        """
+        self.unregister()
 
 
 class Subscriber(object):
@@ -55,10 +129,9 @@ class Subscriber(object):
 
         Args:
             subscriber_id (int, None): subscriber id which is used when initializing the node. If None, a name will be
-                auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous`
-                parameter in `rospy.init_node`.
+              auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous`
+              parameter in `rospy.init_node`.
         """
-
         # initialize the node
         if subscriber_id is None:
             rospy.init_node(self.__class__.__name__, anonymous=True)
@@ -74,7 +147,8 @@ class Subscriber(object):
 
         Args:
             name (str): unique name of the subscriber. The name must be unique. You will be able to access to this
-            topic (str): name of the topic.
+              subscriber using its name.
+            topic (str, list[str]): name of the topic(s).
             data_class (object): data type class to use for messages
 
         Returns:
@@ -84,10 +158,42 @@ class Subscriber(object):
         self.subscribers[name] = subscriber
         return subscriber
 
-    def __getattr__(self, name):
-        return self.subscribers[name]
+    def has_subscriber(self, name):
+        """
+        Return True if the given subscriber name has been created.
+
+        Args:
+            name (str): unique name of the subscriber.
+
+        Returns:
+            bool: True if the given subscriber name exists.
+        """
+        return name in self.subscribers
+
+    def get_subscriber(self, name):
+        """
+        Return the associated `SubscriberData` given its unique name.
+
+        Args:
+            name (str): unique name of the subscriber.
+
+        Returns:
+            SubscriberData, None: the subscriber data holder. None if the subscriber associated with the given name
+              doesn't exist.
+        """
+        return self.subscribers.get(name)
+
+    # def __getattr__(self, name):
+    #     return self.subscribers[name]
 
     def unregister(self, name=None):
+        """
+        Unsubscribe from a topic. Topic instance is no longer valid after this call. Additional calls to `unregister()`
+        have no effect.
+
+        Args:
+            name (str, None): name of the topic to unsubscribe. If None, it will unsubscribe from all topics.
+        """
         if name is None:
             for subscriber in self.subscribers.values():
                 subscriber.unregister()
@@ -95,9 +201,15 @@ class Subscriber(object):
             self.subscribers[name].unregister()
 
     def close(self):
+        """
+        Close all topics.
+        """
         self.unregister()
 
     def __del__(self):
+        """
+        Close all topics.
+        """
         self.unregister()
 
 
@@ -120,23 +232,79 @@ class RobotSubscriber(Subscriber):
         super(RobotSubscriber, self).__init__(subscriber_id=id_)
         self.name = name.lower()
 
-        # create Joint states
-        self.create_subscriber('joint_states', self.name + '/joint_states', sensor_msg.JointState)
+        # create Joint states (automatically)
+        self.joint_states = self.create_subscriber('joint_states', '/' + self.name + '/joint_states',
+                                                   sensor_msg.JointState)
 
-    def get_joint_positions(self, joint_ids):
-        if len(self.joint_states.position) >= len(joint_ids):
-            return np.asarray(self.joint_states.position)  # [joint_ids]
-        return np.asarray(self.joint_states.position)
+    def get_joint_positions(self, q_indices=None):
+        """
+        Get the joint positions.
 
-    def get_joint_velocities(self, joint_ids):
-        if len(self.joint_states.velocity) >= len(joint_ids):
-            return np.asarray(self.joint_states.velocity)[joint_ids]
-        return np.asarray(self.joint_states.velocity)
+        Args:
+            q_indices (int, list[int], np.array[int], None): joint q index / indices. If None, it will return all the
+              joint positions.
 
-    def get_joint_torques(self, joint_ids):
-        if len(self.joint_states.effort) >= len(joint_ids):
-            return np.asarray(self.joint_states.effort)[joint_ids]
-        return np.asarray(self.joint_states.effort)
+        Returns:
+            float, np.array[float]: joint positions.
+        """
+        if q_indices is None:
+            return np.asarray(self.joint_states.msg.position)
+        return np.asarray(self.joint_states.msg.position)[q_indices]
+
+    def get_joint_velocities(self, q_indices=None):
+        """
+        Get the joint velocities.
+
+        Args:
+            q_indices (int, list[int], np.array[int], None): joint q index / indices. If None, it will return all the
+              joint velocities.
+
+        Returns:
+            float, np.array[float]: joint velocities.
+        """
+        if q_indices is None:
+            return np.asarray(self.joint_states.msg.velocity)
+        return np.asarray(self.joint_states.msg.velocity)[q_indices]
+
+    def get_joint_torques(self, q_indices=None):
+        """
+        Get the joint torques.
+
+        Args:
+            q_indices (int, list[int], np.array[int], None): joint q index / indices. If None, it will return all the
+              joint torques.
+
+        Returns:
+            float, np.array[float]: joint torques.
+        """
+        if q_indices is None:
+            return np.asarray(self.joint_states.msg.effort)
+        return np.asarray(self.joint_states.msg.effort)[q_indices]
+
+    def get_pid(self, q_indices=None):
+        """
+        Get the PID coefficients associated to the given joint ids.
+
+        Args:
+            q_indices (int, list[int], np.array[int], None): joint q index / indices. If None, it will return all the
+              joint PIDs.
+
+        Returns:
+            np.array[float[3]], list[np.array[float[3]]]: list of PID coefficients for each joint.
+        """
+        pass
+
+    def get_jacobian(self):
+        """
+        Return the jacobian.
+        """
+        pass
+
+    def get_inertia_matrix(self):
+        """
+        Return the inertia matrix.
+        """
+        pass
 
 
 # Tests
@@ -147,9 +315,9 @@ if __name__ == '__main__':
 
     subscriber = RobotSubscriber('walter')
     print("Published topics: {}".format(rospy.get_published_topics()))
-    print("Robot joint state attributes: {}".format(subscriber.joint_states.attributes))
+    print("Robot joint state attributes: {}".format(subscriber.joint_states.msg.attributes))
 
     for t in count():
         print(t)
-        print("Joint position data: {}".format(subscriber.joint_states.position))
+        print("Joint position data: {}".format(subscriber.joint_states.msg.position))
         time.sleep(0.1)

pyrobolearn/simulators/simulator.py

@@ -202,6 +202,8 @@ class Simulator(object):
         self.kwargs = kwargs
         self._num_instances = num_instances
         self.middleware = middleware
+        self._middleware_enabled = True  # by default
+        self._middleware_ids = {}  # {simulator_body_id: middleware_body_id}
 
         # main camera in the simulator
         self._camera = None
@@ -322,10 +324,9 @@ class Simulator(object):
         """Return True if the simulator can simulate soft bodies."""
         return False
 
-    @staticmethod
-    def has_middleware_communication_layer():
+    def has_middleware_communication_layer(self):
         """Return True if the simulator has a middleware communication layer (like ROS, YARP, etc)."""
-        return self._middleware is not None
+        return self.middleware is not None
 
     @staticmethod
     def supports_dynamic_loading():
@@ -420,6 +421,23 @@ class Simulator(object):
     # Methods #
     ###########
 
+    # Middleware
+
+    def enable_middleware(self, enable=True):
+        """
+        Enable the middleware.
+
+        Args:
+            enable (bool): True if we need to enable it. If False, it will disable it.
+        """
+        self._middleware_enabled = enable
+
+    def disable_middleware(self):
+        """
+        Disable the middleware.
+        """
+        self.enable_middleware(enable=False)
+
     # Simulators
 
     def reset(self, *args, **kwargs):
@@ -427,6 +445,12 @@ class Simulator(object):
         pass
 
     def close(self):
+        """Close the simulator."""
+        if self.middleware is not None:
+            self.middleware.close()
+        self._close()
+
+    def _close(self):
         """Close the simulator."""
         pass
 
@@ -584,7 +608,31 @@ class Simulator(object):
 
     # loading URDFs, SDFs, MJCFs
 
-    def load_urdf(self, filename, position, orientation, use_fixed_base=0, scale=1.0, *args, **kwargs):
+    def load_urdf(self, filename, position, orientation=(0., 0., 0., 1.), use_fixed_base=0, scale=1.0,
+                  *args, **kwargs):
+        """Load a URDF file in the simulator.
+
+        Args:
+            filename (str): a relative or absolute path to the URDF file on the file system of the physics server.
+            position (np.array[float[3]]): create the base of the object at the specified position in world space
+              coordinates [x,y,z].
+            orientation (np.array[float[4]]): create the base of the object at the specified orientation as world
+              space quaternion [x,y,z,w].
+            use_fixed_base (bool): force the base of the loaded object to be static
+            scale (float): scale factor to the URDF model.
+
+        Returns:
+            int (non-negative): unique id associated to the load model.
+        """
+        sim_body_id = self._load_urdf(filename, position, orientation, use_fixed_base=use_fixed_base, scale=scale,
+                                      *args, **kwargs)
+        if self.middleware is not None:
+            middleware_body_id = self.middleware.load_urdf(filename)
+            self._middleware_ids[sim_body_id] = middleware_body_id
+        return sim_body_id
+
+    def _load_urdf(self, filename, position, orientation=(0., 0., 0., 1.), use_fixed_base=0, scale=1.0,
+                   *args, **kwargs):
         """Load a URDF file in the simulator.
 
         Args:
@@ -1723,7 +1771,7 @@ class Simulator(object):
         self._set_joint_positions(body_id, joint_ids, positions, velocities, kps, kds, forces)
 
         # publish the joint positions through the middleware
-        if self.middleware is not None:
+        if self.middleware is not None and self._middleware_enabled:
             self.middleware.set_joint_positions(body_id, joint_ids, positions, velocities, kps, kds, forces)
 
     def _set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None):
@@ -1756,11 +1804,17 @@ class Simulator(object):
                 np.array[float[N]]: joint positions [rad]
         """
         # if a middleware is defined
-        if self.middleware is not None:
+        if self.middleware is not None and self._middleware_enabled:
+            middleware_id = self._middleware_ids[body_id]
             # get joint positions from the middleware
-            q = self.middleware.get_joint_positions(body_id, joint_ids)
+            q = self.middleware.get_joint_positions(middleware_id, joint_ids)
             if q is None:  # if we didn't get the joint positions from the middleware, get them from the simulator
                 q = self._get_joint_positions(body_id, joint_ids)
+
+                # if the middleware is set on teleoperation mode, publish the joint positions through the middleware
+                if self.middleware.is_teleoperating:
+                    self.middleware.set_joint_positions(middleware_id, joint_ids, q, check_teleoperate=True)
+
             else:  # if we got them from the middleware, set them in the simulator
                 self._set_joint_positions(body_id=body_id, joint_ids=joint_ids, positions=q)
 
@@ -1768,10 +1822,6 @@ class Simulator(object):
             # get the joint positions from the simulator
             q = self._get_joint_positions(body_id, joint_ids)
 
-            # if the middleware is set on the teleoperation mode, publish the joint positions through the middleware
-            if self.middleware is not None:
-                self.middleware.set_joint_positions(body_id, joint_ids, q, check_teleoperate=True)
-
         return q
 
     def _get_joint_positions(self, body_id, joint_ids):
@@ -1804,7 +1854,7 @@ class Simulator(object):
         self._set_joint_velocities(body_id, joint_ids, velocities, max_force)
 
         # publish the joint velocities through the middleware
-        if self.middleware is not None:
+        if self.middleware is not None and self._middleware_enabled:
             self.middleware.set_joint_velocities(body_id, joint_ids, velocities, max_force)
 
     def _set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
@@ -1834,7 +1884,7 @@ class Simulator(object):
                 np.array[float[N]]: joint velocities [rad/s]
         """
         # if a middleware is defined
-        if self.middleware is not None:
+        if self.middleware is not None and self._middleware_enabled:
             # get joint velocities from the middleware
             dq = self.middleware.get_joint_velocities(body_id, joint_ids)
             if dq is None:  # if we didn't get the joint velocities from the middleware, get them from the simulator
@@ -1847,7 +1897,7 @@ class Simulator(object):
             dq = self._get_joint_velocities(body_id, joint_ids)
 
             # if the middleware is set on the teleoperation mode, publish the joint velocities through the middleware
-            if self.middleware is not None:
+            if self.middleware is not None and self._middleware_enabled:
                 self.middleware.set_joint_velocities(body_id, joint_ids, dq, check_teleoperate=True)
 
         return dq
@@ -1910,7 +1960,7 @@ class Simulator(object):
         self._set_joint_torques(body_id, joint_ids, torques)
 
         # publish the joint torques through the middleware
-        if self.middleware is not None:
+        if self.middleware is not None and self._middleware_enabled:
             self.middleware.set_joint_torques(body_id, joint_ids, torques)
 
     def _set_joint_torques(self, body_id, joint_ids, torques):
@@ -1939,7 +1989,7 @@ class Simulator(object):
                 np.array[float[N]]: torques associated to the given joints [Nm]
         """
         # if a middleware is defined
-        if self.middleware is not None:
+        if self.middleware is not None and self._middleware_enabled:
             # get joint torques from the middleware
             tau = self.middleware.get_joint_torques(body_id, joint_ids)
             if tau is None:  # if we didn't get the joint torques from the middleware, get them from the simulator
@@ -1952,7 +2002,7 @@ class Simulator(object):
             tau = self._get_joint_torques(body_id, joint_ids)
 
             # if the middleware is set on the teleoperation mode, publish the joint torques through the middleware
-            if self.middleware is not None:
+            if self.middleware is not None and self._middleware_enabled:
                 self.middleware.set_joint_torques(body_id, joint_ids, tau, check_teleoperate=True)
 
         return tau

pyrobolearn/utils/parsers/robots/data_structures.py

@@ -3477,7 +3477,7 @@ class GPURay(LinkSensor):
         if angle is not None:
             angle = float(angle)
             if self.range_angle is None:
-                self.range_angle = (None, None)
+                self.range_angle = [None, None]
             self.range_angle[0] = angle
 
     @property
@@ -3490,7 +3490,7 @@ class GPURay(LinkSensor):
         if angle is not None:
             angle = float(angle)
             if self.range_angle is None:
-                self.range_angle = (None, None)
+                self.range_angle = [None, None]
             self.range_angle[1] = angle
 
     @property
@@ -3503,7 +3503,7 @@ class GPURay(LinkSensor):
         if dist is not None:
             dist = float(dist)
             if self.range is None:
-                self.range = (None, None)
+                self.range = [None, None]
             self.range[0] = dist
 
     @property
@@ -3516,7 +3516,7 @@ class GPURay(LinkSensor):
         if dist is not None:
             dist = float(dist)
             if self.range is None:
-                self.range = (None, None)
+                self.range = [None, None]
             self.range[1] = dist
 
     @property