Added prediction with latest imu timestamp and added timestamp to odom msg

Author: ongdexter

glider/include/glider/core/odometry.hpp

@@ -35,6 +35,7 @@ class Odometry
         Odometry(gtsam::Values& val, gtsam::Key key, double scale, int64_t timestamp, bool init = true);
         Odometry(gtsam::NavState& ns, bool init = true);
         Odometry(gtsam::NavState& ns, Eigen::Vector3d& gyro, bool init = true);
+        Odometry(gtsam::NavState& ns, Eigen::Vector3d& gyro, int64_t timestamp, bool init = true);
 
         template<typename T>
         T getPose() const;

glider/include/ros/glider_node.hpp

@@ -72,5 +72,7 @@ class GliderNode : public rclcpp::Node
 
         // tracker
         Glider::State current_state_;
+
+        int64_t latest_imu_timestamp_;
 };
 }

glider/ros/glider_node.cpp

@@ -57,6 +57,8 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide
 
     current_state_ = Glider::State::Uninitialized();
 
+    latest_imu_timestamp_ = 0;
+
     if (use_odom)
     {
         auto odom_sub_options = rclcpp::SubscriptionOptions();
@@ -94,7 +96,11 @@ void GliderNode::interpolationCallback()
 {
     if (!initialized_ || !current_state_.isInitialized()) return;
 
-    int64_t timestamp = getTime(this->now());
+    if (latest_imu_timestamp_ <= 0)
+    {
+        return;
+    }
+    int64_t timestamp = latest_imu_timestamp_;
     Glider::Odometry odom = glider_->interpolate(timestamp);
 
     if (!odom.isInitialized()) return;
@@ -121,6 +127,8 @@ void GliderNode::imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
     int64_t timestamp = getTime(msg->header.stamp);
 
     glider_->addIMU(timestamp, accel, gyro, orient);
+
+    latest_imu_timestamp_ = timestamp;
 }
 
 void GliderNode::magCallback(const sensor_msgs::msg::MagneticField::ConstSharedPtr msg)

glider/src/factor_manager.cpp

@@ -275,7 +275,7 @@ Odometry FactorManager::predict(int64_t timestamp)
     }
     gtsam::NavState result = pim_copy_->predict(current_state_.getNavState(), bias_);
 
-    Odometry ret(result, gyro_);
+    Odometry ret(result, gyro_, timestamp);
 
     return ret; 
 }

glider/src/odometry.cpp

@@ -50,6 +50,19 @@ Odometry::Odometry(gtsam::NavState& ns, Eigen::Vector3d& gyro, bool init)
     initialized_ = init;
 }
 
+Odometry::Odometry(gtsam::NavState& ns, Eigen::Vector3d& gyro, int64_t timestamp, bool init)
+{
+    position_ = ns.position();
+    orientation_ = ns.attitude();
+    pose_ = gtsam::Pose3(orientation_, position_);
+    velocity_ = ns.v();
+    altitude_ = position_.z();
+    heading_ = orientation_.yaw();
+    gyro_ = gyro;
+    initialized_ = init;
+    timestamp_ = timestamp;
+}
+
 Odometry Odometry::Uninitialized()
 {
     Odometry odom;