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, 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(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()
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 _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 _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(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 _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): 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)
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)