コード例 #1
0
 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
コード例 #2
0
 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
コード例 #3
0
    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])