def test_delivery(robot):
from robot_skills.util.kdl_conversions import kdlFrameStampedFromXYZRPY
robot.ed.update_entity(id="one", kdlFrameStamped=kdlFrameStampedFromXYZRPY(x=1.0, y=0, frame_id="/map"), type="waypoint")
robot.ed.update_entity(id="two", kdlFrameStamped=kdlFrameStampedFromXYZRPY(x=-1.2, y=0.0, frame_id="/map"), type="waypoint")
robot.ed.update_entity(id="three", kdlFrameStamped=kdlFrameStampedFromXYZRPY(x=1.950, y=1.551, frame_id="/map"), type="waypoint")
global ORDERS
ORDERS = {"beverage":{"name":"coke", "location":"one"}, "combo":{"name":"pringles and chocolate", "location":"two"}}
deliver = smach.StateMachine(outcomes=['done', 'aborted'])
with deliver:
smach.StateMachine.add('DELIVER_BEVERAGE', DeliverOrderWithBasket(robot, "beverage"), transitions={'succeeded':'NAVIGATE_TO_KITCHEN', 'failed':'NAVIGATE_TO_KITCHEN'})
smach.StateMachine.add('NAVIGATE_TO_KITCHEN', states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id="kitchen"), radius = WAYPOINT_RADIUS),
transitions={'arrived': 'DELIVER_COMBO', 'unreachable':'DELIVER_COMBO', 'goal_not_defined':'DELIVER_COMBO'})
smach.StateMachine.add('DELIVER_COMBO', DeliverOrderWithBasket(robot, "combo"), transitions={'succeeded':'done', 'failed':'aborted'})
deliver.execute(None)
def determine_points_of_interest_with_area(self, e, area):
""" Determines the points of interest using an area
:param e:
:param area:
:return:
"""
# Just to be sure, copy e
e = self.robot.ed.get_entity(id=e.id, parse=True)
# We want to give it a convex hull using the designated area
if area in e.volumes:
box = e.volumes[area]
else:
rospy.logwarn("Entity {0} has no volume named {1}".format(
e.id, area))
if not self._candidate_list_obj:
for y in [
0, self._spacing, -self._spacing, 2.0 * self._spacing,
-2.0 * self._spacing
]:
if y < box.min_corner.y() or y > box.max_corner.y():
rospy.logerr(
"Spacing of empty spot designator is too large!!!")
continue
frame_stamped = kdlFrameStampedFromXYZRPY(
x=box.max_corner.x() - self._edge_distance,
y=y,
z=box.min_corner.z() - 0.04, # 0.04 is the usual offset)
frame_id=e.id)
# e.convex_hull = []
# e.convex_hull.append(gm.Point(box['min']['x'], box['min']['y'], box['min']['z'])) # 1
# e.convex_hull.append(gm.Point(box['max']['x'], box['min']['y'], box['min']['z'])) # 2
# e.convex_hull.append(gm.Point(box['max']['x'], box['max']['y'], box['min']['z'])) # 3
# e.convex_hull.append(gm.Point(box['min']['x'], box['max']['y'], box['min']['z'])) # 4
#
# # Make sure we overwrite the e.z_max
# e.z_max = box['min']['z'] - 0.04 # 0.04 is the usual offset
# return self.determinePointsOfInterest(e)
self._candidate_list_obj.append(frame_stamped)
# publish marker
self.marker_array = MarkerArray()
for frame_stamped in self._candidate_list_obj:
self.marker_array.markers.append(
self.create_marker(frame_stamped.frame.p.x(),
frame_stamped.frame.p.y(),
frame_stamped.frame.p.z(), e.id))
self.marker_pub.publish(self.marker_array)
return self._candidate_list_obj
def determine_points_of_interest_with_area(self, e, area):
""" Determines the points of interest using an area
:param e:
:param area:
:return:
"""
# Just to be sure, copy e
e = self.robot.ed.get_entity(id=e.id, parse=True)
# We want to give it a convex hull using the designated area
if area in e.volumes:
box = e.volumes[area]
else:
rospy.logwarn("Entity {0} has no volume named {1}".format(e.id, area))
if not self._candidate_list_obj:
for y in [0, self._spacing, -self._spacing, 2.0 * self._spacing, -2.0 * self._spacing]:
if y < box.min_corner.y() or y > box.max_corner.y():
rospy.logerr("Spacing of empty spot designator is too large!!!")
continue
fs = kdlFrameStampedFromXYZRPY(frame_id=e.id,
x=box.max_corner.x() - self._edge_distance,
y=y,
z=box.min_corner.z() - 0.04) # 0.04 is the usual z offset
self._candidate_list_obj.append(fs)
# publish marker
self.marker_array = MarkerArray()
for ps in self._candidate_list_obj:
self.marker_array.markers.append(self.create_marker(fs.vector.p.x,
fs.vector.p.y,
fs.vector.p.z,
e.id))
self.marker_pub.publish(self.marker_array)
return self._candidate_list_obj
def execute(self, userdata=None):
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_fs = self._placement_pose_designator.resolve()
if not placement_fs:
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 PyKDL.Frame
place_pose_bl = placement_fs.projectToFrame(
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.frame.p.z()
except KeyError:
height = 0.8
# Pre place
if not arm.send_goal(kdlFrameStampedFromXYZRPY(
place_pose_bl.frame.p.x(),
place_pose_bl.frame.p.y(),
height + 0.15,
0.0,
0.0,
0.0,
frame_id="/{0}/base_link".format(self._robot.robot_name)),
timeout=10,
pre_grasp=True):
# If we can't place, try a little closer
place_pose_bl.frame.p.x(place_pose_bl.frame.p.x() - 0.025)
rospy.loginfo("Retrying preplace")
if not arm.send_goal(kdlFrameStampedFromXYZRPY(
place_pose_bl.frame.p.x(),
place_pose_bl.frame.p.y(),
height + 0.15,
0.0,
0.0,
0.0,
frame_id="/{0}/base_link".format(self._robot.robot_name)),
timeout=10,
pre_grasp=True):
rospy.logwarn("Cannot pre-place the object")
arm.cancel_goals()
return 'failed'
# Place
if not arm.send_goal(kdlFrameStampedFromXYZRPY(
place_pose_bl.frame.p.x(),
place_pose_bl.frame.p.y(),
height + 0.1,
0.0,
0.0,
0.0,
frame_id="/{0}/base_link".format(self._robot.robot_name)),
timeout=10,
pre_grasp=False):
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,
frame_stamped=placement_fs)
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(kdlFrameStampedFromXYZRPY(
place_pose_bl.frame.p.x() - 0.1,
place_pose_bl.frame.p.y(),
place_pose_bl.frame.p.z() + 0.15,
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'
def _update_navigation(self):
"""Set the navigation plan to match the breadcrumbs collected into self._breadcrumbs.
This list has all the Entity's of where the operator has been"""
self._robot.head.cancel_goal()
f = self._robot.base.get_location().frame
robot_position = f.p
operator_position = self._last_operator._pose.p
''' Define end goal constraint, solely based on the (old) operator position '''
p = PositionConstraint()
p.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (operator_position.x(),
operator_position.y(),
self._operator_radius)
o = OrientationConstraint()
if self._operator_id:
o.frame = self._operator_id
else:
o.frame = 'map'
o.look_at = kdl_conversions.kdlVectorToPointMsg(
self._last_operator.pose.frame.p)
''' Calculate global plan from robot position, through breadcrumbs, to the operator '''
res = 0.05
kdl_plan = []
previous_point = robot_position
if self._operator:
breadcrumbs = self._breadcrumbs + [self._operator]
else:
breadcrumbs = self._breadcrumbs + [self._last_operator]
for crumb in breadcrumbs:
assert isinstance(crumb, Entity)
diff = crumb._pose.p - previous_point
dx, dy = diff.x(), diff.y()
length = crumb.distance_to_2d(previous_point)
if length != 0:
dx_norm = dx / length
dy_norm = dy / length
yaw = math.atan2(dy, dx)
start = 0
end = int(length / res)
for i in range(start, end):
x = previous_point.x() + i * dx_norm * res
y = previous_point.y() + i * dy_norm * res
kdl_plan.append(
kdl_conversions.kdlFrameStampedFromXYZRPY(x=x,
y=y,
z=0,
yaw=yaw))
previous_point = copy.deepcopy(crumb._pose.p)
# Delete the elements from the plan within the operator radius from the robot
cutoff = int(self._operator_radius / (2.0 * res))
if len(kdl_plan) > cutoff:
del kdl_plan[-cutoff:]
ros_plan = frame_stampeds_to_pose_stampeds(kdl_plan)
# Check if plan is valid. If not, remove invalid points from the path
if not self._robot.base.global_planner.checkPlan(ros_plan):
print "Breadcrumb plan is blocked, removing blocked points"
# Go through plan from operator to robot and pick the first unoccupied point as goal point
ros_plan = [
point for point in ros_plan
if self._robot.base.global_planner.checkPlan([point])
]
self._visualize_plan(ros_plan)
self._robot.base.local_planner.setPlan(ros_plan, p, o)
# First make sure the arms are in a known state
for side, arm in robot.arms.items():
robot.speech.speak("I will first reset my {} arm".format(side))
arm.reset()
for side, arm in robot.arms.items():
failed_actions = []
robot.speech.speak("I will test my {} arm".format(side))
if not arm.operational:
rospy.logerr("{} arm is not operational".format(side))
sys.exit(-1)
# Notice the - for the right arm's Y position.
if arm == robot.leftArm:
goal1 = kdl_conversions.kdlFrameStampedFromXYZRPY(0.342, 0.125, 0.748, 0, 0, 0, "/"+robot.robot_name+"/base_link")
if arm == robot.rightArm:
goal1 = kdl_conversions.kdlFrameStampedFromXYZRPY(0.342, -0.125, 0.748, 0, 0, 0, "/"+robot.robot_name+"/base_link")
robot.speech.speak("Moving {} arm to dummy goal pose".format(side))
if not arm.send_goal(goal1):
robot.speech.speak("{} arm could not reach dummy goal pose".format(side))
failed_actions += [goal1]
arm.wait_for_motion_done()
if arm.send_gripper_goal("open"):
robot.speech.speak("My {} hand is now open".format(side))
else:
robot.speech.speak("Could not open {} hand".format(side))
failed_actions += ["open gripper"]
arm.wait_for_motion_done()