From 257df549ed564421d04eb7d37a46a9d6b857d706 Mon Sep 17 00:00:00 2001 From: Albert Hofkamp Date: Tue, 10 Mar 2020 21:03:06 +0100 Subject: [PATCH 1/3] Move handover_to_human and handover_to_robot functions. --- robot_skills/src/robot_skills/arms.py | 80 +++++++++++++++++++-------- 1 file changed, 56 insertions(+), 24 deletions(-) diff --git a/robot_skills/src/robot_skills/arms.py b/robot_skills/src/robot_skills/arms.py index 7843a8bcc5..24a1da1da6 100644 --- a/robot_skills/src/robot_skills/arms.py +++ b/robot_skills/src/robot_skills/arms.py @@ -330,6 +330,58 @@ class GripperState(object): CLOSE = "close" +class ArmHandover(object): + """ + :var robot_name: Name of the robot. + :var arm_name: Name of the arm (typically 'left' or 'right'). + """ + def __init__(self, robot_name, arm_name): + self.robot_name = robot_name + self.arm_name = arm_name + + def handover_to_human(self, timeout=10): + """ + Handover an item from the gripper to a human. + + Feels if user slightly pulls or pushes (the item in) the arm. On timeout, it will return False. + :param timeout: timeout in seconds + :return: True or False + """ + pub = rospy.Publisher('/'+self.robot_name+'/handoverdetector_'+self.arm_name+'/toggle_robot2human', + std_msgs.msg.Bool, queue_size=1, latch=True) + pub.publish(std_msgs.msg.Bool(True)) + + try: + rospy.wait_for_message('/'+self.robot_name+'/handoverdetector_'+self.arm_name+'/result', std_msgs.msg.Bool, + timeout) + # print('/'+self.robot_name+'/handoverdetector_'+self.side+'/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. + + Feels if user slightly pushes an item in the gripper. On timeout, it will return False. + :param timeout: timeout in seconds + :return: True or False + """ + pub = rospy.Publisher('/'+self.robot_name+'/handoverdetector_'+self.arm_name+'/toggle_human2robot', + std_msgs.msg.Bool, queue_size=1, latch=True) + pub.publish(std_msgs.msg.Bool(True)) + + try: + rospy.wait_for_message('/'+self.robot_name+'/handoverdetector_'+self.arm_name+'/result', std_msgs.msg.Bool, + timeout) + # print('/'+self.robot_name+'/handoverdetector_'+self.side+'/result') + return True + except rospy.ROSException as e: + rospy.logerr(e) + return False + + class Arm(RobotPart): """ A single arm can be either left or right, extends Arms: @@ -404,6 +456,8 @@ def __init__(self, robot_name, tf_listener, get_joint_states, side): self.get_joint_states = get_joint_states + self._handover = ArmHandover(self.robot_name, self.side) + def collect_gripper_types(self, gripper_type): """ Query the arm for having the proper gripper type and collect the types that fulfill the @@ -691,18 +745,7 @@ 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)) - - 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') - return True - except rospy.ROSException as e: - rospy.logerr(e) - return False + return self._handover.handover_to_human(timeout) def handover_to_robot(self, timeout=10): """ @@ -712,18 +755,7 @@ 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)) - - 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') - return True - except rospy.ROSException as e: - rospy.logerr(e) - return False + self._handover.handover_to_robot(timeout) def _send_joint_trajectory(self, joints_references, max_joint_vel=0.7, timeout=rospy.Duration(5)): """ From 1a79e100dc8af99d7089d49e3d1dbdd45b567889 Mon Sep 17 00:00:00 2001 From: Albert Hofkamp Date: Tue, 10 Mar 2020 21:14:59 +0100 Subject: [PATCH 2/3] Deduplicate handover function. --- robot_skills/src/robot_skills/arms.py | 28 ++++++++++++--------------- 1 file changed, 12 insertions(+), 16 deletions(-) diff --git a/robot_skills/src/robot_skills/arms.py b/robot_skills/src/robot_skills/arms.py index 24a1da1da6..e1c5974310 100644 --- a/robot_skills/src/robot_skills/arms.py +++ b/robot_skills/src/robot_skills/arms.py @@ -347,18 +347,7 @@ def handover_to_human(self, timeout=10): :param timeout: timeout in seconds :return: True or False """ - pub = rospy.Publisher('/'+self.robot_name+'/handoverdetector_'+self.arm_name+'/toggle_robot2human', - std_msgs.msg.Bool, queue_size=1, latch=True) - pub.publish(std_msgs.msg.Bool(True)) - - try: - rospy.wait_for_message('/'+self.robot_name+'/handoverdetector_'+self.arm_name+'/result', std_msgs.msg.Bool, - timeout) - # print('/'+self.robot_name+'/handoverdetector_'+self.side+'/result') - return True - except rospy.ROSException as e: - rospy.logerr(e) - return False + return self._exchange_with_human(False, timeout) def handover_to_robot(self, timeout=10): """ @@ -368,13 +357,20 @@ def handover_to_robot(self, timeout=10): :param timeout: timeout in seconds :return: True or False """ - pub = rospy.Publisher('/'+self.robot_name+'/handoverdetector_'+self.arm_name+'/toggle_human2robot', - std_msgs.msg.Bool, queue_size=1, latch=True) + return self._exchange_with_human(True, timeout) + + def _exchange_with_human(self, to_robot, timeout=10): + if to_robot: + topic = '/{}/handoverdetector_{}/toggle_human2robot'.format(self.robot_name, self.arm_name) + else: + topic = '/{}/handoverdetector_{}/toggle_robot2human'.format(self.robot_name, self.arm_name) + + pub = rospy.Publisher(topic, std_msgs.msg.Bool, queue_size=1, latch=True) pub.publish(std_msgs.msg.Bool(True)) + result_topic = '/{}/handoverdetector_{}/result'.format(self.robot_name, self.arm_name) try: - rospy.wait_for_message('/'+self.robot_name+'/handoverdetector_'+self.arm_name+'/result', std_msgs.msg.Bool, - timeout) + rospy.wait_for_message(result_topic, std_msgs.msg.Bool, timeout) # print('/'+self.robot_name+'/handoverdetector_'+self.side+'/result') return True except rospy.ROSException as e: From 0bf4d60ccd5b487529d36cb1eec523d004f07d0d Mon Sep 17 00:00:00 2001 From: Albert Hofkamp Date: Tue, 10 Mar 2020 21:18:15 +0100 Subject: [PATCH 3/3] Add Python2 typing hints --- robot_skills/src/robot_skills/arms.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/robot_skills/src/robot_skills/arms.py b/robot_skills/src/robot_skills/arms.py index e1c5974310..aeeca71666 100644 --- a/robot_skills/src/robot_skills/arms.py +++ b/robot_skills/src/robot_skills/arms.py @@ -332,6 +332,8 @@ class GripperState(object): class ArmHandover(object): """ + Handover part of the arm functionality. + :var robot_name: Name of the robot. :var arm_name: Name of the arm (typically 'left' or 'right'). """ @@ -340,6 +342,7 @@ def __init__(self, robot_name, arm_name): self.arm_name = arm_name def handover_to_human(self, timeout=10): + # type: (float) -> bool """ Handover an item from the gripper to a human. @@ -350,6 +353,7 @@ def handover_to_human(self, timeout=10): return self._exchange_with_human(False, timeout) def handover_to_robot(self, timeout=10): + # type: (float) -> bool """ Handover an item from a human to the robot. @@ -360,6 +364,7 @@ def handover_to_robot(self, timeout=10): return self._exchange_with_human(True, timeout) def _exchange_with_human(self, to_robot, timeout=10): + # type: (bool, float) -> bool if to_robot: topic = '/{}/handoverdetector_{}/toggle_human2robot'.format(self.robot_name, self.arm_name) else: @@ -734,6 +739,7 @@ def send_gripper_goal(self, state, timeout=5.0, max_torque=0.1): return goal_status == GoalStatus.SUCCEEDED def handover_to_human(self, timeout=10): + # type: (float) -> bool """ Handover an item from the gripper to a human. @@ -744,6 +750,7 @@ def handover_to_human(self, timeout=10): return self._handover.handover_to_human(timeout) def handover_to_robot(self, timeout=10): + # type: (float) -> bool """ Handover an item from a human to the robot.