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 in map frame
goal_map = msgs.Point(0, 0, 0)
try:
# Transform to base link frame
goal_bl = transformations.tf_transform(
goal_map,
grab_entity.id,
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 execute(self, gl):
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "point_failed"
entity = self.point_entity_designator.resolve()
if not entity:
rospy.logerr("Could not resolve entity")
return "target_lost"
if arm == self.robot.leftArm:
end_effector_frame_id = "/amigo/grippoint_left"
elif arm == self.robot.rightArm:
end_effector_frame_id = "/amigo/grippoint_right"
target_position = msgs.PointStamped(entity.pose, frame_id = "/map", stamp = rospy.Time())
rospy.loginfo("[robot_smach_states:Point_at_object] Target position: {0}".format(target_position))
# Keep looking at end-effector for ar marker detection
self.robot.head.look_at_point(msgs.PointStamped(0,0,0,frame_id=end_effector_frame_id))
# Transform to base link
target_position_bl = transformations.tf_transform(target_position, "/map","/amigo/base_link", tf_listener=self.robot.tf_listener)
rospy.loginfo("[robot_smach_states] Target position in base link: {0}".format(target_position_bl))
# Send goal
if arm.send_goal(target_position_bl.x-0.1, target_position_bl.y, target_position_bl.z, 0, 0, 0, 120, pre_grasp = True):
rospy.loginfo("arm at object")
else:
rospy.loginfo("Arm cannot reach object")
self.robot.head.reset()
return 'point_succeeded'
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 in map frame
goal_map = msgs.Point(0, 0, 0)
try:
# Transform to base link frame
goal_bl = transformations.tf_transform(
goal_map, grab_entity.id, 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 execute(self, gl):
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "point_failed"
entity = self.point_entity_designator.resolve()
if not entity:
rospy.logerr("Could not resolve entity")
return "target_lost"
if arm == self.robot.leftArm:
end_effector_frame_id = "/amigo/grippoint_left"
elif arm == self.robot.rightArm:
end_effector_frame_id = "/amigo/grippoint_right"
target_position = msgs.PointStamped(entity.pose,
frame_id="/map",
stamp=rospy.Time())
rospy.loginfo(
"[robot_smach_states:Point_at_object] Target position: {0}".format(
target_position))
# Keep looking at end-effector for ar marker detection
self.robot.head.look_at_point(
msgs.PointStamped(0, 0, 0, frame_id=end_effector_frame_id))
# Transform to base link
target_position_bl = transformations.tf_transform(
target_position,
"/map",
"/amigo/base_link",
tf_listener=self.robot.tf_listener)
rospy.loginfo(
"[robot_smach_states] Target position in base link: {0}".format(
target_position_bl))
# Send goal
if arm.send_goal(target_position_bl.x - 0.1,
target_position_bl.y,
target_position_bl.z,
0,
0,
0,
120,
pre_grasp=True):
rospy.loginfo("arm at object")
else:
rospy.loginfo("Arm cannot reach object")
self.robot.head.reset()
return 'point_succeeded'
def execute(self, userdata=None):
# ToDo: check point_designator?
goal = self.point_designator.resolve()
if not goal:
rospy.loginfo(
"point_designator {0} cannot be resolved: {1}".format(
self.point_designator))
return 'failed'
rospy.loginfo("ArmToQueryPoint: goal = {0}".format(goal))
# Note: answers are typically in "map"frame, check whether this works out
rospy.logwarn(
"Transforming to base_link frame for amigo_arm_navigation")
goal_bl = transformations.tf_transform(
goal.point,
goal.header.frame_id,
"/amigo/base_link",
tf_listener=self.robot.tf_listener)
if goal_bl == None:
return 'failed'
if self.side.send_goal(goal_bl.x,
goal_bl.y,
goal_bl.z,
0,
0,
0,
frame_id="/amigo/base_link",
timeout=self.time_out,
pre_grasp=self.pre_grasp,
first_joint_pos_only=self.first_joint_pos_only):
return 'succeeded'
else:
rospy.logwarn("Goal unreachable: {0}".format(goal_bl).replace(
"\n", " "))
return 'failed'
def execute(self, userdata):
# ToDo: check point_designator?
goal = self.point_designator.resolve()
if not goal:
rospy.loginfo("point_designator {0} cannot be resolved: {1}".format(self.point_designator))
return 'failed'
rospy.loginfo("ArmToQueryPoint: goal = {0}".format(goal))
# Note: answers are typically in "map"frame, check whether this works out
rospy.logwarn("Transforming to base_link frame for amigo_arm_navigation")
goal_bl = transformations.tf_transform(goal.point, goal.header.frame_id, "/amigo/base_link", tf_listener=self.robot.tf_listener)
if goal_bl == None:
return 'failed'
if self.side.send_goal(goal_bl.x, goal_bl.y, goal_bl.z, 0, 0, 0,
frame_id="/amigo/base_link",
timeout=self.time_out,
pre_grasp=self.pre_grasp,
first_joint_pos_only=self.first_joint_pos_only):
return 'succeeded'
else:
rospy.logwarn("Goal unreachable: {0}".format(goal_bl).replace("\n", " "))
return 'failed'
def execute(self, userdata=None):
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
if arm == self.robot.arms["left"]:
end_effector_frame_id = "/" + self.robot.robot_name + "/grippoint_left"
ar_frame_id = "/hand_marker_left"
elif arm == self.robot.arms["right"]:
end_effector_frame_id = "/" + self.robot.robot_name + "/grippoint_right"
ar_frame_id = "/hand_marker_right"
target_position = self.grab_point_designator.resolve()
if not target_position:
rospy.loginfo(
"Could not resolve grab_point_designator {0}: {1}".format(
self.grab_point_designator))
return "target_lost"
# Keep looking at end-effector for ar marker detection
self.robot.head.look_at_goal(
msgs.PointStamped(0, 0, 0, frame_id=end_effector_frame_id))
rospy.loginfo("[robot_smach_states:grasp] Target position: {0}".format(
target_position))
target_position_bl = transformations.tf_transform(
target_position.point,
target_position.header.frame_id,
"/amigo/base_link",
tf_listener=self.robot.tf_listener)
rospy.loginfo(
"[robot_smach_states] Target position in base link: {0}".format(
target_position_bl))
target_position_delta = geometry_msgs.msg.Point()
# First check to see if visual servoing is possible
self.robot.perception.toggle(['ar_pose'])
# Transform point(0,0,0) in ar marker frame to grippoint frame
ar_point = msgs.PointStamped(0,
0,
0,
frame_id=ar_frame_id,
stamp=rospy.Time())
# Check several times if transform is available
cntr = 0
ar_marker_available = False
while (cntr < 5 and not ar_marker_available):
rospy.logdebug("Checking AR marker for the {0} time".format(cntr +
1))
ar_point_grippoint = transformations.tf_transform(
ar_point,
ar_frame_id,
end_effector_frame_id,
tf_listener=self.robot.tf_listener)
if not ar_point_grippoint == None:
ar_marker_available = True
else:
cntr += 1
rospy.sleep(0.2)
# If transform is not available, try again, but use head movement as well
if not ar_marker_available:
arm.send_delta_goal(0.05,
0.0,
0.0,
0.0,
0.0,
0.0,
timeout=5.0,
frame_id=end_effector_frame_id,
pre_grasp=False)
self.robot.speech.speak("Let me have a closer look", block=False)
ar_point_grippoint = transformations.tf_transform(
ar_point,
ar_frame_id,
end_effector_frame_id,
tf_listener=self.robot.tf_listener)
rospy.loginfo(
"AR marker in end-effector frame = {0}".format(ar_point_grippoint))
# Transform target position to grippoint frame
target_position_grippoint = transformations.tf_transform(
target_position,
"/map",
end_effector_frame_id,
tf_listener=self.robot.tf_listener)
rospy.loginfo("Target position in end-effector frame = {0}".format(
target_position_grippoint))
# Compute difference = delta (only when both transformations have succeeded) and correct for offset ar_marker and grippoint
if not (ar_point_grippoint == None
or target_position_grippoint == None):
target_position_delta = msgs.Point(
target_position_grippoint.x - ar_point_grippoint.x +
arm.markerToGrippointOffset.x, target_position_grippoint.y -
ar_point_grippoint.y + arm.markerToGrippointOffset.y,
target_position_grippoint.z - ar_point_grippoint.z +
arm.markerToGrippointOffset.z)
rospy.loginfo("Delta target = {0}".format(target_position_delta))
ar_marker_available = True
else:
ar_marker_available = False
rospy.logwarn(
"ar_marker_available (2) = {0}".format(ar_marker_available))
# Sanity check
if ar_marker_available:
#rospy.loginfo("Delta target = {0}".format(target_position_delta))
if (target_position_delta.x < 0 or target_position_delta.x > 0.6
or target_position_delta.y < -0.3
or target_position_delta.y > 0.3
or target_position_delta.z < -0.3
or target_position_delta.z > 0.3):
rospy.logwarn("Ar marker detection probably incorrect")
self.robot.speech.speak(
"I guess I cannot see my hand properly", block=False)
ar_marker_available = False
rospy.logwarn(
"ar_marker_available (3) = {0}".format(ar_marker_available))
# Switch off ar marker detection
self.robot.perception.toggle([])
# Original, pregrasp is performed by the compute_pre_grasp node
if not ar_marker_available:
self.robot.speech.speak("Let's see", block=False)
if arm.send_goal(target_position_bl.x,
target_position_bl.y,
target_position_bl.z,
0,
0,
0,
120,
pre_grasp=True):
rospy.loginfo("arm at object")
else:
rospy.logerr(
"Goal unreachable: {0}".format(target_position_bl).replace(
"\n", " "))
self.robot.speech.speak(
"I am sorry but I cannot move my arm to the object position",
block=False)
return 'grab_failed'
else:
self.robot.speech.speak("Let's go", block=False)
if arm.send_delta_goal(target_position_delta.x,
target_position_delta.y,
target_position_delta.z,
0,
0,
0,
120,
frame_id=end_effector_frame_id,
pre_grasp=True):
rospy.loginfo("arm at object")
else:
rospy.logerr(
"Goal unreachable: {0}".format(target_position_bl).replace(
"\n", " "))
self.robot.speech.speak(
"I am sorry but I cannot move my arm to the object position",
block=False)
return 'grab_failed'
self.robot.head.reset_position(timeout=0.0)
return 'grab_succeeded'
def execute(self, userdata=None):
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
if arm == self.robot.arms["left"]:
end_effector_frame_id = "/"+self.robot.robot_name+"/grippoint_left"
ar_frame_id = "/hand_marker_left"
elif arm == self.robot.arms["right"]:
end_effector_frame_id = "/"+self.robot.robot_name+"/grippoint_right"
ar_frame_id = "/hand_marker_right"
target_position = self.grab_point_designator.resolve()
if not target_position:
rospy.loginfo("Could not resolve grab_point_designator {0}: {1}".format(self.grab_point_designator))
return "target_lost"
# Keep looking at end-effector for ar marker detection
self.robot.head.look_at_goal(msgs.PointStamped(0,0,0,frame_id=end_effector_frame_id))
rospy.loginfo("[robot_smach_states:grasp] Target position: {0}".format(target_position))
target_position_bl = transformations.tf_transform(target_position.point, target_position.header.frame_id,"/amigo/base_link", tf_listener=self.robot.tf_listener)
rospy.loginfo("[robot_smach_states] Target position in base link: {0}".format(target_position_bl))
target_position_delta = geometry_msgs.msg.Point()
# First check to see if visual servoing is possible
self.robot.perception.toggle(['ar_pose'])
# Transform point(0,0,0) in ar marker frame to grippoint frame
ar_point = msgs.PointStamped(0, 0, 0, frame_id = ar_frame_id, stamp = rospy.Time())
# Check several times if transform is available
cntr = 0
ar_marker_available = False
while (cntr < 5 and not ar_marker_available):
rospy.logdebug("Checking AR marker for the {0} time".format(cntr+1))
ar_point_grippoint = transformations.tf_transform(ar_point, ar_frame_id, end_effector_frame_id, tf_listener=self.robot.tf_listener)
if not ar_point_grippoint == None:
ar_marker_available = True
else:
cntr += 1
rospy.sleep(0.2)
# If transform is not available, try again, but use head movement as well
if not ar_marker_available:
arm.send_delta_goal(0.05,0.0,0.0,0.0,0.0,0.0, timeout=5.0, frame_id=end_effector_frame_id, pre_grasp = False)
self.robot.speech.speak("Let me have a closer look", block=False)
ar_point_grippoint = transformations.tf_transform(ar_point, ar_frame_id, end_effector_frame_id, tf_listener=self.robot.tf_listener)
rospy.loginfo("AR marker in end-effector frame = {0}".format(ar_point_grippoint))
# Transform target position to grippoint frame
target_position_grippoint = transformations.tf_transform(target_position, "/map", end_effector_frame_id, tf_listener=self.robot.tf_listener)
rospy.loginfo("Target position in end-effector frame = {0}".format(target_position_grippoint))
# Compute difference = delta (only when both transformations have succeeded) and correct for offset ar_marker and grippoint
if not (ar_point_grippoint == None or target_position_grippoint == None):
target_position_delta = msgs.Point(target_position_grippoint.x - ar_point_grippoint.x + arm.markerToGrippointOffset.x,
target_position_grippoint.y - ar_point_grippoint.y + arm.markerToGrippointOffset.y,
target_position_grippoint.z - ar_point_grippoint.z + arm.markerToGrippointOffset.z)
rospy.loginfo("Delta target = {0}".format(target_position_delta))
ar_marker_available = True
else:
ar_marker_available = False
rospy.logwarn("ar_marker_available (2) = {0}".format(ar_marker_available))
# Sanity check
if ar_marker_available:
#rospy.loginfo("Delta target = {0}".format(target_position_delta))
if (target_position_delta.x < 0 or target_position_delta.x > 0.6 or target_position_delta.y < -0.3 or target_position_delta.y > 0.3 or target_position_delta.z < -0.3 or target_position_delta.z > 0.3):
rospy.logwarn("Ar marker detection probably incorrect")
self.robot.speech.speak("I guess I cannot see my hand properly", block=False)
ar_marker_available = False
rospy.logwarn("ar_marker_available (3) = {0}".format(ar_marker_available))
# Switch off ar marker detection
self.robot.perception.toggle([])
# Original, pregrasp is performed by the compute_pre_grasp node
if not ar_marker_available:
self.robot.speech.speak("Let's see", block=False)
if arm.send_goal(target_position_bl.x, target_position_bl.y, target_position_bl.z, 0, 0, 0, 120, pre_grasp = True):
rospy.loginfo("arm at object")
else:
rospy.logerr("Goal unreachable: {0}".format(target_position_bl).replace("\n", " "))
self.robot.speech.speak("I am sorry but I cannot move my arm to the object position", block=False)
return 'grab_failed'
else:
self.robot.speech.speak("Let's go", block=False)
if arm.send_delta_goal(target_position_delta.x, target_position_delta.y, target_position_delta.z,
0, 0, 0, 120, frame_id=end_effector_frame_id, pre_grasp = True):
rospy.loginfo("arm at object")
else:
rospy.logerr("Goal unreachable: {0}".format(target_position_bl).replace("\n", " "))
self.robot.speech.speak("I am sorry but I cannot move my arm to the object position", block=False)
return 'grab_failed'
self.robot.head.reset_position(timeout=0.0)
return 'grab_succeeded'
def _transform_center_point_to_map(self, pointstamped):
point_in_map = transformations.tf_transform(
pointstamped.point, pointstamped.header.frame_id, "/map",
self.tf_listener)
return point_in_map
def execute(self, userdata): #robot, item_to_place, placement_pose, arm):
item_to_place = self._item_to_place_designator.resolve()
if not item_to_place:
rospy.logerr("Could not resolve item_to_place")
#return "failed"
placement_pose = self._placement_pose_designator.resolve()
if not placement_pose:
rospy.logerr("Could not resolve placement_pose")
return "failed"
arm = self._arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
rospy.loginfo("Placing")
# placement_pose is a PoseStamped
place_pose_bl = transformations.tf_transform(
placement_pose.pose.position,
placement_pose.header.frame_id,
self._robot.robot_name + '/base_link',
tf_listener=self._robot.tf_listener)
# Wait for torso and arm to finish their motions
self._robot.torso.wait_for_motion_done()
arm.wait_for_motion_done()
try:
height = place_pose_bl.z
except KeyError:
height = 0.8
# Pre place
if not arm.send_goal(place_pose_bl.x,
place_pose_bl.y,
height + 0.2,
0.0,
0.0,
0.0,
timeout=10,
pre_grasp=False,
frame_id="/{0}/base_link".format(
self._robot.robot_name)):
# If we can't place, try a little closer
place_pose_bl.x -= 0.05
rospy.loginfo("Retrying preplace")
if not arm.send_goal(place_pose_bl.x,
place_pose_bl.y,
height + 0.2,
0.0,
0.0,
0.0,
timeout=10,
pre_grasp=False,
frame_id="/{0}/base_link".format(
self._robot.robot_name)):
rospy.logwarn("Cannot pre-place the object")
arm.cancel_goals()
return 'failed'
# Place
if not arm.send_goal(place_pose_bl.x,
place_pose_bl.y,
height + 0.15,
0.0,
0.0,
0.0,
timeout=10,
pre_grasp=False,
frame_id="/{0}/base_link".format(
self._robot.robot_name)):
rospy.logwarn("Cannot place the object, dropping it...")
place_entity = arm.occupied_by
if not place_entity:
rospy.logerr(
"Arm not holding an entity to place. This should never happen")
else:
self._robot.ed.update_entity(place_entity.id,
posestamped=placement_pose)
arm.occupied_by = None
# Open gripper
# Since we cannot reliably wait for the gripper, just set this timeout
arm.send_gripper_goal('open', timeout=2.0)
arm.occupied_by = None
# Retract
arm.send_goal(place_pose_bl.x - 0.1,
place_pose_bl.y,
place_pose_bl.z + 0.05,
0.0,
0.0,
0.0,
frame_id='/' + self._robot.robot_name + '/base_link',
timeout=0.0)
self._robot.base.force_drive(-0.125, 0, 0, 1.5)
if not arm.wait_for_motion_done(timeout=5.0):
rospy.logwarn('Retraction failed')
arm.cancel_goals()
# Close gripper
arm.send_gripper_goal('close', timeout=0.0)
self._robot.torso.reset()
arm.reset()
arm.wait_for_motion_done()
return 'succeeded'
class PickUp(smach.State):
def __init__(self, robot, arm, grab_entity):
"""
Pick up an item given an arm and an entity to be picked up
:param robot: robot to execute this state with
:param arm: Designator that resolves to the arm to grab the grab_entity with. E.g. UnoccupiedArmDesignator
:param grab_entity: Designator that resolves to the entity to grab. e.g EntityByIdDesignator
"""
smach.State.__init__(self,
outcomes=['succeeded', 'failed'],
output_keys=['arm'])
# Assign member variables
self.robot = robot
self.arm_designator = arm
self.grab_entity_designator = grab_entity
self._gpd = GraspPointDeterminant(robot)
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 in map frame
goal_map = msgs.Point(0, 0, 0)
try:
# Transform to base link frame
goal_bl = transformations.tf_transform(
goal_map,
grab_entity.id,
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'
# Make sure the torso and the arm are done
self.robot.torso.wait_for_motion_done()
arm.wait_for_motion_done()
# This is needed because the head is not entirely still when the
# look_at_point function finishes
rospy.sleep(rospy.Duration(0.5))
# Update the entity (position)
segm_res = self.robot.ed.update_kinect("%s" % grab_entity.id)
# 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.position.x -
grab_entity.pose.position.x,
updated_grab_entity.pose.position.y -
grab_entity.pose.position.y,
updated_grab_entity.pose.position.z -
grab_entity.pose.position.z))
grab_entity = updated_grab_entity
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Grasp point determination
grasp_pose = self._gpd.get_grasp_pose(grab_entity, arm)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# goal in map frame
goal_map = msgs.Point(0, 0, 0)
#try:
# In case grasp point determination didn't work
if grasp_pose is None:
if self.robot.tf_listener.waitForTransform(
grab_entity.id, self.robot.robot_name + "/base_link"):
# Transform to base link frame
goal_bl = transformations.tf_transform(
goal_map,
grab_entity.id,
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
if self.robot.tf_listener.waitForTransform(
"/map", self.robot.robot_name + "/base_link"):
# Transform to base link frame
goal_bl = transformations.tf_transform(
msgs.Point(grasp_pose.p.x(), grasp_pose.p.y(),
grasp_pose.p.z()),
"/map",
self.robot.robot_name + "/base_link",
tf_listener=self.robot.tf_listener)
if goal_bl is None:
return 'failed'
else:
return 'failed'
#except tf.Exception 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.x,
goal_bl.y,
goal_bl.z,
0,
0,
0,
frame_id='/' + self.robot.robot_name + '/base_link',
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
# rospy.loginfo('Start lifting')
if arm.side == "left":
roll = 0.3
else:
roll = -0.3
if not arm.send_goal(
goal_bl.x,
goal_bl.y,
goal_bl.z + 0.05,
roll,
0.0,
0.0,
frame_id='/' + self.robot.robot_name + '/base_link',
timeout=20,
allowed_touch_objects=[grab_entity.id]):
rospy.logerr('Failed lift')
# Retract
# rospy.loginfo('Start retracting')
if arm.side == "left":
roll = 0.6
else:
roll = -0.6
if not arm.send_goal(
goal_bl.x - 0.1,
goal_bl.y,
goal_bl.z + 0.05,
roll,
0.0,
0.0,
frame_id='/' + self.robot.robot_name + '/base_link',
timeout=0.0,
allowed_touch_objects=[grab_entity.id]):
rospy.logerr('Failed retract')
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()
# Carrying pose
# rospy.loginfo('start moving to carrying pose')
arm.send_joint_goal('carrying_pose', timeout=0.0)
# Reset head
self.robot.head.cancel_goal()
return 'succeeded'
def execute(self, userdata): #robot, item_to_place, placement_pose, arm):
item_to_place = self._item_to_place_designator.resolve()
if not item_to_place:
rospy.logerr("Could not resolve item_to_place")
#return "failed"
placement_pose = self._placement_pose_designator.resolve()
if not placement_pose:
rospy.logerr("Could not resolve placement_pose")
return "failed"
arm = self._arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
rospy.loginfo("Placing")
# placement_pose is a PoseStamped
place_pose_bl = transformations.tf_transform(placement_pose.pose.position,
placement_pose.header.frame_id,
self._robot.robot_name+'/base_link',
tf_listener=self._robot.tf_listener)
# Wait for torso and arm to finish their motions
self._robot.torso.wait_for_motion_done()
arm.wait_for_motion_done()
try:
height = place_pose_bl.z
except KeyError:
height = 0.8
# Pre place
if not arm.send_goal(place_pose_bl.x, place_pose_bl.y, height+0.2, 0.0, 0.0, 0.0,
timeout=10, pre_grasp=False, frame_id="/{0}/base_link".format(self._robot.robot_name)):
# If we can't place, try a little closer
place_pose_bl.x -= 0.05
rospy.loginfo("Retrying preplace")
if not arm.send_goal(place_pose_bl.x, place_pose_bl.y, height+0.2, 0.0, 0.0, 0.0,
timeout=10, pre_grasp=False, frame_id="/{0}/base_link".format(self._robot.robot_name)):
rospy.logwarn("Cannot pre-place the object")
arm.cancel_goals()
return 'failed'
# Place
if not arm.send_goal(place_pose_bl.x, place_pose_bl.y, height+0.15, 0.0, 0.0, 0.0,
timeout=10, pre_grasp=False, frame_id="/{0}/base_link".format(self._robot.robot_name)):
rospy.logwarn("Cannot place the object, dropping it...")
place_entity = arm.occupied_by
if not place_entity:
rospy.logerr("Arm not holding an entity to place. This should never happen")
else:
self._robot.ed.update_entity(place_entity.id, posestamped=placement_pose)
arm.occupied_by = None
# Open gripper
# Since we cannot reliably wait for the gripper, just set this timeout
arm.send_gripper_goal('open', timeout=2.0)
arm.occupied_by = None
# Retract
arm.send_goal(place_pose_bl.x - 0.1, place_pose_bl.y, place_pose_bl.z + 0.05, 0.0, 0.0, 0.0,
frame_id='/'+self._robot.robot_name+'/base_link',
timeout=0.0)
self._robot.base.force_drive(-0.125, 0, 0, 1.5)
if not arm.wait_for_motion_done(timeout=5.0):
rospy.logwarn('Retraction failed')
arm.cancel_goals()
# Close gripper
arm.send_gripper_goal('close', timeout=0.0)
self._robot.torso.reset()
arm.reset()
arm.wait_for_motion_done()
return 'succeeded'