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 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 _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 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 _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 _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)
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 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)