Feature/slam/propagate state

src/slam/graph_slam/config/graph_slam_config.yaml

@@ -10,6 +10,7 @@ graph_slam:
     slam_stats_topic: "/slam/stats"
     global_frame: "arussim/world"
     local_frame: "slam/vehicle"
+    integration_method: "Euler"           #        Options: "RK2" o "Euler"
 
     set_user_lambda_init: 10.0          #        Initial lambda of the optimizer
     optimizer_freq: 10.0             # Hz     Frequency at which the optimizer works

src/slam/graph_slam/include/graph_slam/graph_slam_node.hpp

@@ -69,7 +69,8 @@ class GraphSlam : public rclcpp::Node
     double perception_noise_ = 0.05;
     double dt = 0.005;
     g2o::VertexSE2* last_optimized_pose_ = nullptr;
-    std::vector<Eigen::Vector3d> odom_buffer_;
+    struct OdomBuffer { Eigen::Vector3d u; double dt; };
+    std::vector<OdomBuffer> odom_buffer_;
 
     // Graph variables
     DataAssociation DA;
@@ -113,6 +114,7 @@ class GraphSlam : public rclcpp::Node
     std::string kGlobalFrame;
     std::string kLocalFrame;
     std::string kMapFrame;
+    std::string kIntegrationMethod;
 
 
     double kSetUserLambdaInit;

src/slam/graph_slam/src/graph_slam_node.cpp

@@ -10,6 +10,7 @@
 GraphSlam::GraphSlam() : Node("graph_slam")
 { 
     // Declare parameters
+    this->declare_parameter("integration_method", "RK2");
     this->declare_parameter("set_user_lambda_init", 10.0);
     this->declare_parameter("lidar_topic", "/perception/map");
     this->declare_parameter("camera_topic", "/camera/map");
@@ -64,6 +65,7 @@ GraphSlam::GraphSlam() : Node("graph_slam")
     this->declare_parameter("debug", true);
 
     // Get parameters
+    this->get_parameter("integration_method", kIntegrationMethod);
     this->get_parameter("set_user_lambda_init", kSetUserLambdaInit);
     this->get_parameter("lidar_topic", kLidarTopic);
     this->get_parameter("camera_topic", kCameraTopic);
@@ -190,24 +192,47 @@ GraphSlam::GraphSlam() : Node("graph_slam")
 /////////////////////////////////////////////////////////////////////////
 
 void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg)
-{   
+{ 
+    // Real time diff calculation
+    double current_time = rclcpp::Time(msg->header.stamp).seconds();
+    double dt = current_time - prev_t_.seconds();
+
+    if (prev_t_.seconds() == 0.0 || dt <= 0.0 || dt > 0.1) {
+        dt = 0.005; // Default value (200 Hz)
+    }
+    prev_t_ = rclcpp::Time(msg->header.stamp);
+
     // Update pose estimation from odometry
     double phi = vehicle_pose_(2);
-    B.block(0, 0, 2, 2) << std::cos(phi), -std::sin(phi), std::sin(phi), std::cos(phi);
-    u << msg->vx, msg->vy, msg->r;
-    vehicle_pose_ += B * u * dt; // TODO check phi in range [-pi, pi]
-    driven_distance_ += (B * u * dt).norm(); 
+    double r = msg->r;
+    u << msg->vx, msg->vy, r;
+
+    if (kIntegrationMethod == "Euler") {
+        B.block(0, 0, 2, 2) << std::cos(phi), -std::sin(phi), std::sin(phi), std::cos(phi);
+
+    } else if (kIntegrationMethod == "RK2") {
+        double phi_mid = phi + (r * dt / 2.0);
+        B.block(0, 0, 2, 2) << std::cos(phi_mid), -std::sin(phi_mid), std::sin(phi_mid), std::cos(phi_mid);
+
+    } else {
+        RCLCPP_WARN(this->get_logger(), 
+                         "Unknown integration method: '%s'. Fallback to Euler.", kIntegrationMethod.c_str());       
+        B.block(0, 0, 2, 2) << std::cos(phi), -std::sin(phi), std::sin(phi), std::cos(phi);
+    }
+
+    Eigen::Vector3d delta_pose = B * u * dt;
+    vehicle_pose_ += delta_pose; 
+    driven_distance_ += delta_pose.head<2>().norm();
 
     // Add the odometry to the buffer to re-predict the pose after optimization
-    odom_buffer_.push_back(u);
-    
+    odom_buffer_.push_back({u, dt});
+
     if (vehicle_pose_(2) > M_PI) {
         vehicle_pose_(2) -= 2*M_PI;
     } else if (vehicle_pose_(2) < -M_PI) {
         vehicle_pose_(2) += 2*M_PI;
     }
 
-    prev_t_ = this->now();
     // Publish the vehicle pose as a tf
     send_tf();
     // Check if the vehicle has crossed the finish line
@@ -250,7 +275,6 @@ void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg)
 }
 
 
-
 void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
 {   
     std::vector<Landmark> observed_landmarks;
@@ -550,19 +574,32 @@ void GraphSlam::update_pose_predictions(){
     int index = last_optimized_pose_->id();
     Eigen::Matrix3d B2 = Eigen::Matrix3d::Identity();
 
-    // Re-predict the pose for each odometry measurement in the buffer
-    for (auto u2: odom_buffer_){
+    // Re-predict the pose for each odometry measurement
+    for (const auto& step : odom_buffer_) {
+        Eigen::Vector3d u2 = step.u;
+        double dt_hist = step.dt;
+
         double phi = updated_pose(2);
-        B2.block(0, 0, 2, 2) << std::cos(phi), -std::sin(phi), std::sin(phi), std::cos(phi);
-        updated_pose += B2 * u2 * dt;
+        double r = u2(2); 
+
+        if (kIntegrationMethod == "Euler") {
+            // EULER
+            B2.block(0, 0, 2, 2) << std::cos(phi), -std::sin(phi), std::sin(phi), std::cos(phi);
+
+        } else if (kIntegrationMethod == "RK2") {
+            // RK2
+            double phi_mid = phi + (r * dt_hist / 2.0);
+            B2.block(0, 0, 2, 2) << std::cos(phi_mid), -std::sin(phi_mid), std::sin(phi_mid), std::cos(phi_mid);
+
+        } else {
+            // EULER as fallback
+            B2.block(0, 0, 2, 2) << std::cos(phi), -std::sin(phi), std::sin(phi), std::cos(phi);
+        }
+
+        updated_pose += B2 * u2 * dt_hist;
 
         updated_poses.push_back(updated_pose);
     }
-
-    // Update the pose vertices with the new predictions
-    for(auto pose_vertex: pose_vertices_to_add_){
-        pose_vertex->setEstimate(g2o::SE2(updated_poses[(pose_vertex->id() - index)/2]));
-    }
 
     odom_buffer_.clear();