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)
