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])