def test_delivery(robot):
from robot_skills.util.kdl_conversions import kdl_frame_stamped_from_XYZRPY
robot.ed.update_entity(id="one",
kdl_frame_stamped=kdl_frame_stamped_from_XYZRPY(
x=1.0, y=0, frame_id="/map"),
type="waypoint")
robot.ed.update_entity(id="two",
kdl_frame_stamped=kdl_frame_stamped_from_XYZRPY(
x=-1.2, y=0.0, frame_id="/map"),
type="waypoint")
robot.ed.update_entity(id="three",
kdl_frame_stamped=kdl_frame_stamped_from_XYZRPY(
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(self, center_frame, z_max, convex_hull):
"""
Determine candidates for place poses
:param center_frame: kdl.Frame, center of the Entity to place on top of
:param z_max: float, height of the entity to place on, w.r.t. the entity
:param convex_hull: [kdl.Vector], convex hull of the entity
:return: [FrameStamped] of candidates for placing
"""
points = []
if len(convex_hull) == 0:
rospy.logerr('determine_points_of_interest: Empty convex hull')
return []
# Convert convex hull to map frame
ch = offsetConvexHull(convex_hull, center_frame)
# Loop over hulls
self.marker_array.markers = []
for i in xrange(len(ch)):
j = (i + 1) % len(ch)
dx = ch[j].x() - ch[i].x()
dy = ch[j].y() - ch[i].y()
length = kdl.diff(ch[j], ch[i]).Norm()
d = self._edge_distance
while d < (length - self._edge_distance):
# Point on edge
xs = ch[i].x() + d / length * dx
ys = ch[i].y() + d / length * dy
# Shift point inwards and fill message
fs = kdl_frame_stamped_from_XYZRPY(
x=xs - dy / length * self._edge_distance,
y=ys + dx / length * self._edge_distance,
z=center_frame.p.z() + z_max,
frame_id="/map")
# It's nice to put an object on the middle of a long edge. In case of a cabinet, e.g., this might
# prevent the robot from hitting the cabinet edges
# print "Length: {}, edge score: {}".format(length, min(d, length-d))
setattr(fs, 'edge_score', min(d, length - d))
points += [fs]
self.marker_array.markers.append(
self.create_marker(fs.frame.p.x(), fs.frame.p.y(),
fs.frame.p.z()))
# ToDo: check if still within hull???
d += self._spacing
self.marker_pub.publish(self.marker_array)
return points
def determine_points_of_interest(self, center_frame, z_max, convex_hull):
"""
Determine candidates for place poses
:param center_frame: kdl.Frame, center of the Entity to place on top of
:param z_max: float, height of the entity to place on, w.r.t. the entity
:param convex_hull: [kdl.Vector], convex hull of the entity
:return: [FrameStamped] of candidates for placing
"""
points = []
if len(convex_hull) == 0:
rospy.logerr('determine_points_of_interest: Empty convex hull')
return []
# Convert convex hull to map frame
ch = offsetConvexHull(convex_hull, center_frame)
# Loop over hulls
self.marker_array.markers = []
for i in xrange(len(ch)):
j = (i + 1) % len(ch)
dx = ch[j].x() - ch[i].x()
dy = ch[j].y() - ch[i].y()
length = kdl.diff(ch[j], ch[i]).Norm()
d = self._edge_distance
while d < (length - self._edge_distance):
# Point on edge
xs = ch[i].x() + d / length * dx
ys = ch[i].y() + d / length * dy
# Shift point inwards and fill message
fs = kdl_frame_stamped_from_XYZRPY(x=xs - dy / length * self._edge_distance,
y=ys + dx / length * self._edge_distance,
z=center_frame.p.z() + z_max,
frame_id="/map")
# It's nice to put an object on the middle of a long edge. In case of a cabinet, e.g., this might
# prevent the robot from hitting the cabinet edges
# print "Length: {}, edge score: {}".format(length, min(d, length-d))
setattr(fs, 'edge_score', min(d, length-d))
points += [fs]
self.marker_array.markers.append(self.create_marker(fs.frame.p.x(), fs.frame.p.y(), fs.frame.p.z()))
# ToDo: check if still within hull???
d += self._spacing
self.marker_pub.publish(self.marker_array)
return points
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)
# 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 = kdl_frame_stamped_from_XYZRPY(
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
frame_stamped = kdl_frame_stamped_from_XYZRPY(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)
# 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 = kdl_frame_stamped_from_XYZRPY(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 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 = kdl_frame_stamped_from_XYZRPY(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(kdl_frame_stamped_from_XYZRPY(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(kdl_frame_stamped_from_XYZRPY(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
place_pose_bl = placement_fs.projectToFrame(self._robot.robot_name+'/base_link',
tf_listener=self._robot.tf_listener)
if not arm.send_goal(kdl_frame_stamped_from_XYZRPY(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
place_pose_bl = placement_fs.projectToFrame(self._robot.robot_name+'/base_link',
tf_listener=self._robot.tf_listener)
arm.send_goal(kdl_frame_stamped_from_XYZRPY(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)
arm.wait_for_motion_done()
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)
arm.reset()
arm.wait_for_motion_done()
self._robot.torso.reset()
self._robot.torso.wait_for_motion_done()
return 'succeeded'
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(kdl_frame_stamped_from_XYZRPY(
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(kdl_frame_stamped_from_XYZRPY(
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
place_pose_bl = placement_fs.projectToFrame(
self._robot.robot_name + '/base_link',
tf_listener=self._robot.tf_listener)
if not arm.send_goal(kdl_frame_stamped_from_XYZRPY(
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
place_pose_bl = placement_fs.projectToFrame(
self._robot.robot_name + '/base_link',
tf_listener=self._robot.tf_listener)
arm.send_goal(kdl_frame_stamped_from_XYZRPY(
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)
arm.wait_for_motion_done()
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)
arm.reset()
arm.wait_for_motion_done()
self._robot.torso.reset()
self._robot.torso.wait_for_motion_done()
return 'succeeded'
# 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.kdl_frame_stamped_from_XYZRPY(0.342, 0.125, 0.748, 0, 0, 0, "/"+robot.robot_name+"/base_link")
if arm == robot.rightArm:
goal1 = kdl_conversions.kdl_frame_stamped_from_XYZRPY(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()
def execute(self, userdata=None):
""" A concurrent state machine which follows and afterwards deletes breadcrumbs from the buffer variable
:return: follow_bread if the list of breadcrumbs is not empty
no_follow_bread_ask_finalize if the single remaining breadcrumb is the operator
no_follow_bread_recovery if buffer is completely empty
"""
while self._buffer:
if self.preempt_requested():
return 'aborted'
crumb = self._buffer.popleft()
if not self._breadcrumb or self._breadcrumb[-1].crumb.distance_to_2d(
crumb._pose.p) > self._breadcrumb_distance:
self._breadcrumb.append(CrumbWaypoint(crumb))
rospy.loginfo("Self._breadcrumb length: {}".format(len(self._breadcrumb)))
# We don't want to base this on distance since that relies on the operator behavior
# if self._robot.base.local_planner.getDistanceToGoal() > 0.65: # 2.0: # len(buffer) > 5:
# self._have_followed = True
robot_position = self._robot.base.get_location().frame
if self._breadcrumb:
self._operator = self._breadcrumb[-1].crumb
else:
return 'no_follow_bread_recovery'
#
# if self._operator:
# operator = self._robot.ed.get_entity(id=self._operator.id)
# while True: # Should be timer, I think
try:
_, semantics = self._robot.hmi.query(description="", grammar="T[true] -> %s stop" % self._robot.robot_name, target="T")
if semantics:
return 'no_follow_bread_ask_finalize'
except hmi.TimeoutException:
rospy.logwarn("[HMC] Listening timeout")
# return 'no_follow_bread_ask_finalize'
# if operator:
# rospy.loginfo("Distance to operator: {}".format(operator.distance_to_2d(robot_position.p)))
# if self._have_followed and operator.distance_to_2d(robot_position.p) < 1.0:
# self._have_followed = False
# return 'no_follow_bread_ask_finalize'
# self._breadcrumb = [crwp for crwp in self._breadcrumb
# if crwp.crumb.distance_to_2d(robot_position.p) > self._lookat_radius]
current_index = -1
for i, crumb in enumerate(self._breadcrumb):
if crumb.crumb.distance_to_2d(robot_position.p) < self._lookat_radius:
current_index = i
# throw away all breadcrumbs before where we are
self._breadcrumb = self._breadcrumb[current_index+1:]
f = self._robot.base.get_location().frame
previous_point = f.p
operator_position = self._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()
o.frame = self._operator.id
''' Calculate global plan from robot position, through breadcrumbs, to the operator '''
for crumb_waypoint in self._breadcrumb:
crumb = crumb_waypoint.crumb
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 / self._resolution)
for i in range(start, end):
x = previous_point.x() + i * dx_norm * self._resolution
y = previous_point.y() + i * dy_norm * self._resolution
kdl_pose = kdl_conversions.kdl_frame_stamped_from_XYZRPY(x=x, y=y, z=0, yaw=yaw)
crumb_waypoint.waypoint = kdl_conversions.kdl_frame_stamped_to_pose_stamped_msg(kdl_pose)
previous_point = copy.deepcopy(crumb._pose.p)
if len(self._breadcrumb) > 0:
ros_plan = [crwp.waypoint for crwp in self._breadcrumb]
if not self._robot.base.global_planner.checkPlan(ros_plan):
rospy.loginfo("Breadcrumb plan is blocked, removing blocked points")
rospy.loginfo("Breadcrumbs before removing blocked points: {}".format(len(ros_plan)))
ros_plan = [crwp.waypoint for crwp in self._breadcrumb
if self._robot.base.global_planner.checkPlan([crwp.waypoint])]
rospy.loginfo("Breadcrumbs length after removing blocked points: {}".format(len(ros_plan)))
# if the new ros plan is empty, we do have an operator but there are no breadcrumbs that we can reach
# if not ros_plan:
# return 'no_follow_bread_recovery'
else:
return 'no_follow_bread_recovery'
buffer_msg = Marker()
buffer_msg.type = Marker.POINTS
buffer_msg.header.stamp = rospy.get_rostime()
buffer_msg.header.frame_id = "/map"
buffer_msg.id = 0
buffer_msg.action = Marker.DELETEALL
self._breadcrumb_pub.publish(buffer_msg)
buffer_msg = Marker()
buffer_msg.type = Marker.POINTS
buffer_msg.scale.x = 0.05
buffer_msg.scale.y = 0.05
buffer_msg.header.stamp = rospy.get_rostime()
buffer_msg.header.frame_id = "/map"
buffer_msg.color.a = 1
buffer_msg.color.r = 1
buffer_msg.color.g = 0
buffer_msg.color.b = 0
# buffer_msg.lifetime = rospy.Time(1)
buffer_msg.id = 0
buffer_msg.action = Marker.ADD
for crumb in self._breadcrumb:
buffer_msg.points.append(crumb.waypoint.pose.position)
self._breadcrumb_pub.publish(buffer_msg)
# line_strip = Marker()
# line_strip.type = Marker.LINE_STRIP
# line_strip.scale.x = 0.05
# line_strip.header.frame_id = "/map"
# line_strip.header.stamp = rospy.Time.now()
# line_strip.color.a = 1
# line_strip.color.r = 0
# line_strip.color.g = 1
# line_strip.color.b = 1
# line_strip.id = 0
# line_strip.action = Marker.ADD
#
# for crwp in ros_plan:
# line_strip.points.append(crwp)
#self._plan_marker_pub.publish(line_strip)
self._robot.base.local_planner.setPlan(ros_plan, p, o)
rospy.sleep(rospy.Duration(0.5))
return 'follow_bread'
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.kdl_frame_stamped_from_XYZRPY(
0.342, 0.125, 0.748, 0, 0, 0,
"/" + robot.robot_name + "/base_link")
if arm == robot.rightArm:
goal1 = kdl_conversions.kdl_frame_stamped_from_XYZRPY(
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))
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.kdl_vector_to_point_msg(
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.kdl_frame_stamped_from_XYZRPY(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)
def execute(self, userdata):
""" A concurrent state machine which follows and afterwards deletes breadcrumbs from the buffer variable
:return: follow_bread if the list of breadcrumbs is not empty
no_follow_bread_ask_finalize if the single remaining breadcrumb is the operator
no_follow_bread_recovery if buffer is completely empty
"""
operator = None
while self._buffer:
crumb = self._buffer.popleft()
if not self._breadcrumb or self._buffer[-1].crumb.distance_to_2d(
crumb._pose.p) > self._breadcrumb_distance:
self._breadcrumb.append(CrumbWaypoint(crumb))
# if self._buffer[-1].distance_to_2d(crumb._pose.p) > self._breadcrumb_distance:
# self._breadcrumb.append(crumb)
# rospy.loginfo("Appending operator, length of buffer: {}".format(len(buffer)))
#
# self._breadcrumb.append((crumb, None)) # Don't use a tuple, that doesn't work...
print self._breadcrumb
# buffer = userdata.buffer_follow_in
if self._robot.base.local_planner.getDistanceToGoal(
) > 2.0: # len(buffer) > 5:
self._have_followed = True
robot_position = self._robot.base.get_location().frame
if not self._breadcrumb:
rospy.sleep(
2
) # magic number, not necessary, can also return a different outcome that does not lead to
# recovery/ask finalize
if self._breadcrumb:
self._operator = self._breadcrumb[-1].crumb
else:
return 'no_follow_bread_recovery'
if self._operator: #and len(buffer) > 1:
operator = self._robot.ed.get_entity(id=self._operator.id)
rospy.loginfo(
"Buffer length at start of FollowBread: {}, have followed: {}".
format(len(self._breadcrumb), self._have_followed))
if operator:
rospy.loginfo("Distance to operator: {}".format(
operator.distance_to_2d(robot_position.p)))
if self._have_followed and operator.distance_to_2d(
robot_position.p) < 1.0:
self._have_followed = False
return 'no_follow_bread_ask_finalize'
rospy.loginfo("Buffer length before radius check: {}".format(
len(self._breadcrumb)))
# Throw away crumbs that are too close
self._breadcrumb = [
crwp for crwp in self._breadcrumb if
crwp.crumb.distance_to_2d(robot_position.p) > self._lookat_radius
]
rospy.loginfo("Buffer length after radius check: {}".format(
len(self._buffer)))
if not self._breadcrumb:
return 'no_follow_bread_recovery'
f = self._robot.base.get_location().frame
previous_point = f.p
operator_position = self._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()
o.frame = self._operator.id
''' Calculate global plan from robot position, through breadcrumbs, to the operator '''
res = 0.05 # magic number number 2
# ros_plan = []
for crumb_waypoint in self._breadcrumb:
crumb = crumb_waypoint.crumb
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_pose = kdl_conversions.kdl_frame_stamped_from_XYZRPY(
x=x, y=y, z=0, yaw=yaw)
crumb_waypoint.waypoint = kdl_conversions.kdl_frame_stamped_to_pose_stamped_msg(
kdl_pose)
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 = [kdl_conversions.kdl_frame_stamped_to_pose_stamped_msg(frame) for frame in kdl_plan]
# Check if plan is valid. If not, remove invalid points from the path
if len(self._breadcrumb) > 0:
ros_plan = [crwp.waypoint for crwp in self._breadcrumb]
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._breadcrumb = [
crwp for crwp in self._breadcrumb if
self._robot.base.global_planner.checkPlan([crwp.waypoint])
]
# buffer_msg = Marker()
# buffer_msg.type = Marker.POINTS
# buffer_msg.scale.x = 0.05
# buffer_msg.scale.y = 0.05
# buffer_msg.header.stamp = rospy.get_rostime()
# buffer_msg.header.frame_id = "/map"
# buffer_msg.color.a = 1
# buffer_msg.color.r = 1
# buffer_msg.color.g = 0
# buffer_msg.color.b = 0
# #buffer_msg.lifetime = rospy.Time(.0)
# buffer_msg.id = 0
# buffer_msg.action = Marker.ADD
#
# for crumb in buffer:
# buffer_msg.points.append(kdl_conversions.kdl_vector_to_point_msg(crumb.pose.frame.p))
line_strip = Marker()
line_strip.type = Marker.LINE_STRIP
line_strip.scale.x = 0.05
line_strip.header.frame_id = "/map"
line_strip.header.stamp = rospy.Time.now()
line_strip.color.a = 1
line_strip.color.r = 0
line_strip.color.g = 1
line_strip.color.b = 1
line_strip.id = 0
line_strip.action = Marker.ADD
# Push back all pnts
for pose_stamped in ros_plan:
line_strip.points.append(pose_stamped.pose.position)
self._plan_marker_pub.publish(line_strip)
# self._breadcrumb_pub.publish(buffer_msg)
self._robot.base.local_planner.setPlan(ros_plan, p, o)
# userdata.buffer_follow_out = buffer
# ToDo: make nice
rospy.sleep(rospy.Duration(0.5))
return 'follow_bread'
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.kdl_vector_to_point_msg(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.kdl_frame_stamped_from_XYZRPY(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)
