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 circle_search(self): platform_np = mil_tools.rosmsg_to_numpy( self.waypoint_res.objects[0].position) yield self.move.look_at(platform_np).set_position(platform_np).backward(self.circle_radius)\ .yaw_left(90, unit='deg').go(move_type="drive") done_circle = False @txros.util.cancellableInlineCallbacks def do_circle(): yield self.move.circle_point(platform_np, direction=self.circle_direction).go() done_circle = True # noqa flake8 cant see that it is defined above do_circle() while not done_circle: res = yield self.get_any_shape() if res is False: yield self.nh.sleep(0.25) continue fprint( "Shape ({}found, using normal to look at other 3 shapes if needed" .format(res[0]), title="DETECT DELIVER", msg_color="green") # circle_defer.cancel() shape_color, found_shape_pose = res if self.correct_shape(shape_color): self.shape_pose = found_shape_pose return # Pick other 3 to look at rot_right = np.array([[0, -1], [1, 0]]) (shape_point, shape_normal) = found_shape_pose rotated_norm = np.append(rot_right.dot(shape_normal[:2]), 0) center_point = shape_point - shape_normal * (self.platform_radius / 2.0) point_opposite_side = center_point - shape_normal * self.circle_radius move_opposite_side = self.move.set_position( point_opposite_side).look_at(center_point).yaw_left(90, unit='deg') left_or_whatever_point = center_point + rotated_norm * self.circle_radius move_left_or_whatever = self.move.set_position( left_or_whatever_point).look_at(center_point).yaw_left( 90, unit='deg') right_or_whatever_point = center_point - rotated_norm * self.circle_radius move_right_or_whatever = self.move.set_position( right_or_whatever_point).look_at(center_point).yaw_left( 90, unit='deg') yield self.search_sides( (move_right_or_whatever, move_opposite_side, move_left_or_whatever)) return fprint("No shape found after complete circle", title="DETECT DELIVER", msg_color='red') raise Exception("No shape found on platform")
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 _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 _run_search_pattern(self, loop, **kwargs): ''' Look around using the search pattern. If `loop` is true, then keep iterating over the list until timeout is reached or we find it. ''' if self.search_pattern is None: return def pattern(): for pose in self.search_pattern: fprint("Going to next position.", title="SEARCHER") if type(pose) == list or type(pose) == np.ndarray: yield self.nav.move.relative(pose).go(**kwargs) else: yield pose.go(**kwargs) yield self.nav.nh.sleep(2) if not loop: fprint("Search Pattern Over", title="SEARCHER") self.pattern_done = True fprint("Executing search pattern.", title="SEARCHER") if loop: while True: yield util.cancellableInlineCallbacks(pattern)() else: yield util.cancellableInlineCallbacks(pattern)()
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 continuously_align(self): fprint("Starting Forest Align", title="DETECT DELIVER", msg_color='green') while True: shooter_pose = yield self.shooter_pose_sub.get_next_message() if self.align_forest_pause: yield self.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.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')
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.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 _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 _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 _run_search_pattern(self, loop, **kwargs): ''' Look around using the search pattern. If `loop` is true, then keep iterating over the list until timeout is reached or we find it. ''' if self.search_pattern is None: return def pattern(): for pose in self.search_pattern: fprint("Going to next position.", title="SEARCHER") if type(pose) == list or type(pose) == np.ndarray: yield self.nav.move.relative(pose).go(**kwargs) else: yield pose.go(**kwargs) yield self.nav.nh.sleep(2) if not loop: fprint("Search Pattern Over", title="SEARCHER") self.pattern_done = True fprint("Executing search pattern.", title="SEARCHER") if loop: while True: yield util.cancellableInlineCallbacks(pattern)() else: yield util.cancellableInlineCallbacks(pattern)()
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 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 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 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 _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 _init(cls, mission_runner): super(Navigator, cls)._init(mission_runner) cls.vision_proxies = {} cls._load_vision_services() cls.launcher_state = "inactive" cls._actuator_timing = yield cls.nh.get_param("~actuator_timing") cls.mission_params = {} cls._load_mission_params() # If you don't want to use txros cls.pose = None cls.ecef_pose = None cls.killed = '?' cls.odom_loss = '?' if (yield cls.nh.has_param('/is_simulation')): cls.sim = yield cls.nh.get_param('/is_simulation') else: cls.sim = False # For missions to access clicked points / poses cls.rviz_goal = cls.nh.subscribe("/move_base_simple/goal", PoseStamped) cls.rviz_point = cls.nh.subscribe("/clicked_point", PointStamped) cls._moveto_client = action.ActionClient(cls.nh, 'move_to', MoveAction) def odom_set(odom): return setattr(cls, 'pose', mil_tools.odometry_to_numpy(odom)[0]) cls._odom_sub = cls.nh.subscribe('odom', Odometry, odom_set) def enu_odom_set(odom): return setattr(cls, 'ecef_pose', mil_tools.odometry_to_numpy(odom)[0]) cls._ecef_odom_sub = cls.nh.subscribe('absodom', Odometry, enu_odom_set) cls.hydrophones = TxHydrophonesClient(cls.nh) cls.poi = TxPOIClient(cls.nh) cls._grinch_lower_time = yield cls.nh.get_param("~grinch_lower_time") cls._grinch_raise_time = yield cls.nh.get_param("~grinch_raise_time") cls.grinch_limit_switch_pressed = False cls._grinch_limit_switch_sub = yield cls.nh.subscribe('/limit_switch', Bool, cls._grinch_limit_switch_cb) cls._winch_motor_pub = cls.nh.advertise("/grinch_winch/cmd", Command) cls._grind_motor_pub = cls.nh.advertise("/grinch_spin/cmd", Command) try: cls._actuator_client = cls.nh.get_service_client('/actuator_driver/actuate', SetValve) cls._database_query = cls.nh.get_service_client('/database/requests', ObjectDBQuery) cls._camera_database_query = cls.nh.get_service_client( '/camera_database/requests', navigator_srvs.CameraDBQuery) cls._change_wrench = cls.nh.get_service_client('/wrench/select', MuxSelect) cls._change_trajectory = cls.nh.get_service_client('/trajectory/select', MuxSelect) except AttributeError, err: fprint("Error getting service clients in nav singleton init: {}".format( err), title="NAVIGATOR", 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 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 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 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 search_side(self): fprint("Searching side", title="DETECT DELIVER", msg_color='green') start_time = self.nh.get_time() while self.nh.get_time() - start_time < genpy.Duration(self.LOOK_AT_TIME): res = yield self.get_any_shape() if res is not False: defer.returnValue(res) yield self.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 get_waypoint(self): res = yield self.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 _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 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 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.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.nh.sleep(self.LISTEN_TIME) yield self.stop_listen()
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 _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 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 _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 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 _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 setup_mission(self): stc_color = yield self.mission_params["scan_the_code_color3"].get(raise_exception=False) if stc_color is 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 self.mission_params["detect_deliver_shape"].set(shape) yield self.mission_params["detect_deliver_color"].set(color)
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 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 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.mission_params["acoustic_pinger_active_index_correct"].set(1) elif self.color_wrong and self.gate_index == 0: yield self.mission_params["acoustic_pinger_active_index_correct"].set(3) else: yield self.mission_params["acoustic_pinger_active_index_correct"].set(int(self.gate_index) + 1) yield self.mission_params["acoustic_pinger_active_index"].set(int(self.gate_index) + 1)
def pattern(): for pose in self.search_pattern: fprint("Going to next position.", title="SEARCHER") if type(pose) == list or type(pose) == np.ndarray: yield self.nav.move.relative(pose).go(**kwargs) else: yield pose.go(**kwargs) yield self.nav.nh.sleep(2) if not loop: fprint("Search Pattern Over", title="SEARCHER") self.pattern_done = True
def _make_bounds(cls): fprint("Constructing bounds.", title="NAVIGATOR") if (yield cls.nh.has_param("/bounds/enforce")): _bounds = cls.nh.get_service_client('/get_bounds', navigator_srvs.Bounds) yield _bounds.wait_for_service() resp = yield _bounds(navigator_srvs.BoundsRequest()) if resp.enforce: cls.enu_bounds = [mil_tools.rosmsg_to_numpy(bound) for bound in resp.bounds] else: fprint("No bounds param found, defaulting to none.", title="NAVIGATOR") cls.enu_bounds = None
def run(self, parameters): yield self.setup_mission() fprint("STARTING MISSION", title="DETECT DELIVER", msg_color='green') yield self.vision_proxies["get_shape"].start() yield self.set_shape_and_color() # Get correct goal shape/color from params yield self.get_waypoint() # Get waypoint of shooter target yield self.circle_search() # Go to waypoint and circle until target found # yield self.shoot_and_align() # Align to target and shoot yield self.shoot_and_align_forest() # Align to target and shoot yield self.backup_from_target() yield self.vision_proxies["get_shape"].stop() fprint("ENDING MISSION", 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 pattern(): for pose in self.search_pattern: fprint("Going to next position.", title="SEARCHER") if type(pose) == list or type(pose) == np.ndarray: yield self.nav.move.relative(pose).go(**kwargs) else: yield pose.go(**kwargs) yield self.nav.nh.sleep(2) if not loop: fprint("Search Pattern Over", title="SEARCHER") self.pattern_done = True
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 _init(cls, task_runner): super(Navigator, cls)._init(task_runner) cls.vision_proxies = {} cls._load_vision_services() cls.mission_params = {} cls._load_mission_params() # If you don't want to use txros cls.pose = None cls.ecef_pose = None cls.enu_bounds = None cls.killed = '?' cls.odom_loss = '?' if (yield cls.nh.has_param('/is_simulation')): cls.sim = yield cls.nh.get_param('/is_simulation') else: cls.sim = False # Just some pre-created publishers for missions to use for debugging cls._point_cloud_pub = cls.nh.advertise("navigator_points", PointCloud) cls._pose_pub = cls.nh.advertise("navigator_pose", PoseStamped) cls._moveto_client = action.ActionClient(cls.nh, 'move_to', MoveAction) def odom_set(odom): return setattr(cls, 'pose', mil_tools.odometry_to_numpy(odom)[0]) cls._odom_sub = cls.nh.subscribe('odom', Odometry, odom_set) def enu_odom_set(odom): return setattr(cls, 'ecef_pose', mil_tools.odometry_to_numpy(odom)[0]) cls._ecef_odom_sub = cls.nh.subscribe('absodom', Odometry, enu_odom_set) try: cls._database_query = cls.nh.get_service_client( '/database/requests', navigator_srvs.ObjectDBQuery) cls._camera_database_query = cls.nh.get_service_client( '/camera_database/requests', navigator_srvs.CameraDBQuery) cls._change_wrench = cls.nh.get_service_client( '/wrench/select', MuxSelect) except AttributeError, err: fprint("Error getting service clients in nav singleton init: {}". format(err), title="NAVIGATOR", msg_color='red')
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_any_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) self.update_shape(shape, normal_res, enu_cam_tf) defer.returnValue( ((shape.Shape, shape.Color), self.identified_shapes[(shape.Shape, shape.Color)]) ) else: fprint("Normal not found Error={}".format(normal_res.error), title="DETECT DELIVER", msg_color='red') else: fprint("shape not found Error={}".format(shapes.error), title="DETECT DELIVER", msg_color="red") defer.returnValue(False)
def _make_alarms(cls): cls.kill_listener = yield TxAlarmListener.init(cls.nh, 'kill', cls.kill_alarm_cb) cls.odom_loss_listener = yield TxAlarmListener.init( cls.nh, 'odom-kill', lambda _, alarm: setattr(cls, 'odom_loss', alarm.raised)) fprint("Alarm listener created, listening to alarms: ", title="NAVIGATOR") cls.kill_alarm = yield cls.kill_listener.get_alarm() cls.killed = cls.kill_alarm.raised cls.odom_loss = yield cls.odom_loss_listener.is_raised() fprint("\tkill :", newline=False) fprint(cls.killed) fprint("\todom-kill :", newline=False) fprint(cls.odom_loss)
def got_range(msg): '''TODO: - Make parallel to surface ''' translation = (0.0, 0.0, -msg.range) if rospy.Time.now() < rospy.Time(0.5): listener.clear() t = rospy.Time(0) try: listener.waitForTransform('/base_link', '/map', t, rospy.Duration(1)) trans, rot = listener.lookupTransform("/base_link", "/map", t) bc.sendTransform(translation, rot, rospy.Time.now(), "/ground", "/dvl") except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: te.fprint(e, title='TF REPUB')
def start_search(self, timeout=120, loop=True, spotings_req=2, **kwargs): fprint("Starting.", title="SEARCHER") looker = self._run_look(spotings_req).addErrback(self.catch_error) finder = self._run_search_pattern(loop, **kwargs).addErrback(self.catch_error) start_pose = self.nav.move.forward(0) start_time = self.nav.nh.get_time() try: while self.nav.nh.get_time() - start_time < genpy.Duration(timeout): if self.object_found: finder.cancel() fprint("Object found.", title="SEARCHER") defer.returnValue(True) if self.pattern_done and not loop: finder.cancel() defer.returnValue(False) yield self.nav.nh.sleep(0.1) except KeyboardInterrupt: # This doesn't work... fprint("Control C detected!", title="SEARCHER") fprint("Object NOT found. Returning to start position.", title="SEARCHER") finder.cancel() looker.cancel() yield start_pose.go()
def _load_mission_params(cls, fname="mission_params.yaml"): rospack = rospkg.RosPack() config_file = os.path.join(rospack.get_path('navigator_missions'), 'launch', fname) f = yaml.load(open(config_file, 'r')) for name in f: try: param = f[name]["param"] options = f[name]["options"] desc = f[name]["description"] default = f[name].get("default") cls.mission_params[name] = MissionParam(cls.nh, param, options, desc, default) except Exception, e: err = "Error loading mission params: {}".format(e) fprint("" + err, title="NAVIGATOR", msg_color='red')