Purge 'side' parameter

robot_skills/CMakeLists.txt

@@ -37,4 +37,5 @@ if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   catkin_add_nosetests(test/nosetest)
   add_rostest(test/rostest/test_compose_robot.test)
+  add_rostest(test/rostest/test_hero.test)
 endif()

robot_skills/package.xml

@@ -47,6 +47,7 @@
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-mock</exec_depend>
 
   <test_depend>test_tools</test_depend>
+  <test_depend>hero_description</test_depend>
   <test_depend condition="$ROS_PYTHON_VERSION == 2">python-six</test_depend>
   <test_depend condition="$ROS_PYTHON_VERSION == 3">python3-six</test_depend>
 

robot_skills/src/robot_skills/arm/arms.py

@@ -262,67 +262,64 @@ def _test_die(self, cond, feature, hint=''):
         """
         if not cond:
             msg = "get_arm for '{}' arm did not request '{}' access. Hint: {}"
-            raise AssertionError(msg.format(self._arm.side, feature, hint))
+            raise AssertionError(msg.format(self._arm.name, feature, hint))
 
     def __repr__(self):
         return "PublicArm(arm={arm})".format(arm=self._arm)
 
+
 class Arm(RobotPart):
     """
-    A single arm can be either left or right, extends Arms:
-    Use left or right to get arm while running from the python console
-
-    Examples:
-    >>> left.send_goal(0.265, 1, 0.816, 0, 0, 0, 60)  # doctest: +SKIP
-    or Equivalently:
-    >>> left.send_goal(px=0.265, py=1, pz=0.816, yaw=0, pitch=0, roll=0, time_out=60, pre_grasp=False, frame_id='/amigo/base_link')  # doctest: +SKIP
+    A kinematic chain ending in an end_effector. Can be controlled using either joint goals or a goal to reach with
+    the end_effector described in carthesian coordinates.
     """
-    def __init__(self, robot_name, tf_listener, get_joint_states, side):
+    def __init__(self, robot_name, tf_listener, get_joint_states, name):
         """
         constructor
 
         :param robot_name: robot_name
         :param tf_listener: tf_server.TFClient()
         :param get_joint_states: get_joint_states function for getting the last joint states
-        :param side: left or right
+        :param name: string used to identify the arm
         """
         super(Arm, self).__init__(robot_name=robot_name, tf_listener=tf_listener)
-        self.side = side
+        self.name = name
 
         self._operational = True  # In simulation, there will be no hardware cb
 
         # Get stuff from the parameter server
-        offset = self.load_param('skills/arm/' + self.side + '/grasp_offset/')
+        offset = self.load_param('skills/gripper/grasp_offset/')
         self.offset = kdl.Frame(kdl.Rotation.RPY(offset["roll"], offset["pitch"], offset["yaw"]),
                                 kdl.Vector(offset["x"], offset["y"], offset["z"]))
 
-        self.marker_to_grippoint_offset = self.load_param('skills/arm/' + self.side + '/marker_to_grippoint')
+        self.marker_to_grippoint_offset = self.load_param('skills/gripper/marker_to_grippoint')
 
         # Grasp offsets
-        go = self.load_param('skills/arm/' + self.side + '/base_offset')
+        go = self.load_param('skills/' + self.name + '/base_offset')
         self._base_offset = kdl.Vector(go["x"], go["y"], go["z"])
 
-        self.joint_names = self.load_param('skills/arm/joint_names')
-        self.joint_names = [name + "_" + self.side for name in self.joint_names]
+        self.joint_names = self.load_param('skills/' + self.name + '/joint_names')
         self.torso_joint_names = self.load_param('skills/torso/joint_names')
 
-        self.default_configurations = self.load_param('skills/arm/default_configurations')
-        self.default_trajectories   = self.load_param('skills/arm/default_trajectories')
+        self.default_configurations = self.load_param('skills/' + self.name + '/default_configurations')
+        self.default_trajectories = self.load_param('skills/' + self.name + '/default_trajectories')
+
+        self.grasp_frame = self.load_param('skills/gripper/grasp_frame')  #TODO remove gripper specific parameters
 
         # listen to the hardware status to determine if the arm is available
-        self.subscribe_hardware_status(self.side + '_arm')
+        self.subscribe_hardware_status(self.name)
 
         # Init grasp precompute actionlib
         self._ac_grasp_precompute = self.create_simple_action_client(
-            "/" + robot_name + "/" + self.side + "_arm/grasp_precompute", GraspPrecomputeAction)
+            "/" + robot_name + "/" + self.name + "/grasp_precompute", GraspPrecomputeAction)
 
         # Init joint trajectory action server
         self._ac_joint_traj = self.create_simple_action_client(
             "/" + robot_name + "/body/joint_trajectory_action", FollowJointTrajectoryAction)
 
         # Init marker publisher
         self._marker_publisher = rospy.Publisher(
-            "/" + robot_name + "/" + self.side + "_arm/grasp_target",
+            "/" + robot_name + "/" + self.name + "/grasp_target",
             visualization_msgs.msg.Marker, queue_size=10)
 
         self.get_joint_states = get_joint_states
@@ -397,10 +394,10 @@ def close(self):
         :return: no return
         """
         try:
