def got_request(self, srv):
self.point = np.array(srv.point)
self.enu_pos = navigator_tools.odometry_to_numpy((yield self.odom_sub.get_last_message()))[0][0]
self.ecef_pos = navigator_tools.odometry_to_numpy((yield self.abs_odom_sub.get_last_message()))[0][0]
enu, ecef, lla = getattr(self, srv.frame)()
distance = np.linalg.norm(enu - self.enu_pos)
defer.returnValue(CoordinateConversionResponse(enu=enu, ecef=ecef, lla=lla, distance=distance))
def request(self, srv):
self.point = np.array(srv.point)
last_odom = yield self.odom_sub.get_last_message()
last_absodom = yield self.abs_odom_sub.get_last_message()
if last_odom is None or last_absodom is None:
defer.returnValue(CoordinateConversionResponse())
self.enu_pos = navigator_tools.odometry_to_numpy(last_odom)[0][0]
self.ecef_pos = navigator_tools.odometry_to_numpy(last_absodom)[0][0]
enu, ecef, lla = getattr(self, srv.frame)()
distance = np.linalg.norm(enu - self.enu_pos)
defer.returnValue(CoordinateConversionResponse(enu=enu, ecef=ecef, lla=lla, distance=distance))
def init_(self, cam):
image_sub = "/stereo/right/image_rect_color"
cam_info = "/stereo/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(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, 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 tx_ecef_pose(self):
last_odom_msg = yield self._ecef_odom_sub.get_next_message()
defer.returnValue(odometry_to_numpy(last_odom_msg)[0])
def got_odom(self, msg):
pose, twist = navigator_tools.odometry_to_numpy(msg)[:2]
#self.draw_shape(pose)
self._nav_dummy.pose = pose
def _odom_cb(self, odom):
self.position, self.rot = nt.odometry_to_numpy(odom)[0]
def pose(self):
last_odom_msg = self._odom_sub.get_last_message()
return odometry_to_numpy(last_odom_msg)[0]
def _get_position(self):
last_odom_msg = yield self._odom_sub.get_next_message()
defer.returnValue(nt.odometry_to_numpy(last_odom_msg)[0][0])
def pose(self):
last_odom_msg = self._odom_sub.get_last_message()
return odometry_to_numpy(last_odom_msg)[0]
