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)