-            rospy.loginfo("{0} arm cancelling all goals on all arm-related ACs on close".format(self.side))
+            rospy.loginfo("{0} arm cancelling all goals on all arm-related ACs on close".format(self.name))
         except AttributeError:
             print("{0} arm cancelling all goals on all arm-related ACs on close. rospy is already deleted.".
-                  format(self.side))
+                  format(self.name))
 
         self._ac_grasp_precompute.cancel_all_goals()
         self._ac_joint_traj.cancel_all_goals()
@@ -494,7 +491,7 @@ def send_goal(self, frameStamped, timeout=30, pre_grasp=False, first_joint_pos_o
             if result == GoalStatus.SUCCEEDED:
 
                 result_pose = self.tf_listener.lookupTransform(self.robot_name + "/base_link",
-                                                               self.robot_name + "/grippoint_{}".format(self.side),
+                                                               self.robot_name + "/" + self.grasp_frame,
                                                                rospy.Time(0))
                 dx = grasp_precompute_goal.goal.x - result_pose[0][0]
                 dy = grasp_precompute_goal.goal.y - result_pose[0][1]
@@ -603,7 +600,7 @@ def _send_joint_trajectory(self, joints_references, max_joint_vel=0.7, timeout=r
                                            points=ps)
         goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory, goal_time_tolerance=timeout)
 
-        rospy.logdebug("Send {0} arm to jointcoords \n{1}".format(self.side, ps))
+        rospy.logdebug("Send {0} arm to jointcoords \n{1}".format(self.name, ps))
 
         import time; time.sleep(0.001)  # This is necessary: the rtt_actionlib in the hardware seems
                                         # to only have a queue size of 1 and runs at 1000 hz. This
@@ -690,4 +687,4 @@ def _publish_marker(self, goal, color, ns=""):
         self._marker_publisher.publish(marker)
 
     def __repr__(self):
-        return "Arm(side='{side}')".format(side=self.side)
+        return "Arm(name='{}')".format(self.name)

robot_skills/src/robot_skills/arm/grasp_sensor.py

@@ -1,3 +1,7 @@
+import rospy
+
+from std_msgs.msg import Float32MultiArray
+
 from robot_skills.robot_part import RobotPart
 
 
@@ -85,20 +89,18 @@ class GraspSensor(RobotPart):
     """
     Sensor to detect whether or not the robot is holding an object.
     """
-
-    def __init__(self, robot_name, tf_listener, side):
+    def __init__(self, robot_name, tf_listener, sensor_topic):
         """
         constructor
 
         :param robot_name: robot_name
         :param tf_listener: tf_server.TFClient()
-        :param side: left or right arm of the robot.
+        :param sensor_topic: name of the topic where measurements are published
         """
         super(GraspSensor, self).__init__(robot_name=robot_name, tf_listener=tf_listener)
         # Init grasp sensor subscriber
         self._grasp_sensor_state = GripperMeasurement(0.0)
-        rospy.Subscriber("/" + robot_name + "/" + side + "_arm/proximity_sensor",
-                         std_msgs.msg.Float32MultiArray, self._grasp_sensor_callback)
+        rospy.Subscriber(sensor_topic, Float32MultiArray, self._grasp_sensor_callback)
 
     @property
     def object_in_gripper_measurement(self):

robot_skills/src/robot_skills/arm/gripper.py

