Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Move arm handover #1023

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Move arm handover #1023

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
257df54
Move handover_to_human and handover_to_robot functions.
alberth Mar 10, 2020
1a79e10
Deduplicate handover function.
alberth Mar 10, 2020
0bf4d60
Add Python2 typing hints
alberth Mar 10, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
83 changes: 59 additions & 24 deletions robot_skills/src/robot_skills/arms.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -330,6 +330,59 @@ class GripperState(object):
CLOSE = "close"


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').
"""
def __init__(self, robot_name, arm_name):
self.robot_name = robot_name
self.arm_name = arm_name

def handover_to_human(self, timeout=10):
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The definitions handover_to_human and handover_to_robot are now defined twice (here and in the arm class). I think they can be removed here if the functionality (return exchange_with_human) is shifted to the arm class.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actually, this and #1024 are the start of cleaning out the much of the Arm class by decomposing its code into a set of smaller interfaces. this and #1024 are the first steps in that.

However, since that is a complicated operation on a crucial part of the robot, I am taking baby steps, using knowledge gained in #1011. As such, your suggestion is in the opposite direction of what is being aimed for :)

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I like that you are taking small steps and I think it is not a big issue to have a little bit of duplicate code, but I guess that in the end we want to get rid of this. But for now I guess this is fine.

# type: (float) -> bool
"""
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
"""
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.

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
"""
return self._exchange_with_human(True, timeout)

def _exchange_with_human(self, to_robot, timeout=10):
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good idea to combine this! I like it!

# type: (bool, float) -> bool
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(result_topic, 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:
Expand Down Expand Up @@ -404,6 +457,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
Expand Down Expand Up @@ -684,46 +739,26 @@ 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.

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.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):
# type: (float) -> bool
"""
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.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)):
"""
Expand Down