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]