Merge pull request #1083 from tue-robotics/fix/side_param

Purge 'side' parameter

amigo_skills/src/amigo_skills/amigo.py

@@ -22,17 +22,17 @@ def __init__(self, wait_services=False):
         self.add_body_part('torso', torso.Torso(self.robot_name, self.tf_listener, self.get_joint_states))
 
         # construct left arm
-        left_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "left")
-        left_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'left'))
+        left_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "arm_left")
+        left_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'gripper_left'))
         left_arm.add_part('handover_detector',
-                          handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'left'))
+                          handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'handoverdetector_left'))
         self.add_arm_part('leftArm', left_arm)
 
         # construct right arm
-        right_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "right")
-        right_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'right'))
+        right_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "arm_right")
+        right_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_listener, 'gripper_right'))
         right_arm.add_part('handover_detector',
-                           handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'right'))
+                           handover_detector.HandoverDetector(self.robot_name, self.tf_listener, 'handoverdetector_right'))
         self.add_arm_part('rightArm', right_arm)
 
         self.add_body_part('head', head.Head(self.robot_name, self.tf_listener))

hero_skills/src/hero_skills/hero.py

@@ -23,17 +23,15 @@ def __init__(self, wait_services=False):
         self._ignored_parts = ["leftArm", "torso", "spindle", "head"]
 
         self.add_body_part('base', base.Base(self.robot_name, self.tf_listener))
-        self.add_body_part('torso', torso.Torso(self.robot_name, self.tf_listener, self.get_joint_states))
+
+        arm_joint_names = rospy.get_param('/' + self.robot_name + '/skills/arm_center/joint_names')
+        self.add_body_part('torso', torso.Torso(self.robot_name, self.tf_listener, self.get_joint_states, arm_joint_names))
 
         # add hero's arm
-        hero_arm = arms.Arm(self.robot_name, self.tf_listener, self.get_joint_states, "left")
-        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 = 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, '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)
 
@@ -61,9 +59,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/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,13 +69,13 @@ 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"]))
 
         # Init gripper actionlib
-        self._ac_gripper = self.create_simple_action_client(
-            "/" + robot_name + "/" + self.gripper_name + "_arm/gripper/action", GripperCommandAction)
+        ac_name = self.load_param('skills/' + self.gripper_name + '/ac_gripper')
+        self._ac_gripper = self.create_simple_action_client(ac_name, GripperCommandAction)
 
     def send_goal(self, state, timeout=5.0, max_torque=0.1): #Todo: should send goal be universal for all grippers?
         """

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/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

@@ -14,7 +14,7 @@
 
 
 class Torso(RobotPart):
-    def __init__(self, robot_name, tf_listener, get_joint_states):
+    def __init__(self, robot_name, tf_listener, get_joint_states, arm_joint_names=None):
         """
         constructor
 
@@ -24,12 +24,13 @@ 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']
 
+        self._arm_joint_names = [] if arm_joint_names is None else arm_joint_names  #TODO make sure this is no longer needed
+
         # 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

@@ -45,11 +45,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)
@@ -76,4 +71,5 @@ def test_compose_functionality(self):
 
 if __name__ == '__main__':
     import rostest
+    rospy.init_node('test_composable_robot_interface')
     rostest.rosrun('robot_skills', 'test_composable_robot_interface', TestComposeRobot)