def _init(self, need_trajectory=True): self._trajectory_sub = self._node_handle.subscribe( 'trajectory', PoseTwistStamped) self._moveto_action_client = action.ActionClient( self._node_handle, 'moveto', MoveToAction) self._tf_listener = tf.TransformListener(self._node_handle) self._camera_2d_action_clients = dict(forward=action.ActionClient( self._node_handle, 'find2_forward_camera', legacy_vision_msg.FindAction), ) self._camera_3d_action_clients = dict(forward=action.ActionClient( self._node_handle, 'find_forward', object_finder_msg.FindAction), ) self._odom_sub = self._node_handle.subscribe('odom', Odometry) self._wrench_sub = self._node_handle.subscribe('wrench', WrenchStamped) #The absodom topic has the lat long information ebeded in the nav_msgs/Odometry. the lat long is under pose.pose.position and this is what is reported back in the JSON messages for a chalenge. self._absodom_sub = self._node_handle.subscribe('absodom', Odometry) #SPP subscribe the boat to the processed pings, so the missions can utalize the processed acoustic pings self._hydrophone_ping_sub = self._node_handle.subscribe( 'hydrophones/processed', ProcessedPing) #SPP subscribe the boat to the GPS messages, so the missions can easly get the data for their JSON messages self._hydrophone_freq_sub = self._node_handle.subscribe( 'hydrophones/desired_freq', Float64) self.servo_full_config_pub = self._node_handle.advertise( 'dynamixel/dynamixel_full_config', DynamixelFullConfig) self._send_constant_wrench_service = self._node_handle.get_service_client( 'send_constant_wrench', SendConstantWrench) self._lidar_sub = self._node_handle.subscribe('lidar/scan', LaserScan) if (need_trajectory == True): yield self._trajectory_sub.get_next_message() defer.returnValue(self)
def _init(self): self._trajectory_sub = self._node_handle.subscribe( 'trajectory', PoseTwistStamped) self._moveto_action_client = action.ActionClient( self._node_handle, 'moveto', MoveToAction) self._camera_2d_action_clients = dict( forward=action.ActionClient(self._node_handle, 'find2_forward_camera', legacy_vision_msg.FindAction), down=action.ActionClient(self._node_handle, 'find2_down_camera', legacy_vision_msg.FindAction), ) self._camera_3d_action_clients = dict( forward=action.ActionClient(self._node_handle, 'find_forward', object_finder_msg.FindAction), down=action.ActionClient(self._node_handle, 'find_down', object_finder_msg.FindAction), ) self._tf_listener = tf.TransformListener(self._node_handle) self._dvl_range_sub = self._node_handle.subscribe( 'dvl/range', Float64Stamped) yield self._trajectory_sub.get_next_message() defer.returnValue(self)
def _init(self): self._trajectory_sub = self._node_handle.subscribe('trajectory', PoseTwistStamped) self._moveto_action_client = action.ActionClient(self._node_handle, 'moveto', MoveToAction) self._tf_listener = tf.TransformListener(self._node_handle) yield self._trajectory_sub.get_next_message() defer.returnValue(self)
def f(nh): tf_broadcaster = tf.TransformBroadcaster(nh) @apply @util.cancellableInlineCallbacks def publish_thread(): try: while True: t = nh.get_time() tf_broadcaster.send_transform( TransformStamped( header=Header( stamp=t, frame_id='/parent', ), child_frame_id='/child', transform=Transform( translation=Vector3(*[1, 2, 3]), rotation=Quaternion( *transformations.quaternion_about_axis( t.to_sec(), [0, 0, 1])), ), )) yield nh.sleep(0.1) except Exception: traceback.print_exc() try: tf_listener = tf.TransformListener( nh, history_length=genpy.Duration(1) ) # short history length so that we cover history being truncated try: yield tf_listener.get_transform( '/parent', '/child', nh.get_time() - genpy.Duration(100)) except tf.TooPastError: pass else: self.fail('expected TooPastError') start_time = nh.get_time() for i in xrange(200): time = start_time + genpy.Duration.from_sec(1 + i / 100) dt = 1e-3 transform = yield tf_listener.get_transform( '/parent', '/child', time) transform2 = yield tf_listener.get_transform( '/parent', '/child', time + genpy.Duration.from_sec(1e-3)) assert numpy.allclose((transform2 - transform) / dt, [0, 0, 0, 0, 0, 1]) finally: yield publish_thread.cancel() publish_thread.addErrback( lambda fail: fail.trap(defer.CancelledError))
def _init(self): self._moveto_action_client = yield action.ActionClient(self._node_handle, 'moveto', MoveToAction) self._odom_sub = yield self._node_handle.subscribe('odom', Odometry) self._trajectory_sub = yield self._node_handle.subscribe('trajectory', PoseTwistStamped) self._trajectory_pub = yield self._node_handle.advertise('trajectory', PoseTwistStamped) self._dvl_range_sub = yield self._node_handle.subscribe('dvl/range', Float64Stamped) self._tf_listener = yield tf.TransformListener(self._node_handle) self.channel_marker = VisionProxy('vision/channel_marker', self._node_handle) self.buoy = VisionProxy('vision/buoys', self._node_handle) defer.returnValue(self)
def _init(self): self._moveto_action_client = yield action.ActionClient( self.nh, 'moveto', MoveToAction) self._odom_sub = yield self.nh.subscribe('odom', Odometry) self._change_wrench = yield self.nh.get_service_client( '/change_wrench', navigator_srvs.WrenchSelect) #self._enu_odom_sub = yield self.nh.subscribe('world_odom', Odometry) self.tf_listener = yield tf.TransformListener(self.nh) yield self.nh.sleep(.3) # Build tf and odom buffers defer.returnValue(self)
def _init(self): self._moveto_action_client = yield action.ActionClient( self.nh, 'moveto', MoveToAction) self._odom_sub = yield self.nh.subscribe('odom', Odometry) self._trajectory_sub = yield self.nh.subscribe('trajectory', PoseTwistStamped) self._trajectory_pub = yield self.nh.advertise('trajectory', PoseTwistStamped) self._dvl_range_sub = yield self.nh.subscribe('dvl/range', RangeStamped) self._tf_listener = yield tf.TransformListener(self.nh) self.vision_proxies = _VisionProxies(self.nh, 'vision_proxies.yaml') defer.returnValue(self)
def _init(cls, mission_server): super(SubjuGator, cls)._init(mission_server) cls._moveto_action_client = yield action.ActionClient( cls.nh, 'moveto', MoveToAction) cls._odom_sub = yield cls.nh.subscribe('odom', Odometry) cls._trajectory_sub = yield cls.nh.subscribe('trajectory', PoseTwistStamped) cls._trajectory_pub = yield cls.nh.advertise('trajectory', PoseTwistStamped) cls._dvl_range_sub = yield cls.nh.subscribe('dvl/range', RangeStamped) cls._tf_listener = yield tf.TransformListener(cls.nh) cls.vision_proxies = _VisionProxies(cls.nh, 'vision_proxies.yaml') cls.actuators = _ActuatorProxy(cls.nh) cls.test_mode = False
def init_(self, cam): image_sub = "/camera/front/right/image_rect_color" cam_info = "/camera/front/right/camera_info" yield self.nh.subscribe(image_sub, Image, self._img_cb) self._database = yield self.nh.get_service_client( '/database/requests', ObjectDBQuery) self._odom_sub = yield self.nh.subscribe( '/odom', Odometry, lambda odom: setattr(self, 'pose', nt.odometry_to_numpy(odom)[0])) self.cam_info_sub = yield self.nh.subscribe(cam_info, CameraInfo, self._info_cb) self.tf_listener = tf.TransformListener(self.nh) defer.returnValue(self)
def init(): if hasattr(Vrx, '_vrx_init'): return Vrx.from_lla = Vrx.nh.get_service_client("/fromLL", FromLL) Vrx.to_lla = Vrx.nh.get_service_client("/toLL", ToLL) Vrx.task_info_sub = Vrx.nh.subscribe("/vrx/task/info", Task) Vrx.scan_dock_color_sequence = Vrx.nh.get_service_client( "/vrx/scan_dock_deliver/color_sequence", ColorSequence) Vrx.fire_ball = Vrx.nh.advertise("/wamv/shooters/ball_shooter/fire", Empty) Vrx.station_keep_goal = Vrx.nh.subscribe("/vrx/station_keeping/goal", GeoPoseStamped) Vrx.wayfinding_path_sub = Vrx.nh.subscribe("/vrx/wayfinding/waypoints", GeoPath) Vrx.station_keeping_pose_error = Vrx.nh.subscribe( "/vrx/station_keeping/pose_error", Float64) Vrx.station_keeping_rms_error = Vrx.nh.subscribe( "/vrx/station_keeping/rms_error", Float64) Vrx.wayfinding_min_errors = Vrx.nh.subscribe( "/vrx/wayfinding/min_errors", Float64MultiArray) Vrx.wayfinding_mean_error = Vrx.nh.subscribe( "/vrx/wayfinding/mean_error", Float64) Vrx.perception_landmark = Vrx.nh.advertise("/vrx/perception/landmark", GeoPoseStamped) Vrx.animal_landmarks = Vrx.nh.subscribe("/vrx/wildlife/animals/poses", GeoPath) Vrx.beacon_landmark = Vrx.nh.get_service_client( "beaconLocator", AcousticBeacon) Vrx.circle_animal = Vrx.nh.get_service_client("/choose_animal", ChooseAnimal) Vrx.set_long_waypoint = Vrx.nh.get_service_client( "/set_long_waypoint", MoveToWaypoint) Vrx.darknet_objects = Vrx.nh.subscribe("/darknet_ros/bounding_boxes", BoundingBoxes) Vrx.tf_listener = tf.TransformListener(Vrx.nh) Vrx.database_response = Vrx.nh.get_service_client( '/database/requests', ObjectDBQuery) Vrx.get_two_closest_cones = Vrx.nh.get_service_client( '/get_two_closest_cones', TwoClosestCones) #Vrx.scan_dock_placard_symbol = Vrx.nh.subscribe("/vrx/scan_dock/placard_symbol", String) Vrx.front_left_camera_info_sub = None Vrx.front_left_camera_sub = None Vrx.front_right_camera_info_sub = None Vrx.front_right_camera_sub = None Vrx._vrx_init = True
def _init(self): self._moveto_action_client = yield action.ActionClient( self.nh, 'moveto', MoveToAction) self._odom_sub = yield self.nh.subscribe( 'odom', Odometry, lambda odom: setattr(self, 'pose', odometry_to_numpy(odom)[0])) self._ecef_odom_sub = yield self.nh.subscribe( 'absodom', Odometry, lambda odom: setattr(self, 'ecef_pose', odometry_to_numpy(odom)[0])) self._change_wrench = yield self.nh.get_service_client( '/change_wrench', navigator_srvs.WrenchSelect) self.tf_listener = yield tf.TransformListener(self.nh) print "Waiting for odom..." yield self._odom_sub.get_next_message( ) # We want to make sure odom is working before we continue yield self._ecef_odom_sub.get_next_message() defer.returnValue(self)
class Navigator(BaseMission): circle = "CIRCLE" cross = "CROSS" triangle = "TRIANGLE" red = "RED" green = "GREEN" blue = "BLUE" net_stc_results = None net_entrance_results = None max_grinch_effort = 500 def __init__(self, **kwargs): super(Navigator, self).__init__(**kwargs) @classmethod @util.cancellableInlineCallbacks def _init(cls, mission_runner): super(Navigator, cls)._init(mission_runner) cls.is_vrx = yield cls.nh.get_param("/is_vrx") cls._moveto_client = action.ActionClient(cls.nh, 'move_to', MoveAction) # 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._change_wrench = cls.nh.get_service_client('/wrench/select', MuxSelect) cls._change_trajectory = cls.nh.get_service_client( '/trajectory/select', MuxSelect) cls._database_query = cls.nh.get_service_client( '/database/requests', ObjectDBQuery) cls._reset_pcodar = cls.nh.get_service_client('/pcodar/reset', Trigger) cls._pcodar_set_params = cls.nh.get_service_client( '/pcodar/set_parameters', Reconfigure) cls.pose = None 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) if cls.is_vrx: yield cls._init_vrx() else: yield cls._init_not_vrx() @classmethod def _init_vrx(cls): cls.killed = False cls.odom_loss = False cls.tf_listener = tf.TransformListener(cls.nh) cls.set_vrx_classifier_enabled = cls.nh.get_service_client( '/vrx_classifier/set_enabled', SetBool) @classmethod @util.cancellableInlineCallbacks def _init_not_vrx(cls): 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.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 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._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') cls.tf_listener = tf.TransformListener(cls.nh) # Vision cls.obstacle_course_vision_enable = cls.nh.get_service_client( '/vision/obsc/enable', SetBool) cls.docks_vision_enable = cls.nh.get_service_client( '/vision/docks/enable', SetBool) yield cls._make_alarms() if cls.sim: fprint("Sim mode active!", title="NAVIGATOR") yield cls.nh.sleep(.5) else: # We want to make sure odom is working before we continue fprint("Action client do you yield?", title="NAVIGATOR") yield util.wrap_time_notice(cls._moveto_client.wait_for_server(), 2, "Lqrrt action server") fprint("Yes he yields!", title="NAVIGATOR") fprint("Waiting for odom...", title="NAVIGATOR") odom = util.wrap_time_notice(cls._odom_sub.get_next_message(), 2, "Odom listener") enu_odom = util.wrap_time_notice( cls._ecef_odom_sub.get_next_message(), 2, "ENU Odom listener") yield defer.gatherResults([odom, enu_odom ]) # Wait for all those to finish cls.docking_scan = 'NA'
def _init_vrx(cls): cls.killed = False cls.odom_loss = False cls.tf_listener = tf.TransformListener(cls.nh) cls.set_vrx_classifier_enabled = cls.nh.get_service_client( '/vrx_classifier/set_enabled', SetBool)
def main(): try: center = None nh = yield txros.NodeHandle.from_argv('shooter_finder', anonymous=True) pc_sub = nh.subscribe('/velodyne_points', PointCloud2) tf_listener = tf.TransformListener(nh) marker_pub = nh.advertise('/shooter_fit', MarkerArray) result_pub = nh.advertise('/shooter_pose', PoseStamped) odom_sub = nh.subscribe('/odom', Odometry) poller(nh) while True: ts = [time.time()] yield util.wall_sleep(.1) ts.append(time.time()) center = center_holder[0] odom = yield odom_sub.get_next_message() cloud = yield pc_sub.get_next_message() if center is None: yield util.wall_sleep(1) continue #print center Z_MIN = 0 RADIUS = 5 ts.append(time.time()) print 'start' print cloud.header.stamp try: transform = yield tf_listener.get_transform( '/enu', cloud.header.frame_id, cloud.header.stamp) except tf.TooPastError: print 'TooPastError!' continue gen = numpy.array( list( pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z")))).T print gen.shape print transform._p gen = (transform._q_mat.dot(gen) + numpy.vstack([transform._p] * gen.shape[1]).T).T good_points = numpy.array([ (x[0], x[1]) for x in gen if x[2] > Z_MIN and math.hypot(x[0] - center[0], x[1] - center[1]) < RADIUS ]) print 'end' if len(good_points) < 10: print 'no good points', len(good_points) continue #if len(good_points) > 100: # good_points = numpy.array(random.sample(list(good_points), 100)) #print good_points ts.append(time.time()) rect = cv2.minAreaRect(good_points.astype(numpy.float32)) rect = numpy.array(rect[0] + rect[1] + (math.radians(rect[2]), )) ts.append(time.time()) #rect = yield threads.deferToThread(fit, good_points, rect) if rect is None: print 'bad fit' continue rect = rect[:2], rect[2:4], rect[4] print 'rect:', rect ts.append(time.time()) marker_pub.publish( MarkerArray(markers=[ Marker( header=Header( frame_id='/enu', stamp=nh.get_time(), ), ns='shooter_ns', id=1, type=Marker.CUBE, action=Marker.ADD, pose=Pose( position=Point(rect[0][0], rect[0][1], .5), orientation=Quaternion( *transformations.quaternion_about_axis( rect[2], [0, 0, 1])), ), scale=Vector3(rect[1][0], rect[1][1], 2), color=ColorRGBA(1, 1, 1, .5), ) ], )) possibilities = [] for i in xrange(4): angle = rect[2] + i / 4 * 2 * math.pi normal = numpy.array([math.cos(angle), math.sin(angle)]) p = numpy.array(rect[0]) + (rect[1][0] if i % 2 == 0 else rect[1][1]) / 2 * normal possibilities.append( (normal, p, transformations.quaternion_about_axis(angle, [0, 0, 1]), rect[1][0] if i % 2 == 1 else rect[1][1])) best = max(possibilities, key=lambda (normal, p, q, size): (msg_helpers.ros_to_np_3D(odom.pose.pose.position)[:2] - rect[0]).dot(normal)) print 'side length:', best[-1] if abs(best[-1] - 1.8) > .25: print 'filtered out based on side length' continue front_points = [ p for p in good_points if abs((p - best[1]).dot(best[0])) < .2 ] print 'len(front_points)', len(front_points) a, b = numpy.linalg.lstsq( numpy.vstack([ [p[0] for p in front_points], [p[1] for p in front_points], ]).T, numpy.ones(len(front_points)), )[0] m, b_ = numpy.linalg.lstsq( numpy.vstack([ [p[0] for p in front_points], numpy.ones(len(front_points)), ]).T, [p[1] for p in front_points], )[0] print 'a, b', a, b, m, b_ print 'RES', numpy.std( [numpy.array([a, b]).dot(p) for p in good_points]) # x = a, b # x . p = 1 # |x| (||x|| . p) = 1 # ||x|| . p = 1 / |x| normal = numpy.array([a, b]) dist = 1 / numpy.linalg.norm(normal) normal = normal / numpy.linalg.norm(normal) p = dist * normal print 'ZZZ', p.dot(numpy.array([a, b])) perp = numpy.array([normal[1], -normal[0]]) x = (best[1] - p).dot(perp) p = p + x * perp if normal.dot(best[0]) < 0: normal = -normal q = transformations.quaternion_about_axis( math.atan2(normal[1], normal[0]), [0, 0, 1]) print 'XXX', p, best[1] #sqrt(a2 + b2) (a x + b y) = 1 result_pub.publish( PoseStamped( header=Header( frame_id='/enu', stamp=nh.get_time(), ), pose=Pose( position=Point(p[0], p[1], 0), orientation=Quaternion(*q), ), )) ts.append(time.time()) print 'timing', ts[-1] - ts[0], map(operator.sub, ts[1:], ts[:-1]) except: traceback.print_exc()
class Navigator(BaseTask): circle = "CIRCLE" cross = "CROSS" triangle = "TRIANGLE" red = "RED" green = "GREEN" blue = "BLUE" def __init__(self, **kwargs): super(Navigator, self).__init__(**kwargs) @classmethod @util.cancellableInlineCallbacks def _init(cls, task_runner): super(Navigator, cls)._init(task_runner) cls.vision_proxies = {} cls._load_vision_services() 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) 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') cls.tf_listener = tf.TransformListener(cls.nh) yield cls._make_alarms() if cls.sim: fprint("Sim mode active!", title="NAVIGATOR") yield cls.nh.sleep(.5) else: # We want to make sure odom is working before we continue fprint("Action client do you yield?", title="NAVIGATOR") yield util.wrap_time_notice(cls._moveto_client.wait_for_server(), 2, "Lqrrt action server") fprint("Yes he yields!", title="NAVIGATOR") fprint("Waiting for odom...", title="NAVIGATOR") odom = util.wrap_time_notice(cls._odom_sub.get_next_message(), 2, "Odom listener") enu_odom = util.wrap_time_notice(cls._ecef_odom_sub.get_next_message(), 2, "ENU Odom listener") yield defer.gatherResults([odom, enu_odom]) # Wait for all those to finish
def _init(self, need_trajectory=True, need_odom=True): self._trajectory_sub = self._node_handle.subscribe( 'trajectory', PoseTwistStamped) self._moveto_action_client = action.ActionClient( self._node_handle, 'moveto', MoveToAction) self._tf_listener = tf.TransformListener(self._node_handle) self._camera_2d_action_clients = dict(forward=action.ActionClient( self._node_handle, 'find2_forward_camera', legacy_vision_msg.FindAction), ) self._camera_3d_action_clients = dict(forward=action.ActionClient( self._node_handle, 'find_forward', object_finder_msg.FindAction), ) self._odom_sub = self._node_handle.subscribe('odom', Odometry) self._wrench_sub = self._node_handle.subscribe('wrench', WrenchStamped) #The absodom topic has the lat long information ebeded in the nav_msgs/Odometry. the lat long is under pose.pose.position and this is what is reported back in the JSON messages for a chalenge. self._absodom_sub = self._node_handle.subscribe('absodom', Odometry) #SPP subscribe the boat to the processed pings, so the missions can utalize the processed acoustic pings self._hydrophone_ping_sub = self._node_handle.subscribe( 'hydrophones/processed', ProcessedPing) #SPP subscribe the boat to the GPS messages, so the missions can easly get the data for their JSON messages self._hydrophone_freq_sub = self._node_handle.subscribe( 'hydrophones/desired_freq', Float64) self.servo_full_config_pub = self._node_handle.advertise( 'dynamixel/dynamixel_full_config', DynamixelFullConfig) self._send_constant_wrench_service = self._node_handle.get_service_client( 'send_constant_wrench', SendConstantWrench) self._set_lidar_mode = self._node_handle.get_service_client( 'lidar/lidar_servo_mode', lidar_servo_mode) self._set_path_planner_mode = self._node_handle.get_service_client( '/azi_waypoint_mode', trajectory_mode) self._lidar_sub_raw = self._node_handle.subscribe( 'lidar/scan', LaserScan) self._lidar_sub_pointcloud = self._node_handle.subscribe( 'lidar/raw_pc', PointCloud2) self._buoy_sub = self._node_handle.subscribe('/object_handling/buoys', object_handling.msg.Buoys) self._gate_sub = self._node_handle.subscribe('/object_handling/gates', object_handling.msg.Gates) self._start_gate_vision_sub = self._node_handle.subscribe( 'start_gate_vision', Float64) self._circle_sub = self._node_handle.subscribe("Circle_Sign", Circle) self._cross_sub = self._node_handle.subscribe("Cross_Sign", Cross) self._triangle_sub = self._node_handle.subscribe( "Triangle_Sign", Triangle) self._odom_pub = self._node_handle.advertise('odom', Odometry) self.float_srv = self._node_handle.get_service_client( '/float_mode', AziFloat) self_bouy_subsriber = self._node_handle.subscribe( 'vision_sandbox/buoy_info', vision_sandbox.msg.Buoys) # Make sure trajectory topic is publishing if (need_trajectory == True): print 'Boat class __init__: Waiting on trajectory..' yield self._trajectory_sub.get_next_message() print 'Boat class __init__: Got trajectory' # Make sure odom is publishing if (need_odom == True): print 'Boat class __init__: Waiting on odom...' yield self._odom_sub.get_next_message() print 'Boat class __init__: Got odom' defer.returnValue(self)