def odom_sync_cb(self, odom, absodom):
'''
Called each time and odom and absodom arrive with the same stamp. Caches these
values for later conversions and publishes current LLA.
'''
if self.first: # Advertise convert service on first odom/absodom pair
self.convert_service = rospy.Service('convert', CoordinateConversion, self.convert_cb)
self.first = False
self.stamp = odom.header.stamp
self.enu_pos = odometry_to_numpy(odom)[0][0]
self.ecef_pos = odometry_to_numpy(absodom)[0][0]
self.last_pair = (odom, absodom)
self.publish_lla()
def odom_sync_cb(self, odom, absodom):
'''
Called each time and odom and absodom arrive with the same stamp. Caches these
values for later conversions and publishes current LLA.
'''
if self.first: # Advertise convert service on first odom/absodom pair
self.convert_service = rospy.Service('convert',
CoordinateConversion,
self.convert_cb)
self.first = False
self.stamp = odom.header.stamp
self.enu_pos = odometry_to_numpy(odom)[0][0]
self.ecef_pos = odometry_to_numpy(absodom)[0][0]
self.last_pair = (odom, absodom)
self.publish_lla()
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 = odometry_to_numpy(last_odom)[0][0]
self.ecef_pos = 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 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 = odometry_to_numpy(last_odom)[0][0]
self.ecef_pos = 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 tx_ecef_pose(self):
last_odom_msg = yield self._ecef_odom_sub.get_next_message()
defer.returnValue(mil_tools.odometry_to_numpy(last_odom_msg)[0])
def enu_odom_set(odom):
return setattr(cls, 'ecef_pose',
mil_tools.odometry_to_numpy(odom)[0])
def odom_set(odom):
return setattr(cls, 'pose', mil_tools.odometry_to_numpy(odom)[0])
def _odom_cb(self, odom):
self.position, self.rot = nt.odometry_to_numpy(odom)[0]
def enu_odom_set(odom):
return setattr(cls, 'ecef_pose', mil_tools.odometry_to_numpy(odom)[0])
def odom_set(odom):
return setattr(cls, 'pose', mil_tools.odometry_to_numpy(odom)[0])
def tx_ecef_pose(self):
last_odom_msg = yield self._ecef_odom_sub.get_next_message()
defer.returnValue(mil_tools.odometry_to_numpy(last_odom_msg)[0])
def _odom_cb(self, odom):
self.position, self.rot = nt.odometry_to_numpy(odom)[0]