@@ -69,7 +69,7 @@ def __init__(self, robot_name, tf_listener, gripper_name):
         """
         super(ParrallelGripper, self).__init__(robot_name=robot_name, tf_listener=tf_listener)
         self.gripper_name = gripper_name
-        offset = self.load_param('skills/arm/' + self.gripper_name + '/grasp_offset/')
+        offset = self.load_param('skills/' + self.gripper_name + '/grasp_offset/')
         self.offset = kdl.Frame(kdl.Rotation.RPY(offset["roll"], offset["pitch"], offset["yaw"]),
                                         kdl.Vector(offset["x"], offset["y"], offset["z"]))
 

robot_skills/src/robot_skills/arm/handover_detector.py

@@ -1,6 +1,7 @@
 from __future__ import print_function
 
 import rospy
+from std_msgs.msg import Bool
 
 from robot_skills.robot_part import RobotPart
 
@@ -9,16 +10,16 @@ class HandoverDetector(RobotPart):
     """
     Sensor functionality to detect when a handover is taking place
     """
-    def __init__(self, robot_name, tf_listener, side):
+    def __init__(self, robot_name, tf_listener, name):
         """
         constructor
 
         :param robot_name: robot_name
         :param tf_listener: tf_server.TFClient()
-        :param side: string used to identify the sensor
+        :param name: string used to identify the sensor
         """
         super(HandoverDetector, self).__init__(robot_name=robot_name, tf_listener=tf_listener)
-        self.side = side
+        self.name = name
 
     def handover_to_human(self, timeout=10):
         """
@@ -28,20 +29,18 @@ def handover_to_human(self, timeout=10):
         :param timeout: timeout in seconds
         :return: True or False
         """
-        pub = rospy.Publisher('/' + self.robot_name + '/handoverdetector_' + self.side + '/toggle_robot2human',
-                              std_msgs.msg.Bool, queue_size=1, latch=True)
-        pub.publish(std_msgs.msg.Bool(True))
+        pub = rospy.Publisher('/' + self.robot_name + '/' + self.name + '/toggle_robot2human', Bool, queue_size=1,
+                              latch=True)
+        pub.publish(Bool(True))
 
         try:
-            rospy.wait_for_message('/' + self.robot_name + '/handoverdetector_' + self.side + '/result', std_msgs.msg.Bool,
-                                   timeout)
-            # print('/'+self.robot_name+'/handoverdetector_'+self.side+'/result')
+            rospy.wait_for_message('/' + self.robot_name + '/' + self.name + '/result', Bool, timeout)
+            # print('/'+self.robot_name+'/'+self.name+'/result')
             return True
         except rospy.ROSException as e:
             rospy.logerr(e)
             return False
 
-
     def handover_to_robot(self, timeout=10):
         """
         Handover an item from a human to the robot.
