pyrobolearn/pyrobolearn/simulators/ros.py

# -*- coding: utf-8 -*-
#!/usr/bin/env python
"""Define the ROS API.

Dependencies in PRL:
* `pyrobolearn.simulators.simulator.Simulator`
"""

# TODO
import os
import subprocess
import psutil
import signal
import importlib
import inspect

from pyrobolearn.simulators.simulator import Simulator


__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["ROS (Willow Garage)", "Brian Delhaisse"]
__license__ = "GNU GPLv3"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"


# TODO: maybe I should inherit from MiddleWare instead of Simulator... Then we can give these MiddleWare to different
#  simulators. Other communication middleware layer includes YARP, etc.

class ROS(Simulator):
    r"""ROS Interface
    """

    def __init__(self, subscribe=False, publish=False, teleoperate=False, master_uri=11311, **kwargs):
        super(ROS, self).__init__(render=False)

        # Environment variable
        self.env = os.environ.copy()
        self.env["ROS_MASTER_URI"] = "http://localhost:" + str(master_uri)

        # this is for the rospy methods such as: wait_for_service(), init_node(), ...
        os.environ['ROS_MASTER_URI'] = self.env['ROS_MASTER_URI']

        # run ROS core if not already running
        self.roscore = None
        if "roscore" not in [p.name() for p in psutil.process_iter()]:
            # subprocess.Popen("roscore", env=self.env)
            self.roscore = subprocess.Popen(["roscore", "-p", str(master_uri)], env=self.env,
                                            preexec_fn=os.setsid)  # , shell=True)

        # set variables
        self.subscribe = subscribe
        self.publish = publish
        self.teleoperate = teleoperate

        # remember each publisher/subscriber
        self.subscribers = {}
        self.publishers = {}
        self.models = []

        self.count_id = -1

    def close(self):
        """
        Close everything
        """
        # delete each subscribers

        # delete each publishers

        # delete ROS
        if self.roscore is not None:
            os.killpg(os.getpgid(self.roscore.pid), signal.SIGTERM)

    @property
    def is_subscribing(self):
        """Return True if we are subscribing to topics."""
        return self.subscribe

    @property
    def is_publishing(self):
        """Return True if we are publishing to topics."""
        return self.publish

    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.

        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:
            filename (str): a relative or absolute path to the URDF file on the file system of the physics server.
            position (vec3): create the base of the object at the specified position in world space coordinates [x,y,z]
            orientation (quat): create the base of the object at the specified orientation as world space quaternion
                [x,y,z,w]
            use_maximal_coordinates (int): Experimental. By default, the joints in the URDF file are created using the
                reduced coordinate method: the joints are simulated using the Featherstone Articulated Body algorithm
                (btMultiBody in Bullet 2.x). The useMaximalCoordinates option will create a 6 degree of freedom rigid
                body for each link, and constraints between those rigid bodies are used to model joints.
            use_fixed_base (bool): force the base of the loaded object to be static
            flags (int): URDF_USE_INERTIA_FROM_FILE (val=2): by default, Bullet recomputed the inertia tensor based on
                mass and volume of the collision shape. If you can provide more accurate inertia tensor, use this flag.
                URDF_USE_SELF_COLLISION (val=8): by default, Bullet disables self-collision. This flag let's you
                enable it.
                You can customize the self-collision behavior using the following flags:
                    * URDF_USE_SELF_COLLISION_EXCLUDE_PARENT (val=16) will discard self-collision between links that
                        are directly connected (parent and child).
                    * URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS (val=32) will discard self-collisions between a
                        child link and any of its ancestors (parents, parents of parents, up to the base).
                    * URDF_USE_IMPLICIT_CYLINDER (val=128), will use a smooth implicit cylinder. By default, Bullet
                        will tessellate the cylinder into a convex hull.
            scale (float): scale factor to the URDF model.

        Returns:
            int (non-negative): unique id associated to the load model.
        """
        id_ = self.count_id
        self.count_id += 1

        # get path to directory of urdf
        path = os.path.dirname(os.path.abspath(filename))           # /path/to/pyrobolearn/robots/urdfs/<robot>/
        robot_directory_name = path.split('/')[-1]                  # <robot>
        path = path + '/../../ros/' + robot_directory_name + '/'    # /path/to/pyrobolearn/robots/ros/<robot>/

        # check if valid robot directory
        # if os.path.isdir(path):
        robot_path = '/'.join(path.split('/')[-5:-2])
        if robot_path == 'pyrobolearn/robots/urdfs':

            # get corresponding subscriber/publisher
            def check_ros(name, dictionary, id_):
                # TODO: do I really need a class for each robot? Can I not just use RobotPublisher?
                # if os.path.isfile(path + name + '.py'):
                #     module = importlib.import_module('pyrobolearn.robots.ros.' + robot_directory_name + '.' + name)
                #     classes = inspect.getmembers(module, inspect.isclass)  # list of (name, class)
                #     length = len(name)
                #     robot_name = ''.join(robot_directory_name.split('_'))
                #
                #     # go through each class and get the :attr:`name` corresponding to the robot and add it to the
                #     # given :attr:`dictionary`
                #     for name, cls in classes:
                #         if name[:-length].lower() == robot_name:
                #             dictionary[id_] = cls(id_=id_)
                #             break
                module = importlib.import_module('pyrobolearn.robots.ros.' + name)
                classes = dict(inspect.getmembers(module, inspect.isclass))
                cls = classes['Robot' + name.capitalize()]
                dictionary[id_] = cls(name=robot_directory_name, id_=id_)

            # load subscriber in simulator
            if self.subscribe:
                check_ros('subscriber', self.subscribers, id_)

            # load publisher in simulator
            if self.publish:
                check_ros('publisher', self.publishers, id_)

        return id_

    def get_joint_positions(self, body_id, joint_ids):
        """
        Get the position of the given joint(s).

        Args:
            body_id (int): unique body id.
            joint_ids (int, list of int): joint id, or list of joint ids.

        Returns:
            if 1 joint:
                float: joint position [rad]
            if multiple joints:
                np.float[N]: joint positions [rad]
        """
        if body_id in self.subscribers:
            return self.subscribers[body_id].get_joint_positions(joint_ids)

    def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None):
        """
        Set the position of the given joint(s) (using position control).

        Args:
            body_id (int): unique body id.
            joint_ids (int, list of int): joint id, or list of joint ids.
            positions (float, np.float[N]): desired position, or list of desired positions [rad]
            velocities (None, float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
            kps (None, float, np.float[N]): position gain(s)
            kds (None, float, np.float[N]): velocity gain(s)
            forces (None, float, np.float[N]): maximum motor force(s)/torque(s) used to reach the target values.
        """
        if body_id in self.publishers:
            self.publishers[body_id].set_joint_positions(joint_ids, positions)
            self.publishers[body_id].publish('joint_states')

    def get_joint_velocities(self, body_id, joint_ids):
        """
        Get the velocity of the given joint(s).

        Args:
            body_id (int): unique body id.
            joint_ids (int, list of int): joint id, or list of joint ids.

        Returns:
            if 1 joint:
                float: joint velocity [rad/s]
            if multiple joints:
                np.float[N]: joint velocities [rad/s]
        """
        if body_id in self.subscribers:
            return self.subscribers[body_id].get_joint_velocities(joint_ids)

    def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
        """
        Set the velocity of the given joint(s) (using velocity control).

        Args:
            body_id (int): unique body id.
            joint_ids (int, list of int): joint id, or list of joint ids.
            velocities (float, np.float[N]): desired velocity, or list of desired velocities [rad/s]
            max_force (None, float, np.float[N]): maximum motor forces/torques
        """
        if body_id in self.publishers:
            self.publishers[body_id].set_joint_velocities(joint_ids, velocities)
            self.publishers[body_id].publish('joint_states')

    def get_joint_torques(self, body_id, joint_ids):
        """
        Get the applied torque(s) on the given joint(s). "This is the motor torque applied during the last `step`.
        Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the
        applied joint motor torque is exactly what you provide, so there is no need to report it separately." [1]

        Args:
            body_id (int): unique body id.
            joint_ids (int, list of int): a joint id, or list of joint ids.

        Returns:
            if 1 joint:
                float: torque [Nm]
            if multiple joints:
                np.float[N]: torques associated to the given joints [Nm]
        """
        if body_id in self.subscribers:
            return self.subscribers[body_id].get_joint_torques(joint_ids)

    def set_joint_torques(self, body_id, joint_ids, torques):
        """
        Set the torque/force to the given joint(s) (using force/torque control).

        Args:
            body_id (int): unique body id.
            joint_ids (int, list of int): joint id, or list of joint ids.
            torques (float, list of float): desired torque(s) to apply to the joint(s) [N].
        """
        if body_id in self.publishers:
            self.publishers[body_id].set_joint_torques(joint_ids, torques)
            self.publishers[body_id].publish('joint_states')