def count_pipes(self):
"""Count the number of pipes in between the breaks."""
second_hpipe_found = False
while not second_hpipe_found:
try:
frame = yield self.curr_image
except util.TimeoutError:
fprint("Image isn't being received, fail mission", msg_color="red")
raise Exception("Image isn't being received")
# get all the pipes in the current frame
hpipes, vpipes = self._get_all_pipes(frame)
# Look for NEW pipes
new_hpipes, new_vpipes = self._update_pipes(
hpipes, self.old_hpipe_pos), self._update_pipes(vpipes, self.old_vpipe_pos)
# the second hpipe is found
if len(new_hpipes) > 0 and self.hpipe_found:
defer.returnValue(self.count)
# the first hpipe is found
elif len(new_hpipes) > 0 and not self.hpipe_found:
self.hpipe_found = True
for pipe in vpipes:
if pipe[1] > new_hpipes[0][1]:
self.count += 1
# if its above first h, add to count
elif self.hpipe_found:
for pipe in new_vpipes:
if pipe[1] > new_hpipes[0][1]:
self.count += 1
self.old_hpipe_pos.extend(new_hpipes)
self.old_vspipe_pos.extend(new_vpipes)
def _err_mission(self, err):
if hasattr(
err, "type"
) and err.type == defer.CancelledError: # This means there was a timeout
fprint(self.current_mission.name,
msg_color="red",
title="MISSION TIMEOUT")
yield self.publish("TimingOut")
if (TimeoutManager.can_repeat(
self.missions_left, self._get_time_left(),
self.current_mission)) or len(self.missions_left) == 1:
yield self.publish("Retrying")
self.failed = False
self.current_mission.timeout = self.current_mission.min_time
yield self._run_mission(self.current_mission, redo=True)
defer.returnValue(False)
else:
fprint(err,
msg_color="red",
title="{} MISSION ERROR: ".format(
self.current_mission.name))
yield self.publish("Failing")
yield self.current_mission.safe_exit(self.navigator, err, self,
self.module)
self._mission_complete()
def setup_mission(navigator):
color = "ANY"
shape = "ANY"
#color = yield navigator.mission_params["scan_the_code_color3"].get()
fprint("Setting search shape={} color={}".format(shape, color), title="DETECT DELIVER", msg_color='green')
yield navigator.mission_params["detect_deliver_shape"].set(shape)
yield navigator.mission_params["detect_deliver_color"].set(color)
def go(self, *args, **kwargs):
if self.nav.killed:
# What do we want to do with missions when the boat is killed
fprint("Boat is killed, ignoring go command!",
title="POSE_EDITOR",
msg_color="red")
yield self.nav.nh.sleep(1)
defer.returnValue(None)
self.goal = self.nav._moveto_client.send_goal(
self.as_MoveGoal(*args, **kwargs))
res = yield self.goal.get_result()
if res.failure_reason == '':
fprint("Move completed successfully!",
title="POSE_EDITOR",
msg_color="green",
newline=2)
elif res.failure_reason == 'occupied':
fprint("Goal was occupied - moved to close point instead.",
title="POSE_EDITOR",
newline=2)
elif res.failure_reason == 'collided':
fprint("Collided with object. Check perception then if this is the real boat and"\
"perception wasn't to blame, promptly kill jason", title="POSE_EDITOR", newline=2, msg_color="red")
else:
fprint("Unknown response from action client: {}".format(res),
title="POSE_EDITOR",
newline=2)
def _err_base_mission(self, err):
self.running_mission = False
self.current_mission_name = None
if err.type == defer.CancelledError:
fprint("Base mission cancelled", msg_color="red", title="BASE MISSION ERROR:")
else:
fprint(err, msg_color="red", title="BASE MISSION ERROR:")
def shoot_and_align(self):
move = yield self.align_to_target()
if move.failure_reason != "":
fprint("Error Aligning with target = {}. Ending mission :(".format(move.failure_reason), title="DETECT DELIVER", msg_color="red")
return
fprint("Aligned successs. Shooting without realignment", title="DETECT DELIVER", msg_color="green")
yield self.shoot_all_balls()
def do_mission(self, navigator, planner, module, **kwargs):
"""Perform this mission."""
yield planner.publish("Starting", self)
to_run = getattr(module, self.mission_script)
fprint(self.name, msg_color="green", title="STARTING MISSION")
res = yield to_run.main(navigator, attempts=self.attempts, **kwargs)
defer.returnValue(res)
def go_to_objects(navigator, position, objs):
objects = get_closest_objects(position, objs)
for o in objects:
fprint("MOVING TO OBJECT WITH ID {}".format(o.id), msg_color="green")
yield navigator.nh.sleep(5)
pos = nt.rosmsg_to_numpy(o.position)
yield navigator.move.look_at(pos).set_position(pos).backward(7).go()
def do_mission(self, navigator, planner, module, **kwargs):
"""Perform this mission."""
yield planner.publish("Starting", self)
to_run = getattr(module, self.mission_script)
fprint(self.name, msg_color="green", title="STARTING MISSION")
res = yield to_run.main(navigator, attempts=self.attempts, **kwargs)
defer.returnValue(res)
def search_shape(self):
shapes = yield self.get_shape()
if shapes.found:
for shape in shapes.shapes.list:
normal_res = yield self.get_normal(shape)
if normal_res.success:
enu_cam_tf = yield self.navigator.tf_listener.get_transform(
'/enu', '/' + shape.header.frame_id,
shape.header.stamp)
if self.correct_shape(shape):
self.shape_pose = self.get_shape_pos(
normal_res, enu_cam_tf)
defer.returnValue(True)
self.update_shape(shape, normal_res, enu_cam_tf)
else:
if not self.last_lidar_error == normal_res.error:
fprint("Normal not found Error={}".format(
normal_res.error),
title="DETECT DELIVER",
msg_color='red')
self.last_lidar_error = normal_res.error
else:
if not self.last_shape_error == shapes.error:
fprint("shape not found Error={}".format(shapes.error),
title="DETECT DELIVER",
msg_color="red")
self.last_shape_error = shapes.error
defer.returnValue(False)
def empty_queue(self):
"""Constantly empties the queue if there is something in it, or run the base mission otherwise."""
while self.keep_running:
if self.current_mission_timeout is not None:
if (self.nh.get_time() - self.current_mission_start_time
) > self.current_mission_timeout:
fprint(self.current_mission_name,
msg_color="red",
title="MISSION TIMEOUT:")
self.mission_defer.cancel()
if self.running_mission:
self.refresh()
yield self.nh.sleep(1)
continue
try:
mission = self.queue.get(block=False)
self._run_mission(mission)
except que.Empty:
self._run_base_mission()
except Exception as exp:
fprint(
exp,
msg_color="red",
title="LOL WHAT THE F**K HAPPENED? THIS IS ON YOU TESS!!!")
self.refresh()
yield self.nh.sleep(1)
def continuously_align(self):
fprint("Starting Forest Align", title="DETECT DELIVER", msg_color='green')
try:
while True:
shooter_pose = yield self.shooter_pose_sub.get_next_message()
if self.align_forest_pause:
yield self.navigator.nh.sleep(0.1)
continue
shooter_pose = shooter_pose.pose
cen = np.array([shooter_pose.position.x, shooter_pose.position.y])
yaw = trns.euler_from_quaternion([shooter_pose.orientation.x,
shooter_pose.orientation.y,
shooter_pose.orientation.z,
shooter_pose.orientation.w])[2]
q = trns.quaternion_from_euler(0, 0, yaw)
p = np.append(cen,0)
#fprint("Forest Aligning to p=[{}] q=[{}]".format(p, q), title="DETECT DELIVER", msg_color='green')
#Prepare move to follow shooter
move = self.navigator.move.set_position(p).set_orientation(q).yaw_right(90, 'deg')
#Adjust move for location of target
move = move.forward(self.target_offset_meters)
#Adjust move for location of launcher
move = move.left(-self.shooter_baselink_tf._p[1]).forward(-self.shooter_baselink_tf._p[0])
#Move away a fixed distance to make the shot
move = move.left(self.shoot_distance_meters)
yield move.go(move_type='bypass')
except Exception:
traceback.print_exc()
raise
def _cb_mission(self, result):
if not self.failed:
yield self.publish("Ending")
fprint(str(result) + " TIME: " + str((self.nh.get_time() - self.current_mission.start_time).to_sec()),
msg_color="green", title="{} MISSION COMPLETE: ".format(self.current_mission.name))
self.points += self.current_mission.points
self._mission_complete()
def _object_gone_missing(self, missing_objects):
fprint("This object {} is no longer in the list".format(missing_objects), msg_color="red")
self.helper.stop_ensuring_object_permanence()
for o in missing_objects:
if o in self.found:
self.helper.remove_found(o)
self.found.remove(o)
self.mission_defer.cancel()
def main(navigator):
nh = navigator.nh
fprint("{} running".format(__name__), msg_color='red')
yield nh.sleep(6)
fprint(
"{} stopped running, you should have stopped by now".format(__name__),
msg_color='red')
def shoot_all_balls(self):
for i in range(self.NUM_BALLS):
goal = yield self.shooterLoad.send_goal(ShooterDoAction())
fprint("Loading Shooter {}".format(i), title="DETECT DELIVER", msg_color='green')
res = yield goal.get_result()
yield self.navigator.nh.sleep(2)
goal = yield self.shooterFire.send_goal(ShooterDoAction())
fprint("Firing Shooter {}".format(i), title="DETECT DELIVER", msg_color='green')
res = yield goal.get_result()
def run(self):
fprint("PINGER EXIT: Starting", msg_color='green')
self.gate_index = yield self.navigator.mission_params["acoustic_pinger_active_index"].get()
self.gate_index = self.gate_index - 1
yield self.get_objects()
yield self.set_side()
self.get_gate_thru_points()
yield self.go_thru_gate()
fprint("PINGER EXIT: Done", msg_color='green')
def search_side(self):
fprint("Searching side",title="DETECT DELIVER", msg_color='green')
start_time = self.navigator.nh.get_time()
while self.navigator.nh.get_time() - start_time < genpy.Duration(self.LOOK_AT_TIME):
res = yield self.get_any_shape()
if not res == False:
defer.returnValue(res)
yield self.navigator.nh.sleep(0.1)
defer.returnValue(False)
def _err_base_mission(self, err):
self.running_mission = False
self.current_mission_name = None
if err.type == defer.CancelledError:
fprint("Base mission cancelled",
msg_color="red",
title="BASE MISSION ERROR:")
else:
fprint(err, msg_color="red", title="BASE MISSION ERROR:")
def select_backup_shape(self):
for (shape, color), point_normal in self.identified_shapes.iteritems():
self.shape_pose = point_normal
if self.Shape == shape or self.Color == color:
fprint("Correct shape not found, resorting to shape={} color={}".format(shape, color), title="DETECT DELIVER", msg_color='yellow')
return
if self.shape_pose == None:
raise Exception("None seen")
fprint("Correct shape not found, resorting to random shape", title="DETECT DELIVER", msg_color='yellow')
def set_shape_and_color(self):
target = yield self.navigator.mission_params["detect_deliver_target"].get()
if target == "BIG":
self.target_offset_meters = self.SHAPE_CENTER_TO_BIG_TARGET
elif target == "SMALL":
self.target_offset_meters = self.SHAPE_CENTER_TO_SMALL_TARGET
self.Shape = yield self.navigator.mission_params["detect_deliver_shape"].get()
self.Color = yield self.navigator.mission_params["detect_deliver_color"].get()
fprint("Color={} Shape={} Target={}".format(self.Color, self.Shape, target), title="DETECT DELIVER", msg_color='green')
def get_waypoint(self):
res = yield self.navigator.database_query("shooter")
if not res.found:
fprint("shooter waypoint not found",
title="DETECT DELIVER",
msg_color='red')
raise MissingPerceptionObject("shooter",
"Detect Deliver Waypoint not found")
self.waypoint_res = res
def align_to_target(self):
if self.shape_pose == None:
self.select_backup_shape()
goal_point, goal_orientation = self.get_aligned_pose(self.shape_pose[0], self.shape_pose[1])
move = self.navigator.move.set_position(goal_point).set_orientation(goal_orientation).forward(self.target_offset_meters)
move = move.left(-self.shooter_baselink_tf._p[1]).forward(-self.shooter_baselink_tf._p[0]) #Adjust for location of shooter
fprint("Aligning to shoot at {}".format(move), title="DETECT DELIVER", msg_color='green')
move_complete = yield move.go(move_type="skid", blind=True)
defer.returnValue(move_complete)
def _object_gone_missing(self, missing_objects):
fprint(
"This object {} is no longer in the list".format(missing_objects),
msg_color="red")
self.helper.stop_ensuring_object_permanence()
for o in missing_objects:
if o in self.found:
self.helper.remove_found(o)
self.found.remove(o)
self.mission_defer.cancel()
def _end_mission(self, result):
fprint(result,
msg_color="green",
title="{} MISSION COMPLETE: ".format(self.current_mission_name))
self.running_mission = False
self.current_mission_name = None
self.helper.stop_ensuring_object_permanence()
self._mission_complete(self.current_mission)
self.current_mission = None
self.current_mission_timeout = None
def search_samples(self):
"""Move to each observation point and listen to the pinger while sitting still"""
yield self.get_observation_poses()
for i,p in enumerate(self.observation_points):
yield self.stop_listen()
yield self.navigator.move.set_position(p).look_at(self.look_at_points[i]).go()
yield self.start_listen()
fprint("PINGER: Listening To Pinger at point {}".format(p), msg_color='green')
yield self.navigator.nh.sleep(self.LISTEN_TIME)
yield self.stop_listen()
def main(navigator, **kwargs):
nh = navigator.nh
attempts = kwargs["attempts"]
fprint("{} running".format(__name__), msg_color='red')
if attempts > 1:
yield nh.sleep(1)
else:
yield nh.sleep(2)
fprint("{} stopped running".format(__name__), msg_color='red')
def search_side(self):
fprint("Searching side", title="DETECT DELIVER", msg_color='green')
start_time = self.navigator.nh.get_time()
while self.navigator.nh.get_time() - start_time < genpy.Duration(
self.LOOK_AT_TIME):
res = yield self.get_any_shape()
if not res == False:
defer.returnValue(res)
yield self.navigator.nh.sleep(0.1)
defer.returnValue(False)
def _mission_complete(self, mission):
self.tree.remove(mission)
self.current_mission_name = None
for m in mission.children:
self.tree.append(m)
self.refresh()
self.completed_mission += 1
if self.completed_mission == self.total_mission_count:
fprint("ALL MISSIONS COMPLETE", msg_color="green")
self.keep_running = False
def _end_mission(self, result):
self.pub_msn_info.publish(String("Ending Mission {}".format(self.current_mission_name)))
fprint(str(result) + " TIME: " + str((self.nh.get_time() - self.current_mission_start_time).to_sec()),
msg_color="green", title="{} MISSION COMPLETE: ".format(self.current_mission_name))
self.running_mission = False
self.current_mission_name = None
self.helper.stop_ensuring_object_permanence()
self._mission_complete(self.current_mission)
self.current_mission = None
self.current_mission_timeout = None
def run(self):
fprint("PINGER EXIT: Starting", msg_color='green')
self.gate_index = yield self.navigator.mission_params[
"acoustic_pinger_active_index"].get()
self.gate_index = self.gate_index - 1
yield self.get_objects()
yield self.set_side()
self.get_gate_thru_points()
yield self.go_thru_gate()
fprint("PINGER EXIT: Done", msg_color='green')
def _mission_complete(self, mission):
self.tree.remove(mission)
self.current_mission_name = None
for m in mission.children:
self.tree.append(m)
self.refresh()
self.completed_mission += 1
if self.completed_mission == self.total_mission_count:
fprint("ALL MISSIONS COMPLETE", msg_color="green")
self.keep_running = False
def refresh(self):
"""
Called when the state of the DAG needs to be updated due to a mission completing or an object being found.
CALLED ASYNCHRONOUS
"""
for mission in self.tree:
if self.can_complete(mission) and not self._is_in_queue(mission) and mission.name != self.current_mission_name:
fprint("mission: {}".format(mission.name), msg_color="blue", title="ADDING")
self.queue.put(mission)
def new_item(self, obj):
"""
Callback for a new object being found.
ASYNCHRONOUS
"""
fprint("NEW ITEM: {}".format(obj.name), msg_color="blue")
if self.base_mission is not None and self.current_mission_name is self.base_mission.name:
self.mission_defer.cancel()
self.found.add(obj.name)
self.refresh()
def _cb_mission(self, result):
if not self.failed:
yield self.publish("Ending")
fprint(str(result) + " TIME: " + str(
(self.nh.get_time() -
self.current_mission.start_time).to_sec()),
msg_color="green",
title="{} MISSION COMPLETE: ".format(
self.current_mission.name))
self.points += self.current_mission.points
self._mission_complete()
def new_item(self, obj):
"""
Callback for a new object being found.
ASYNCHRONOUS
"""
fprint("NEW ITEM: {}".format(obj.name), msg_color="blue")
if self.base_mission is not None and self.current_mission_name is self.base_mission.name:
self.mission_defer.cancel()
self.found.add(obj.name)
self.refresh()
def setup_mission(navigator):
stc_color = yield navigator.mission_params["scan_the_code_color3"].get(raise_exception=False)
if stc_color == False:
color = "ANY"
else:
color = stc_color
#color = "ANY"
shape = "ANY"
fprint("Setting search shape={} color={}".format(shape, color), title="DETECT DELIVER", msg_color='green')
yield navigator.mission_params["detect_deliver_shape"].set(shape)
yield navigator.mission_params["detect_deliver_color"].set(color)
def search_sides(self, moves):
for move in moves:
yield move.go(move_type="drive")
res = yield self.search_side()
if res == False:
fprint("No shape found on side",title="DETECT DELIVER", msg_color='red')
continue
shape_color, found_pose = res
if self.correct_shape(shape_color):
self.shape_pose = found_pose
return
fprint("Saw (Shape={}, Color={}) on this side".format(shape_color[0], shape_color[1]),title="DETECT DELIVER", msg_color='green')
def safe_exit(self, navigator, err, planner, module):
"""Run a safe exit of a mission."""
try:
to_run = getattr(module, self.name)
if hasattr(to_run, 'safe_exit'):
yield to_run.safe_exit(navigator, err)
else:
fprint("Hmmmm. This isn't good. Your mission failed, and there was no safe exit. "
"I hope this mission doesn't have any children.", msg_color="red")
except Exception as exp:
print exp
fprint("Oh man this is pretty bad, your mission's safe exit failed. SHAME!", msg_color="red")
def shoot_and_align(self):
move = yield self.align_to_target()
if move.failure_reason != "":
fprint("Error Aligning with target = {}. Ending mission :(".format(
move.failure_reason),
title="DETECT DELIVER",
msg_color="red")
return
fprint("Aligned successs. Shooting without realignment",
title="DETECT DELIVER",
msg_color="green")
yield self.shoot_all_balls()
def search_samples(self):
"""Move to each observation point and listen to the pinger while sitting still"""
yield self.get_observation_poses()
for i, p in enumerate(self.observation_points):
yield self.stop_listen()
yield self.navigator.move.set_position(p).look_at(
self.look_at_points[i]).go()
yield self.start_listen()
fprint("PINGER: Listening To Pinger at point {}".format(p),
msg_color='green')
yield self.navigator.nh.sleep(self.LISTEN_TIME)
yield self.stop_listen()
def set_active_pinger(self):
"""Set the paramter for the active pinger identified for use in other mission"""
fprint("PINGER: setting active pinger to Gate_{}".format(int(self.gate_index)+1), msg_color='green')
yield self.get_colored_buoys()
if self.color_wrong and self.gate_index == 2:
yield self.navigator.mission_params["acoustic_pinger_active_index_correct"].set(1)
elif self.color_wrong and self.gate_index == 0:
yield self.navigator.mission_params["acoustic_pinger_active_index_correct"].set(3)
else:
yield self.navigator.mission_params["acoustic_pinger_active_index_correct"].set(int(self.gate_index)+1)
yield self.navigator.mission_params["acoustic_pinger_active_index"].set(int(self.gate_index)+1)
def main(navigator):
result = navigator.fetch_result()
#middle_point = np.array([-10, -70, 0])
est_coral_survey = yield navigator.database_query("Coral_Survey")
yield navigator.move.set_position(est_coral_survey.objects[0]).go()
totem = yield navigator.database_query("totem")
# Get the closest totem object to the boat
totem_np = map(lambda obj: navigator_tools.point_to_numpy(obj), totem.objects)
dist = map(lambda totem_np: np.linalg.norm(totem_np - navigator_tools.point_to_numpy(est_coral_survey.objects[0])), totems_np)
middle_point = navigator_tools.point_to_numpy(totem.objects[0].position)
quads_to_search = [1, 2, 3, 4]
if (yield navigator.nh.has_param("/mission/coral_survey/quadrants")):
quads_to_search = yield navigator.nh.get_param("/mission/coral_survey/quadrants")
waypoint_from_center = np.array([10 * np.sqrt(2)])
# Construct waypoint list along NSEW directions then rotate 45 degrees to get a good spot to go to.
directions = [EAST, NORTH, WEST, SOUTH]
waypoints = []
for quad in quads_to_search:
mid = navigator.move.set_position(middle_point).set_orientation(directions[quad - 1])
waypoints.append(mid.yaw_left(45, "deg").forward(waypoint_from_center).set_orientation(NORTH))
# Get into the coral survey area
yield waypoints[0].go()
# Publish ogrid with boundaries to stay inside
ogrid = OgridFactory(middle_point, draw_borders=True)
msg = ogrid.get_message()
latched = navigator.latching_publisher("/mission_ogrid", OccupancyGrid, msg)
searcher = navigator.search("coral_survey", waypoints)
yield searcher.start_search(move_type='skid', spotings_req=1)
fprint("Centering over the thing!", title="CORAL_SURVEY")
# TODO: Center over the thing.
boat_position = (yield navigator.tx_pose)[0]
# TODO: make this easier
quad = np.argmin(np.linalg.norm(boat_position - [[w.pose[0][0][0], w.pose[0][1][0],w.pose[0][2][0]] for w in waypoints], axis=1))
quad = quads_to_search[quad]
fprint("Estimated quadrant: {}".format(quad), title="CORAL_SURVEY", msg_color='green')
yield navigator.nh.sleep(5)
defer.returnValue(result)
def _end_mission(self, result):
self.pub_msn_info.publish(
String("Ending Mission {}".format(self.current_mission_name)))
fprint(str(result) + " TIME: " + str(
(self.nh.get_time() - self.current_mission_start_time).to_sec()),
msg_color="green",
title="{} MISSION COMPLETE: ".format(self.current_mission_name))
self.running_mission = False
self.current_mission_name = None
self.helper.stop_ensuring_object_permanence()
self._mission_complete(self.current_mission)
self.current_mission = None
self.current_mission_timeout = None
def refresh(self):
"""
Called when the state of the DAG needs to be updated due to a mission completing or an object being found.
CALLED ASYNCHRONOUS
"""
for mission in self.tree:
if self.can_complete(mission) and not self._is_in_queue(
mission) and mission.name != self.current_mission_name:
fprint("mission: {}".format(mission.name),
msg_color="blue",
title="ADDING:")
self.queue.put(mission)
def _get_scan_the_code(self):
v = False
if self.scan_the_code is None:
ans = yield self.helper.get_object("scan_the_code", volume_only=v)
else:
try:
ans = yield self.helper.get_object_by_id(self.scan_the_code.id)
except Exception:
print "PROBLEM"
ans = yield self.helper.get_object("scan_the_code", volume_only=v)
fprint("GOT SCAN THE CODE WITH ID {}".format(ans.id), msg_color="blue")
defer.returnValue(ans)
def _get_scan_the_code(self):
v = False
if self.scan_the_code is None:
ans = yield self.helper.get_object("scan_the_code", volume_only=v)
else:
try:
ans = yield self.helper.get_object_by_id(self.scan_the_code.id)
except Exception:
print "PROBLEM"
ans = yield self.helper.get_object("scan_the_code",
volume_only=v)
fprint("GOT SCAN THE CODE WITH ID {}".format(ans.id), msg_color="blue")
defer.returnValue(ans)
def align_to_target(self):
if self.shape_pose == None:
self.select_backup_shape()
goal_point, goal_orientation = self.get_aligned_pose(
self.shape_pose[0], self.shape_pose[1])
move = self.navigator.move.set_position(goal_point).set_orientation(
goal_orientation).forward(self.target_offset_meters)
move = move.left(-self.shooter_baselink_tf._p[1]).forward(
-self.shooter_baselink_tf._p[0]) #Adjust for location of shooter
fprint("Aligning to shoot at {}".format(move),
title="DETECT DELIVER",
msg_color='green')
move_complete = yield move.go(move_type="skid", blind=True)
defer.returnValue(move_complete)
def setup_mission(navigator):
stc_color = yield navigator.mission_params["scan_the_code_color3"].get(
raise_exception=False)
if stc_color == False:
color = "ANY"
else:
color = stc_color
#color = "ANY"
shape = "ANY"
fprint("Setting search shape={} color={}".format(shape, color),
title="DETECT DELIVER",
msg_color='green')
yield navigator.mission_params["detect_deliver_shape"].set(shape)
yield navigator.mission_params["detect_deliver_color"].set(color)
def _err_mission(self, err):
fprint(err,
msg_color="red",
title="{} MISSION ERROR: ".format(self.current_mission_name))
self.running_mission = False
self.current_mission_name = None
self.helper.stop_ensuring_object_permanence()
self.current_mission_timeout = None
if err.type == defer.CancelledError:
return
yield self.current_mission.safe_exit(self.navigator, err, self,
self.module)
self._mission_complete(self.current_mission)
self.current_mission = None
def select_backup_shape(self):
for (shape, color), point_normal in self.identified_shapes.iteritems():
self.shape_pose = point_normal
if self.Shape == shape or self.Color == color:
fprint(
"Correct shape not found, resorting to shape={} color={}".
format(shape, color),
title="DETECT DELIVER",
msg_color='yellow')
return
if self.shape_pose == None:
raise Exception("None seen")
fprint("Correct shape not found, resorting to random shape",
title="DETECT DELIVER",
msg_color='yellow')
def set_shape_and_color(self):
target = yield self.navigator.mission_params[
"detect_deliver_target"].get()
if target == "BIG":
self.target_offset_meters = self.SHAPE_CENTER_TO_BIG_TARGET
elif target == "SMALL":
self.target_offset_meters = self.SHAPE_CENTER_TO_SMALL_TARGET
self.Shape = yield self.navigator.mission_params[
"detect_deliver_shape"].get()
self.Color = yield self.navigator.mission_params[
"detect_deliver_color"].get()
fprint("Color={} Shape={} Target={}".format(self.Color, self.Shape,
target),
title="DETECT DELIVER",
msg_color='green')
def new_item(self, obj):
"""
Callback for a new object being found.
ASYNCHRONOUS
"""
fprint("NEW ITEM: {}".format(obj.name), msg_color="blue")
if self.base_mission is not None and self.current_mission_name is self.base_mission.name:
try:
self.mission_defer.cancel()
self.running_base_mission = False
except Exception:
print "Error Cancelling deferred"
self.found.add(obj.name)
self.refresh()
def main(navigator):
res = navigator.fetch_result()
buoy_field = yield navigator.database_query("BuoyField")
buoy_field_point = navigator_tools.point_to_numpy(buoy_field.objects[0])
#yield navigator.move.set_position(buoy_field_point).go()
circle_colors = ['blue', 'red']
color_map = {'blue': (255, 0, 0), 'red': (0, 0, 255)}
explored_ids = []
all_found = False
while not all_found:
target_totem, explored_ids = yield get_closest_buoy(navigator, explored_ids)
if target_totem is None:
fprint("No suitable totems found.", msg_color='red', title="CIRCLE_TOTEM")
continue
# Visualization
points = [target_totem.position]
pc = PointCloud(header=navigator_tools.make_header(frame='/enu'),
points=points)
yield navigator._point_cloud_pub.publish(pc)
# Let's go there
target_distance = 7 # m
target_totem_np = navigator_tools.point_to_numpy(target_totem.position)
q = get_sun_angle()
lookat = navigator.move.set_position(target_totem_np).set_orientation(q).backward(target_distance)
yield lookat.go()
# Now that we're looking him in the eyes, aim no higher.
# Check the color and see if it's one we want.
fprint("Color request", title="CIRCLE_TOTEM")
#if target_totem is not None:
# all_found = True
defer.returnValue(res)
pattern = navigator.move.circle_point(focus, radius=5)
for p in pattern:
yield p.go(move_type='skid', focus=focus)
print "Nexting"
def safe_exit(self, navigator, err, planner, module):
"""Run a safe exit of a mission."""
try:
to_run = getattr(module, self.name)
if hasattr(to_run, 'safe_exit'):
yield to_run.safe_exit(navigator, err)
else:
fprint(
"""Hmmmm. This isn't good. Your mission failed, and there was no safe exit.
I hope this mission doesn't have any children.""",
msg_color="red")
except Exception as exp:
print exp
fprint(
"Oh man this is pretty bad, your mission's safe exit failed. SHAME!",
msg_color="red")
