def _turn_towards_operator(self):
f = self._robot.base.get_location().frame
robot_position = f.p
operator_position = self._last_operator._pose.p
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)
dx = operator_position.x() - robot_position.x()
dy = operator_position.y() - robot_position.y()
yaw = math.atan2(dy, dx)
# ToDo: make nice!
pose = geometry_msgs.msg.PoseStamped()
pose.header.frame_id = "/map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = robot_position.x()
pose.pose.position.y = robot_position.y()
xx, yy, zz, ww = kdl.Rotation.RPY(0, 0, yaw).GetQuaternion()
pose.pose.orientation.x = xx
pose.pose.orientation.y = yy
pose.pose.orientation.z = zz
pose.pose.orientation.w = ww
plan = [pose]
print "Operator within self._lookat_radius"
self._robot.base.local_planner.setPlan(plan, p, o)
def execute(self, userdata=None):
door_1_position = self._robot.ed.get_entity(id=challenge_knowledge.reentry_door_1).pose.position
door_2_position = self._robot.ed.get_entity(id=challenge_knowledge.reentry_door_2).pose.position
door_1_constraint = PositionConstraint()
door_1_constraint.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2"% (door_1_position.x, door_1_position.y, 0.7)
door_2_constraint = PositionConstraint()
door_2_constraint.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (door_2_position.x, door_2_position.y, 0.7)
# TODO: make sure that waypoints exist!!!
# TODO: make sure door id designator is writeable
while True:
plan1 = self._robot.base.global_planner.getPlan(door_1_constraint)
plan2 = self._robot.base.global_planner.getPlan(door_2_constraint)
print "Plan 1 length: %i" % len(plan1)
print "Plan 2 length: %i" % len(plan2)
if len(plan1) < 3:
self._door_id_designator.writeable.write(challenge_knowledge.target_door_1)
return "door_found"
elif len(plan2) < 3:
self._door_id_designator.writeable.write(challenge_knowledge.target_door_2)
return "door_found"
if self.preempt_requested():
return 'preempted'
time.sleep(1)
def move(self, position_constraint_string, frame):
p = PositionConstraint()
p.constraint = position_constraint_string
p.frame = frame
plan = self.global_planner.getPlan(p)
if plan:
o = OrientationConstraint()
o.frame = frame
self.local_planner.setPlan(plan, p, o)
return plan
def move(self, position_constraint_string, frame):
p = PositionConstraint()
p.constraint = position_constraint_string
p.frame = frame
plan = self.global_planner.getPlan(p)
if plan:
o = OrientationConstraint()
o.frame = frame
self.local_planner.setPlan(plan, p, o)
return plan
def execute(self, userdata=None):
e = self.destination_designator.resolve()
if not e:
rospy.logerr(
"CheckDoorPassable::execute: No entity could be resolved from designator '%s'"
% self.destination_designator)
return None
try:
pose = e.data["pose"]
x = pose["x"]
y = pose["y"]
rz = e.data["pose"]["rz"]
except:
try:
x = e.pose.position.x
y = e.pose.position.y
except:
return None
pc = PositionConstraint(constraint="(x-%f)^2+(y-%f)^2 < %f^2" %
(x, y, 0.5),
frame="/map")
plan = self.robot.base.global_planner.getPlan(pc)
if plan and len(plan) < 3 and computePathLength(plan) < 3.0:
return "passable"
else:
return "blocked"
def generateConstraint(self):
arm = self.arm.resolve()
if arm == self.robot.arms['left']:
angle_offset = math.atan2(-self.robot.grasp_offset.y,
self.robot.grasp_offset.x)
elif arm == self.robot.arms['right']:
angle_offset = math.atan2(self.robot.grasp_offset.y,
self.robot.grasp_offset.x)
else:
raise IndexError('Arm not found')
radius = math.hypot(self.robot.grasp_offset.x,
self.robot.grasp_offset.y)
x = self.point.point.x
y = self.point.point.y
frame_id = self.point.header.frame_id
# Outer radius
radius -= 0.1
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)
pc = PositionConstraint(constraint=ri + " and " + ro, frame=frame_id)
oc = OrientationConstraint(look_at=Point(x, y, 0.0),
frame=frame_id,
angle_offset=angle_offset)
return pc, oc
def execute(self, userdata=None):
door_1_position = self._robot.ed.get_entity(
id=challenge_knowledge.reentry_door_1).pose.position
door_2_position = self._robot.ed.get_entity(
id=challenge_knowledge.reentry_door_2).pose.position
door_1_constraint = PositionConstraint()
door_1_constraint.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (
door_1_position.x, door_1_position.y, 0.7)
door_2_constraint = PositionConstraint()
door_2_constraint.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (
door_2_position.x, door_2_position.y, 0.7)
# TODO: make sure that waypoints exist!!!
# TODO: make sure door id designator is writeable
while True:
plan1 = self._robot.base.global_planner.getPlan(door_1_constraint)
plan2 = self._robot.base.global_planner.getPlan(door_2_constraint)
print "Plan 1 length: %i" % len(plan1)
print "Plan 2 length: %i" % len(plan2)
if len(plan1) < 3:
self._door_id_designator.writeable.write(
challenge_knowledge.target_door_1)
return "door_found"
elif len(plan2) < 3:
self._door_id_designator.writeable.write(
challenge_knowledge.target_door_2)
return "door_found"
if self.preempt_requested():
return 'preempted'
time.sleep(1)
def __init__(self, robot, entity_id, obstacle_radius):
smach.State.__init__(self, outcomes=["done", "timeout"])
self._robot = robot
try:
pose = robot.ed.get_entity(id=entity_id).pose
except Exception as e:
rospy.logerr(e)
sys.exit(1)
self.pc = PositionConstraint(frame="/map",
constraint="(x-%f)^2+(y-%f)^2 < 0.05" %
(pose.position.x, pose.position.y))
self.obstacle_radius = obstacle_radius
def _turn_towards_operator(self):
f = self._robot.base.get_location().frame
robot_position = f.p
operator_position = self._last_operator._pose.p
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)
dx = operator_position.x() - robot_position.x()
dy = operator_position.y() - robot_position.y()
yaw = math.atan2(dy, dx)
# ToDo: make nice!
pose = geometry_msgs.msg.PoseStamped()
pose.header.frame_id = "/map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = robot_position.x()
pose.pose.position.y = robot_position.y()
xx, yy, zz, ww = kdl.Rotation.RPY(0, 0, yaw).GetQuaternion()
pose.pose.orientation.x = xx
pose.pose.orientation.y = yy
pose.pose.orientation.z = zz
pose.pose.orientation.w = ww
plan = [pose]
print "Operator within self._lookat_radius"
self._robot.base.local_planner.setPlan(plan, p, o)
def get_position_constraint(self, entity_id_area_name_map):
try:
res = self._get_constraint_srv(
entity_ids=[k for k in entity_id_area_name_map],
area_names=[v for k, v in entity_id_area_name_map.iteritems()])
except Exception as e:
rospy.logerr("Can't get position constraint: {}".format(e))
return None
if res.error_msg != '':
rospy.logerr(res.error_msg)
return None
return PositionConstraint(constraint=res.position_constraint_map_frame,
frame="/map")
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
def distance_to_poi_area(frame_stamped):
# Derived from navigate_to_place
radius = math.hypot(self.robot.grasp_offset.x, self.robot.grasp_offset.y)
x = frame_stamped.frame.p.x()
y = frame_stamped.frame.p.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=frame_stamped.frame_id)
plan_to_poi = self.robot.base.global_planner.getPlan(pos_constraint)
if plan_to_poi:
distance = len(plan_to_poi)
# print "Distance to {fs}: {dist}".format(dist=distance, fs=frame_stamped.frame.p)
else:
distance = None
return distance
class Navigation(RobotPart):
def __init__(self, robot_name, tf_listener):
super(Navigation, self).__init__(robot_name=robot_name, tf_listener=tf_listener)
self._get_constraint_srv = self.create_service_client('/%s/ed/navigation/get_constraint' % robot_name,
GetGoalConstraint)
def get_position_constraint(self, entity_id_area_name_map):
try:
res = self._get_constraint_srv(entity_ids=[ k for k in entity_id_area_name_map ], area_names=[ v for k,v in entity_id_area_name_map.iteritems() ])
except Exception, e:
rospy.logerr(e)
return None
if res.error_msg != '':
rospy.logerr(res.error_msg)
return None
return PositionConstraint(constraint=res.position_constraint_map_frame, frame="/map")
def distance_to_poi_area(self, frame_stamped):
# ToDo: cook up something better: we need the arm that we're currently using but this would require a
# rather large API break (issue #739)
base_offset = self.robot.arms.values()[0].base_offset
radius = math.hypot(base_offset.x(), base_offset.y())
x = frame_stamped.frame.p.x()
y = frame_stamped.frame.p.y()
radius -= 0.1
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=frame_stamped.frame_id)
plan_to_poi = self.robot.base.global_planner.getPlan(pos_constraint)
if plan_to_poi:
distance = len(plan_to_poi)
# print "Distance to {fs}: {dist}".format(dist=distance, fs=frame_stamped.frame.p)
else:
distance = None
return distance
def _replan(self):
self._replan_attempts += 1
print "Trying to get a global plan"
operator_position = self._last_operator._pose.p
# Define end goal constraint, solely based on the (old) operator position
self._replan_pc = PositionConstraint()
self._replan_pc.constraint = "(x-%f)^2 + (y-%f)^2 < %f^2" % (
operator_position.x(), operator_position.y(),
self._operator_radius)
ros_plan = self._robot.base.global_planner.getPlan(self._replan_pc)
if not ros_plan or not self._robot.base.global_planner.checkPlan(
ros_plan):
print "No global plan possible"
else:
self._robot.speech.speak("Just a sec, let me try this way.")
print "Found a global plan, sending it to local planner"
self._replan_time = rospy.Time.now()
self._replan_active = True
oc = self._robot.base.local_planner.getCurrentOrientationConstraint(
)
self._visualize_plan(ros_plan)
self._robot.base.local_planner.setPlan(ros_plan, self._replan_pc,
oc)
self._breadcrumbs = []
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'
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 __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done','Aborted'])
self.robot = robot
self.position_constraint = PositionConstraint()
self.orientation_constraint = OrientationConstraint()
self.position_constraint.constraint = "x^2+y^2 < 1"
self.requested_location = None
rospy.Subscriber("/location_request", std_msgs.msg.String, self.requestedLocationcallback)
self.random_nav_designator = RandomNavDesignator(self.robot)
with self:
smach.StateMachine.add( "WAIT_A_SEC",
states.WaitTime(robot, waittime=1.0),
transitions={'waited' :"SELECT_ACTION",
'preempted':"Aborted"})
smach.StateMachine.add( "SELECT_ACTION",
SelectAction(),
transitions={ 'continue' : "DETERMINE_TARGET",
'pause' : "SELECT_ACTION",
'stop' : "SAY_DONE"})
@smach.cb_interface(outcomes=['target_determined', 'no_targets_available'],
input_keys=[],
output_keys=[])
def determine_target(userdata, designator):
entity_id = designator.getRandomGoal()
sentences = [ "Lets go look at the %s",
"Lets have a look at the %s",
"Lets go to the %s",
"Lets move to the %s",
"I will go to the %s",
"I will now move to the %s",
"I will now drive to the %s",
"I will look the %s",
"The %s will be my next location",
"The %s it is", "New goal, the %s",
"Going to look at the %s",
"Moving to the %s",
"Driving to the %s",
"On to the %s",
"On the move to the %s",
"Going to the %s"]
robot.speech.speak(random.choice(sentences)%entity_id, block=False)
return 'target_determined'
smach.StateMachine.add('DETERMINE_TARGET', smach.CBState(determine_target,
cb_kwargs={'designator': self.random_nav_designator}),
transitions={ 'target_determined':'DRIVE',
'no_targets_available':'SELECT_ACTION'})
smach.StateMachine.add( 'DRIVE',
states.NavigateToObserve(robot, self.random_nav_designator),
transitions={ "arrived":"SAY_SUCCEEDED",
"unreachable":'SAY_UNREACHABLE',
"goal_not_defined":'SELECT_ACTION'})
smach.StateMachine.add("SAY_SUCCEEDED",
states.Say(robot, [ "I am here",
"Goal succeeded",
"Another goal succeeded",
"Goal reached",
"Another goal reached",
"Target reached",
"Another target reached",
"Destination reached",
"Another destination reached",
"I have arrived",
"I have arrived at my goal",
"I have arrived at my target",
"I have arrived at my destination",
"I am at my goal",
"I am at my target",
"I am at my destination",
"Here I am",]),
transitions={ 'spoken':'SELECT_ACTION'})
smach.StateMachine.add("SAY_UNREACHABLE",
states.Say(robot, [ "I can't find a way to my goal, better try something else",
"This goal is unreachable, I better find somewhere else to go",
"I am having a hard time getting there so I will look for a new target"]),
transitions={ 'spoken':'SELECT_ACTION'})
smach.StateMachine.add("SAY_DONE",
states.Say(robot, [ "That's all folks", "I'll stay here for a while", "Goodbye"]),
transitions={ 'spoken':'Done'})
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.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)