@@ -50,14 +49,13 @@ def handover_to_robot(self, timeout=10):
         :param timeout: timeout in seconds
         :return: True or False
         """
-        pub = rospy.Publisher('/' + self.robot_name + '/handoverdetector_' + self.side + '/toggle_human2robot',
-                              std_msgs.msg.Bool, queue_size=1, latch=True)
-        pub.publish(std_msgs.msg.Bool(True))
+        pub = rospy.Publisher('/' + self.robot_name + '/' + self.name + '/toggle_human2robot', Bool, queue_size=1,
+                              latch=True)
+        pub.publish(Bool(True))
 
         try:
-            rospy.wait_for_message('/' + self.robot_name + '/handoverdetector_' + self.side + '/result', std_msgs.msg.Bool,
-                                   timeout)
-            # print('/'+self.robot_name+'/handoverdetector_'+self.side+'/result')
+            rospy.wait_for_message('/' + self.robot_name + '/' + self.name + '/result', Bool, timeout)
+            # print('/'+self.robot_name+'/'+self.name+'/result')
             return True
         except rospy.ROSException as e:
             rospy.logerr(e)

robot_skills/src/robot_skills/hero.py

@@ -23,10 +23,10 @@ def __init__(self, wait_services=False):
         self.add_body_part('torso', torso.Torso(self.robot_name, self.tf_listener, self.get_joint_states))
 
         # add hero's arm
-        hero_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "left")
+        hero_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "arm_center")
         hero_arm.add_part('force_sensor', force_sensor.ForceSensor(self.robot_name, self.tf_listener, "/" + self.robot_name + "/wrist_wrench/raw"))
-        hero_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'left'))
-        hero_arm.add_part('handover_detector', handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'left'))
+        hero_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'gripper'))
+        hero_arm.add_part('handover_detector', handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'handover_detector'))
 
         self.add_arm_part('leftArm', hero_arm)
 
@@ -55,9 +55,6 @@ def __init__(self, wait_services=False):
         # Reasoning/world modeling
         self.add_body_part('ed', world_model_ed.ED(self.robot_name, self.tf_listener))
 
-        #rename joint names
-        self.parts['leftArm'].joint_names = self.parts['leftArm'].load_param('skills/arm/joint_names')
-
         # These don't work for HSR because (then) Toyota's diagnostics aggregator makes the robot go into error somehow
         for part in self.parts.values():
             part.unsubscribe_hardware_status()

robot_skills/src/robot_skills/mockbot.py

@@ -126,10 +126,10 @@ def __init__(self, robot_name, tf_listener, *args, **kwargs):
 
 
 class Arm(MockedRobotPart):
-    def __init__(self, robot_name, tf_listener, get_joint_states, side):
+    def __init__(self, robot_name, tf_listener, get_joint_states, name):
         super(Arm, self).__init__(robot_name, tf_listener)
 
-        self.side = side
+        self.arm_name = name
         self.get_joint_states = get_joint_states
 
         self._operational = True
@@ -417,8 +417,8 @@ def __init__(self, *args, **kwargs):
         self.add_body_part('base', Base(self.robot_name, self.tf_listener))
         self.add_body_part('torso', Torso(self.robot_name, self.tf_listener, self.get_joint_states))
 
-        self.add_arm_part('leftArm', Arm(self.robot_name, self.tf_listener, self.get_joint_states, "left"))
-        self.add_arm_part('rightArm', Arm(self.robot_name, self.tf_listener, self.get_joint_states, "right"))
+        self.add_arm_part('leftArm', Arm(self.robot_name, self.tf_listener, self.get_joint_states, "arm_left"))
+        self.add_arm_part('rightArm', Arm(self.robot_name, self.tf_listener, self.get_joint_states, "arm_right"))
 
         self.add_body_part('head', Head(self.robot_name, self.tf_listener))
 

robot_skills/src/robot_skills/torso.py

@@ -24,12 +24,16 @@ def __init__(self, robot_name, tf_listener, get_joint_states):
         super(Torso, self).__init__(robot_name=robot_name, tf_listener=tf_listener)
 
         self.joint_names = self.load_param('skills/torso/joint_names')
-        self._arm_joint_names = self.load_param('skills/arm/joint_names')
         self.default_configurations = self.load_param('skills/torso/default_configurations')
         self.default_tolerance = self.load_param('/skills/torso/default_tolerance')
         self.lower_limit = self.default_configurations['lower_limit']
         self.upper_limit = self.default_configurations['upper_limit']
 
+        try:  # TODO make sure this is no longer needed
+            self._arm_joint_names = self.load_param('skills/arm_center/joint_names')
+        except KeyError:
+            self._arm_joint_names = []
+
         # Init action client
         self.ac_move_torso = self.create_simple_action_client('/' + self.robot_name + '/body/joint_trajectory_action',
                                                               control_msgs.msg.FollowJointTrajectoryAction)

robot_skills/test/rostest/test_compose_robot.py

@@ -40,11 +40,6 @@ def testfunction2(self):
 
 
 class TestComposeRobot(unittest.TestCase):
-
-    @classmethod
-    def setUpClass(cls):
-        rospy.init_node('test_composable_robot_interface2')
-
     def test_compose_robot(self):
         robot = Robot("testbot")
         part = RobotPart("testbot", robot.tf_listener)
@@ -68,6 +63,8 @@ def test_compose_functionality(self):
 
         self.assertFalse(hasattr(robot.special_part, 'testfunction2'))
 
+
 if __name__ == '__main__':
     import rostest
+    rospy.init_node('test_composable_robot_interface')
     rostest.rosrun('robot_skills', 'test_composable_robot_interface', TestComposeRobot)

robot_skills/test/rostest/test_hero.py

@@ -0,0 +1,19 @@
+#! /usr/bin/env python
+
+import rospy
+import unittest
+
+# robot skills
+from robot_skills.hero import Hero
+
+
+class TestHeroInterface(unittest.TestCase):
+    def test_construct_interface(self):
+        robot = Hero()
+        self.assertIsNotNone(robot)
+
+
+if __name__ == '__main__':
+    import rostest
+    rospy.init_node('test_hero')
+    rostest.rosrun('robot_skills', 'test_hero_interface', TestHeroInterface)

robot_skills/test/rostest/test_hero.test

@@ -0,0 +1,5 @@
+<?xml version="1.0" ?>
+<launch>
+    <rosparam file="$(find hero_description)/custom/skills.yaml" command="load" ns='hero'/>
+    <test test-name="test_hero_interface" pkg="robot_skills" type="test_hero.py" />
+</launch>