def execute(self, userdata):
grab_entity = self.grab_entity_designator.resolve()
if not grab_entity:
rospy.logerr("Could not resolve grab_entity")
return "failed"
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
userdata.arm = arm.side # Using userdata makes sure we don't need to do any more arm entity magic
goal_map = VectorStamped(0, 0, 0, frame_id=grab_entity.id)
try:
# Transform to base link frame
goal_bl = goal_map.projectToFrame(
self.robot.robot_name + '/base_link',
tf_listener=self.robot.tf_listener)
if goal_bl is None:
rospy.logerr('Transformation of goal to base failed')
return 'failed'
except tf.Exception, tfe:
rospy.logerr(
'Transformation of goal to base failed: {0}'.format(tfe))
return 'failed'
def get_closest_entity(self, type="", center_point=None, radius=0):
if not center_point:
center_point = VectorStamped(x=0,
y=0,
z=0,
frame_id="/" + self.robot_name +
"/base_link")
entities = self.get_entities(type=type,
center_point=center_point,
radius=radius)
# HACK
entities = [
e for e in entities if e.shape is not None and e.type != ""
]
if len(entities) == 0:
return None
# Sort by distance
try:
center_in_map = center_point.projectToFrame(
"/map", self._tf_listener)
entities = sorted(
entities,
key=lambda entity: entity.distance_to_2d(center_in_map.vector))
except:
rospy.logerr("Failed to sort entities")
return None
return entities[0]
def execute(self, userdata=None):
# Get position to look at. Transform the position to map frame since taking the hmi pose may move base link
goal = VectorStamped(1.0,
0.0,
1.6,
frame_id="/" + self._robot.robot_name +
"/base_link")
tf_goal = goal.projectToFrame('/map', self._robot.tf_listener)
self._robot.move_to_hmi_pose()
self._robot.head.look_at_point(tf_goal)
self._robot.head.wait_for_motion_done()
return "succeeded"
def execute(self):
# convert item to pos and send goal
goal_map = VectorStamped(0, 0, 0, frame_id=self.item.id)
goal_bl = goal_map.projectToFrame(self.robot.robot_name + '/base_link',
tf_listener=self.robot.tf_listener)
if goal_bl is None:
return 'failed'
else:
return 'failed'
# Offset so goal_bl is in WS
# tfToWSOffset = find ws from base_link and find the difference
goal_bl.vector[0] = goal_bl.vector.x() - tfToWSOffset
if not self.arm.send_goal(goal_bl,
timeout=0): # maybe register callbacks when
return ('failed') # movement finished? not waiting
def project_roi(self, roi, frame_id=None):
""" Projects a region of interest of a depth image to a 3D Point. Hereto, a service is used
:param roi: sensor_msgs/RegionOfInterest
:param frame_id: if specified, the result is transformed into this frame id
:return: VectorStamped object
"""
response = self.project_rois(rois=[roi]).points[0]
# Convert to VectorStamped
result = VectorStamped(x=response.point.x, y=response.point.y, z=response.point.z,
frame_id=response.header.frame_id)
# If necessary, transform the point
if frame_id is not None:
rospy.loginfo("Transforming roi to {}".format(frame_id))
result = result.projectToFrame(frame_id=frame_id, tf_listener=self.tf_listener)
# Return the result
return result
def execute(self, userdata=None):
point_entity = self.point_entity_designator.resolve()
if not point_entity:
rospy.logerr("Could not resolve grab_entity")
return "failed"
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
goal_map = VectorStamped(0, 0, 0, frame_id=point_entity.id)
try:
# Transform to base link frame
goal_bl = goal_map.projectToFrame(self.robot.base_link_frame, tf_listener=self.robot.tf_listener)
if goal_bl is None:
rospy.logerr('Transformation of goal to base failed')
return 'failed'
except tf2_ros.TransformException as tfe:
rospy.logerr('Transformation of goal to base failed: {0}'.format(tfe))
return 'failed'
# Make sure the torso and the arm are done
self.robot.torso.wait_for_motion_done(cancel=True)
arm.wait_for_motion_done(cancel=True)
# This is needed because the head is not entirely still when the
# look_at_point function finishes
rospy.sleep(rospy.Duration(0.5))
# Resolve the entity again because we want the latest pose
updated_point_entity = self.point_entity_designator.resolve()
rospy.loginfo("ID to update: {0}".format(point_entity.id))
if not updated_point_entity:
rospy.logerr("Could not resolve the updated grab_entity, "
"this should not happen [CHECK WHY THIS IS HAPPENING]")
point_entity = self.associate(original_entity=point_entity)
else:
rospy.loginfo("Updated pose of entity (dx, dy, dz) : (%f, %f, %f)" %
(updated_point_entity.pose.frame.p.x() - point_entity.pose.frame.p.x(),
updated_point_entity.pose.frame.p.y() - point_entity.pose.frame.p.y(),
updated_point_entity.pose.frame.p.z() - point_entity.pose.frame.p.z()))
point_entity = updated_point_entity
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Grasp point determination
goal_bl = self._get_pointing_pose(point_entity)
# Grasp
rospy.loginfo('Start pointing')
for x_offset in [-0.15, 0.0]: # Hack because Hero does not pre-grasp reliably
_goal_bl = FrameStamped(goal_bl.frame * kdl.Frame(kdl.Vector(x_offset, 0.0, 0.0)), goal_bl.frame_id)
if not arm.send_goal(_goal_bl, timeout=20, pre_grasp=False, allowed_touch_objects=[point_entity.id]):
self.robot.speech.speak('I am sorry but I cannot move my arm to the object position', block=False)
rospy.logerr('Grasp failed')
arm.reset()
return 'failed'
# Retract
rospy.loginfo('Start retracting')
roll = 0.0
goal_bl.frame.p.x(goal_bl.frame.p.x() - 0.1) # Retract 10 cm
goal_bl.frame.p.z(goal_bl.frame.p.z() + 0.05) # Go 5 cm higher
goal_bl.frame.M = kdl.Rotation.RPY(roll, 0.0, 0.0) # Update the roll
rospy.loginfo("Start retract")
if not arm.send_goal(goal_bl, timeout=0.0, allowed_touch_objects=[point_entity.id]):
rospy.logerr('Failed retract')
arm.wait_for_motion_done()
self.robot.base.force_drive(-0.125, 0, 0, 2.0)
# Close gripper
arm.wait_for_motion_done(cancel=True)
# Carrying pose
arm.send_joint_goal('carrying_pose', timeout=0.0)
# Reset head
self.robot.head.cancel_goal()
return "succeeded"
def execute(self, userdata=None):
grab_entity = self.grab_entity_designator.resolve()
if not grab_entity:
rospy.logerr("Could not resolve grab_entity")
return "failed"
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
goal_map = VectorStamped(0, 0, 0, frame_id=grab_entity.id)
try:
# Transform to base link frame
goal_bl = goal_map.projectToFrame(
self.robot.robot_name + '/base_link',
tf_listener=self.robot.tf_listener)
if goal_bl is None:
rospy.logerr('Transformation of goal to base failed')
return 'failed'
except tf2_ros.TransformException as tfe:
rospy.logerr(
'Transformation of goal to base failed: {0}'.format(tfe))
return 'failed'
# Make sure the torso and the arm are done
self.robot.torso.wait_for_motion_done(cancel=True)
arm.wait_for_motion_done(cancel=True)
# This is needed because the head is not entirely still when the
# look_at_point function finishes
rospy.sleep(rospy.Duration(0.5))
# Resolve the entity again because we want the latest pose
updated_grab_entity = self.grab_entity_designator.resolve()
rospy.loginfo("ID to update: {0}".format(grab_entity.id))
if not updated_grab_entity:
rospy.logerr(
"Could not resolve the updated grab_entity, "
"this should not happen [CHECK WHY THIS IS HAPPENING]")
grab_entity = self.associate(original_entity=grab_entity)
else:
rospy.loginfo(
"Updated pose of entity (dx, dy, dz) : (%f, %f, %f)" %
(updated_grab_entity.pose.frame.p.x() -
grab_entity.pose.frame.p.x(),
updated_grab_entity.pose.frame.p.y() -
grab_entity.pose.frame.p.y(),
updated_grab_entity.pose.frame.p.z() -
grab_entity.pose.frame.p.z()))
grab_entity = updated_grab_entity
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Grasp point determination
grasp_framestamped = self._gpd.get_grasp_pose(grab_entity, arm)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
goal_map = VectorStamped(0, 0, 0, frame_id=grab_entity.id)
# In case grasp point determination didn't work
if not grasp_framestamped:
goal_bl = goal_map.projectToFrame(
self.robot.robot_name + '/base_link',
tf_listener=self.robot.tf_listener)
if goal_bl is None:
return 'failed'
else:
return 'failed'
else:
# We do have a grasp pose, given as a kdl frame in map
try:
self.robot.tf_listener.waitForTransform(
"/map", self.robot.robot_name + "/base_link",
rospy.Time(0), rospy.Duration(10))
# Transform to base link frame
goal_bl = grasp_framestamped.projectToFrame(
self.robot.robot_name + "/base_link",
tf_listener=self.robot.tf_listener)
if goal_bl is None:
return 'failed'
except tf2_ros.TransformException as tfe:
rospy.logerr(
'Transformation of goal to base failed: {0}'.format(tfe))
return 'failed'
# Pre-grasp --> this is only necessary when using visual servoing
# rospy.loginfo('Starting Pre-grasp')
# if not arm.send_goal(goal_bl.x, goal_bl.y, goal_bl.z, 0, 0, 0,
# frame_id='/'+self.robot.robot_name+'/base_link',
# timeout=20, pre_grasp=True, first_joint_pos_only=True
# ):
# rospy.logerr('Pre-grasp failed:')
# arm.reset()
# arm.send_gripper_goal('close', timeout=None)
# return 'failed'
# Grasp
rospy.loginfo('Start grasping')
if not arm.send_goal(goal_bl,
timeout=20,
pre_grasp=True,
allowed_touch_objects=[grab_entity.id]):
self.robot.speech.speak(
'I am sorry but I cannot move my arm to the object position',
block=False)
rospy.logerr('Grasp failed')
arm.reset()
arm.send_gripper_goal('close', timeout=0.0)
return 'failed'
# Close gripper
arm.send_gripper_goal('close')
arm.occupied_by = grab_entity
# Lift
goal_bl = grasp_framestamped.projectToFrame(
self.robot.robot_name + "/base_link",
tf_listener=self.robot.tf_listener)
rospy.loginfo('Start lifting')
roll = 0.0
goal_bl.frame.p.z(goal_bl.frame.p.z() + 0.05) # Add 5 cm
goal_bl.frame.M = kdl.Rotation.RPY(roll, 0, 0) # Update the roll
rospy.loginfo("Start lift")
if not arm.send_goal(
goal_bl, timeout=20, allowed_touch_objects=[grab_entity.id]):
rospy.logerr('Failed lift')
# Retract
goal_bl = grasp_framestamped.projectToFrame(
self.robot.robot_name + '/base_link',
tf_listener=self.robot.tf_listener)
rospy.loginfo('Start retracting')
roll = 0.0
goal_bl.frame.p.x(goal_bl.frame.p.x() - 0.1) # Retract 10 cm
goal_bl.frame.p.z(goal_bl.frame.p.z() + 0.05) # Go 5 cm higher
goal_bl.frame.M = kdl.Rotation.RPY(roll, 0.0, 0.0) # Update the roll
rospy.loginfo("Start retract")
if not arm.send_goal(
goal_bl, timeout=0.0, allowed_touch_objects=[grab_entity.id]):
rospy.logerr('Failed retract')
arm.wait_for_motion_done()
self.robot.base.force_drive(-0.125, 0, 0, 2.0)
# Update Kinect once again to make sure the object disappears from ED
segm_res = self.robot.ed.update_kinect("%s" % grab_entity.id)
arm.wait_for_motion_done(cancel=True)
# Carrying pose
# rospy.loginfo('start moving to carrying pose')
arm.send_joint_goal('carrying_pose', timeout=0.0)
result = 'succeeded'
if self._check_occupancy:
# Check if the object is present in the gripper
if arm.object_in_gripper_measurement.is_empty:
# If state is empty, grasp has failed
result = "failed"
rospy.logerr("Gripper is not holding an object")
self.robot.speech.speak(
"Whoops, something went terribly wrong")
arm.occupied_by = None # Set the object the arm is holding to None
else:
# State is holding, grasp succeeded.
# If unknown: sensor not there, assume gripper is holding and hope for the best
result = "succeeded"
if arm.object_in_gripper_measurement.is_unknown:
rospy.logwarn("GripperMeasurement unknown")
# Reset head
self.robot.head.cancel_goal()
return result
def get_closest_laser_entity(self,
type="",
center_point=VectorStamped(),
radius=0,
ignore_z=False):
"""
Get the closest entity detected by the laser. The ID's of such entities are postfixed with '-laser'
For the rest, this works exactly like get_closest_entity
:param type: What type of entities to filter on
:param center_point: combined with radius. Around which point to search for entities
:param radius: how far from the center_point to look (in meters)
:param ignore_z: Consider only the distance in the X,Y plane for the radius from center_point criterium.
:return: list of Entity
"""
if ignore_z:
# ED does not allow to ignore Z through its interface, so it has to be solved here.
#
# If we want to ignore the z of entities when checking their distance from center_point,
# the simplest thing to do is to set the Z of the center_point to the same value,
# so that delta Z is always 0.
# But then we first need to know Z (preferably without an additonal parameter or something robot specific)
# So, we query ED for other laser entities. These are *assumed* to all have the same Z,
# so we can just take one and use its Z and substitute that into the center_point.
# To 'just take one', we take entities from a larger range.
entities_for_height = self.get_entities(type="",
center_point=center_point,
radius=sqrt(2**2 +
radius**2))
if entities_for_height:
override_z = entities_for_height[0].frame.extractVectorStamped(
).vector.z()
rospy.logwarn(
"ignoring Z, so overriding z to be equal to laser height: {}"
.format(override_z))
center_point = VectorStamped(center_point.vector.x(),
center_point.vector.y(),
override_z)
else:
return None
entities = self.get_entities(type="",
center_point=center_point,
radius=radius)
# HACK
entities = [
e for e in entities
if e.shape and e.type == "" and e.id.endswith("-laser")
]
if len(entities) == 0:
return None
# Sort by distance
try:
entities = sorted(
entities,
key=lambda entity: entity.distance_to_2d(
center_point.projectToFrame(
"/%s/base_link" % self.robot_name, self.tf_listener).
vector)) # TODO: adjust for robot
except Exception as e:
rospy.logerr("Failed to sort entities: {}".format(e))
return None
return entities[0]
def execute(self, userdata=None):
grab_entity = self.grab_entity_designator.resolve()
if not grab_entity:
rospy.logerr("Could not resolve grab_entity")
return "failed"
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
goal_map = VectorStamped(0, 0, 0, frame_id=grab_entity.id)
try:
# Transform to base link frame
goal_bl = goal_map.projectToFrame(self.robot.robot_name+'/base_link', tf_listener=self.robot.tf_listener)
if goal_bl is None:
rospy.logerr('Transformation of goal to base failed')
return 'failed'
except tf2_ros.TransformException as tfe:
rospy.logerr('Transformation of goal to base failed: {0}'.format(tfe))
return 'failed'
# Make sure the torso and the arm are done
self.robot.torso.wait_for_motion_done(cancel=True)
arm.wait_for_motion_done(cancel=True)
# This is needed because the head is not entirely still when the
# look_at_point function finishes
rospy.sleep(rospy.Duration(0.5))
# Resolve the entity again because we want the latest pose
updated_grab_entity = self.grab_entity_designator.resolve()
rospy.loginfo("ID to update: {0}".format(grab_entity.id))
if not updated_grab_entity:
rospy.logerr("Could not resolve the updated grab_entity, "
"this should not happen [CHECK WHY THIS IS HAPPENING]")
grab_entity = self.associate(original_entity=grab_entity)
else:
rospy.loginfo("Updated pose of entity (dx, dy, dz) : (%f, %f, %f)" %
(updated_grab_entity.pose.frame.p.x() - grab_entity.pose.frame.p.x(),
updated_grab_entity.pose.frame.p.y() - grab_entity.pose.frame.p.y(),
updated_grab_entity.pose.frame.p.z() - grab_entity.pose.frame.p.z()))
grab_entity = updated_grab_entity
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Grasp point determination
grasp_framestamped = self._gpd.get_grasp_pose(grab_entity, arm)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
goal_map = VectorStamped(0, 0, 0, frame_id=grab_entity.id)
# In case grasp point determination didn't work
if not grasp_framestamped:
goal_bl = goal_map.projectToFrame(self.robot.robot_name + '/base_link', tf_listener=self.robot.tf_listener)
if goal_bl is None:
return 'failed'
else:
return 'failed'
else:
# We do have a grasp pose, given as a kdl frame in map
try:
self.robot.tf_listener.waitForTransform("/map", self.robot.robot_name + "/base_link", rospy.Time(0),
rospy.Duration(10))
# Transform to base link frame
goal_bl = grasp_framestamped.projectToFrame(self.robot.robot_name + "/base_link",
tf_listener=self.robot.tf_listener)
if goal_bl is None:
return 'failed'
except tf2_ros.TransformException as tfe:
rospy.logerr('Transformation of goal to base failed: {0}'.format(tfe))
return 'failed'
# Pre-grasp --> this is only necessary when using visual servoing
# rospy.loginfo('Starting Pre-grasp')
# if not arm.send_goal(goal_bl.x, goal_bl.y, goal_bl.z, 0, 0, 0,
# frame_id='/'+self.robot.robot_name+'/base_link',
# timeout=20, pre_grasp=True, first_joint_pos_only=True
# ):
# rospy.logerr('Pre-grasp failed:')
# arm.reset()
# arm.send_gripper_goal('close', timeout=None)
# return 'failed'
# Grasp
rospy.loginfo('Start grasping')
if not arm.send_goal(goal_bl, timeout=20, pre_grasp=True, allowed_touch_objects=[grab_entity.id]):
self.robot.speech.speak('I am sorry but I cannot move my arm to the object position', block=False)
rospy.logerr('Grasp failed')
arm.reset()
arm.send_gripper_goal('close', timeout=0.0)
return 'failed'
# Close gripper
arm.send_gripper_goal('close')
arm.occupied_by = grab_entity
# Lift
goal_bl = grasp_framestamped.projectToFrame(self.robot.robot_name + "/base_link",
tf_listener=self.robot.tf_listener)
rospy.loginfo('Start lifting')
roll = 0.0
goal_bl.frame.p.z(goal_bl.frame.p.z() + 0.05) # Add 5 cm
goal_bl.frame.M = kdl.Rotation.RPY(roll, 0, 0) # Update the roll
rospy.loginfo("Start lift")
if not arm.send_goal(goal_bl, timeout=20, allowed_touch_objects=[grab_entity.id]):
rospy.logerr('Failed lift')
# Retract
goal_bl = grasp_framestamped.projectToFrame(self.robot.robot_name + '/base_link',
tf_listener=self.robot.tf_listener)
rospy.loginfo('Start retracting')
roll = 0.0
goal_bl.frame.p.x(goal_bl.frame.p.x() - 0.1) # Retract 10 cm
goal_bl.frame.p.z(goal_bl.frame.p.z() + 0.05) # Go 5 cm higher
goal_bl.frame.M = kdl.Rotation.RPY(roll, 0.0, 0.0) # Update the roll
rospy.loginfo("Start retract")
if not arm.send_goal(goal_bl, timeout=0.0, allowed_touch_objects=[grab_entity.id]):
rospy.logerr('Failed retract')
arm.wait_for_motion_done()
self.robot.base.force_drive(-0.125, 0, 0, 2.0)
# Update Kinect once again to make sure the object disappears from ED
segm_res = self.robot.ed.update_kinect("%s" % grab_entity.id)
arm.wait_for_motion_done(cancel=True)
# Carrying pose
# rospy.loginfo('start moving to carrying pose')
arm.send_joint_goal('carrying_pose', timeout=0.0)
result = 'succeeded'
if self._check_occupancy:
# Check if the object is present in the gripper
if arm.object_in_gripper_measurement.is_empty:
# If state is empty, grasp has failed
result = "failed"
rospy.logerr("Gripper is not holding an object")
self.robot.speech.speak("Whoops, something went terribly wrong")
arm.occupied_by = None # Set the object the arm is holding to None
else:
# State is holding, grasp succeeded.
# If unknown: sensor not there, assume gripper is holding and hope for the best
result = "succeeded"
if arm.object_in_gripper_measurement.is_unknown:
rospy.logwarn("GripperMeasurement unknown")
# Reset head
self.robot.head.cancel_goal()
return result
