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