def odometry_cb(self, channel, msg):
m = Odometry.decode(msg)
self.OdometryMsg.latitude_deg = m.latitude_deg
self.OdometryMsg.latitude_min = m.latitude_min
self.OdometryMsg.longitude_deg = m.longitude_deg
self.OdometryMsg.longitude_min = m.longitude_min
self.OdometryMsg.bearing_deg = m.bearing_deg
self.OdometryMsg.speed = m.speed
def odom_callback(self, channel, msg):
odom = Odometry.decode(msg)
if (time.time()*1000 - self.odom_millis) > \
self.logConfig['rate_millis']['odom']:
self.write([
odom.latitude_deg, odom.latitude_min, odom.longitude_deg,
odom.longitude_min, odom.bearing_deg, odom.speed
], 'odom')
self.odom_millis = time.time() * 1000
def odomCallback(self, channel, msg):
new_odom_msg = Odometry.decode(msg)
lat = min2decimal(new_odom_msg.latitude_deg, new_odom_msg.latitude_min)
lon = min2decimal(new_odom_msg.longitude_deg,
new_odom_msg.longitude_min)
new_odom = np.array([[
0.0,
lat2meters(lat, ref_lat=self.ref_coords["lat"]),
lon2meters(lon, lat, ref_lon=self.ref_coords["lon"]),
new_odom_msg.bearing_deg, new_odom_msg.speed
]])
if self.odom_history is None:
self.odom_history = new_odom
else:
new_odom[0, 0] = self.odom_history[-1, 0] + self.sim_config["dt"]
self.odom_history = np.vstack([self.odom_history, new_odom])