diff --git a/src/slam/graph_slam/config/graph_slam_config.yaml b/src/slam/graph_slam/config/graph_slam_config.yaml index 794f73ee..e45971fd 100644 --- a/src/slam/graph_slam/config/graph_slam_config.yaml +++ b/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 diff --git a/src/slam/graph_slam/include/graph_slam/graph_slam_node.hpp b/src/slam/graph_slam/include/graph_slam/graph_slam_node.hpp index 629249e6..26caee63 100644 --- a/src/slam/graph_slam/include/graph_slam/graph_slam_node.hpp +++ b/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 odom_buffer_; + struct OdomBuffer { Eigen::Vector3d u; double dt; }; + std::vector 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; diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index b6acc555..58d606a0 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/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 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();