def _turn_towards_operator(self):
robot_position = self._robot.base.get_location().pose.position
operator_position = self._last_operator.pose.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 = self._last_operator.pose.position
dx = operator_position.x - robot_position.x
dy = operator_position.y - robot_position.y
yaw = math.atan2(dy, dx)
plan = [
msg_constructors.PoseStamped(x=robot_position.x,
y=robot_position.y,
z=0,
yaw=yaw)
]
print "Operator within self._lookat_radius"
self._robot.base.local_planner.setPlan(plan, p, o)
def _operator_standing_still_for_x_seconds(self, timeout):
if not self._operator:
return False
operator_current_pose = self._operator.pose
operator_current_pose_stamped = msg_constructors.PoseStamped(
x=operator_current_pose.position.x,
y=operator_current_pose.position.y)
#print "Operator position: %s" % self._operator.pose.position
if not self._last_operator_pose_stamped:
self._last_operator_pose_stamped = operator_current_pose_stamped
else:
# Compare the pose with the last pose and update if difference is larger than x
if math.hypot(
operator_current_pose_stamped.pose.position.x -
self._last_operator_pose_stamped.pose.position.x,
operator_current_pose_stamped.pose.position.y -
self._last_operator_pose_stamped.pose.position.y) > 0.15:
# Update the last pose
# print "Last pose stamped operator (%f,%f) at %f secs"%(self._last_operator_pose_stamped.pose.position.x, self._last_operator_pose_stamped.pose.position.y, self._last_operator_pose_stamped.header.stamp.secs)
self._last_operator_pose_stamped = operator_current_pose_stamped
else:
print "Operator is standing still for %f seconds" % (
operator_current_pose_stamped.header.stamp -
self._last_operator_pose_stamped.header.stamp).to_sec()
# Check whether we passed the timeout
if (operator_current_pose_stamped.header.stamp -
self._last_operator_pose_stamped.header.stamp
).to_sec() > timeout:
return True
return False
def _resolve(self):
place_location = self.place_location_designator.resolve()
# points_of_interest = []
if self._area:
points_of_interest = self.determine_points_of_interest_with_area(place_location, self._area)
else:
points_of_interest = self.determinePointsOfInterest(place_location)
def is_poi_occupied(poi):
entities_at_poi = self.robot.ed.get_entities(center_point=poi, radius=self._spacing)
return not any(entities_at_poi)
open_POIs = filter(is_poi_occupied, points_of_interest)
def distance_to_poi_area(poi):
#Derived from navigate_to_place
radius = math.hypot(self.robot.grasp_offset.x, self.robot.grasp_offset.y)
x = poi.point.x
y = poi.point.y
ro = "(x-%f)^2+(y-%f)^2 < %f^2"%(x, y, radius+0.075)
ri = "(x-%f)^2+(y-%f)^2 > %f^2"%(x, y, radius-0.075)
pos_constraint = PositionConstraint(constraint=ri+" and "+ro, frame="/map")
plan_to_poi = self.robot.base.global_planner.getPlan(pos_constraint)
if plan_to_poi:
distance = len(plan_to_poi)
print "Distance: %s"%distance
else:
distance = None
return distance
# List with tuples containing both the POI and the distance the
# robot needs to travel in order to place there
open_POIs_dist = [(poi, distance_to_poi_area(poi)) for poi in open_POIs]
# Feasible POIS: discard
feasible_POIs = []
for tup in open_POIs_dist:
if tup[1]:
feasible_POIs.append(tup)
if any(feasible_POIs):
feasible_POIs.sort(key=lambda tup: tup[1]) # sorts in place
best_poi = feasible_POIs[0][0] # Get the POI of the best match
placement = msg_constructors.PoseStamped(pointstamped=best_poi)
# rospy.loginfo("Placement = {0}".format(placement).replace('\n', ' '))
selection = self.create_selection_marker(placement)
self.marker_pub.publish(MarkerArray([selection]))
return placement
else:
rospy.logerr("Could not find an empty spot")
return None
def load_waypoints(robot, filename="/param/locations.yaml"):
rp = rospkg.rospack.RosPack()
restaurant_package = rp.get_path("challenge_restaurant")
locations = rosparam.load_file(restaurant_package+filename)
rospy.set_param("/restaurant_locations/", locations)
for tablename in tables.values():
location = locations[0][0][0][0]['locations'][tablename] #Don't ask why there's so many subindices to use...
base_pose = msg_constructors.PoseStamped(location['x'], location['y'], z=0)
base_pose.pose.orientation = transformations.euler_z_to_quaternion(location['phi'])
visualize_location(base_pose, tablename)
robot.ed.update_entity(id=tablename, kdlFrameStamped=FrameStamped(base_pose, "/map"), type="waypoint")
def _update_navigation(self):
self._robot.head.cancel_goal()
robot_position = self._robot.base.get_location().pose.position
operator_position = self._last_operator.pose.position
''' 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 = self._last_operator.pose.position
''' Calculate global plan from robot position, through breadcrumbs, to the operator '''
res = 0.05
plan = []
previous_point = robot_position
if self._operator:
breadcrumbs = self._breadcrumbs + [self._operator]
else:
breadcrumbs = self._breadcrumbs + [self._last_operator]
for crumb in breadcrumbs:
dx = crumb.pose.position.x - previous_point.x
dy = crumb.pose.position.y - previous_point.y
length = math.hypot(dx, dy)
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
plan.append(
msg_constructors.PoseStamped(x=x, y=y, z=0, yaw=yaw))
previous_point = crumb.pose.position
# Delete the elements from the plan within the operator radius from the robot
cutoff = int(self._operator_radius / (2.0 * res))
if len(plan) > cutoff:
del plan[-cutoff:]
# Check if plan is valid. If not, remove invalid points from the path
if not self._robot.base.global_planner.checkPlan(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
plan = [
point for point in plan
if self._robot.base.global_planner.checkPlan([point])
]
self._visualize_plan(plan)
self._robot.base.local_planner.setPlan(plan, p, o)