From 28ce017fa28025735c140121186b3a538f1ab189 Mon Sep 17 00:00:00 2001 From: lp05arn Date: Sun, 8 Mar 2026 13:11:10 +0000 Subject: [PATCH 1/4] =?UTF-8?q?feat:=20Implementation=20of=20a=20second-or?= =?UTF-8?q?der=20Runge=E2=80=93Kutta=20method=20to=20propagate=20the=20car?= =?UTF-8?q?'s=20state.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/slam/graph_slam/src/graph_slam_node.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index fdaf6ffd..b7180a4a 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -193,9 +193,12 @@ void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg) { // 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; + double r = msg->r; + 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); + u << msg->vx, msg->vy, r; vehicle_pose_ += B * u * dt; // TODO check phi in range [-pi, pi] + vehicle_pose_(2) = std::atan2(std::sin(vehicle_pose_(2)), std::cos(vehicle_pose_(2))); driven_distance_ += (B * u * dt).norm(); // Add the odometry to the buffer to re-predict the pose after optimization @@ -555,8 +558,11 @@ void GraphSlam::update_pose_predictions(){ // Re-predict the pose for each odometry measurement in the buffer for (auto u2: odom_buffer_){ double phi = updated_pose(2); - B2.block(0, 0, 2, 2) << std::cos(phi), -std::sin(phi), std::sin(phi), std::cos(phi); + double r = u2(2); // Velocidad angular (yaw rate) del vector u2 + double phi_mid = phi + (r * dt / 2.0); + B2.block(0, 0, 2, 2) << std::cos(phi_mid), -std::sin(phi_mid), std::sin(phi_mid), std::cos(phi_mid); updated_pose += B2 * u2 * dt; + updated_pose(2) = std::atan2(std::sin(updated_pose(2)), std::cos(updated_pose(2))); // Mantenemos el ángulo en el rango [-pi, pi] updated_poses.push_back(updated_pose); } From 94d8b4d1803933be26db86f46105a8a2d7863222 Mon Sep 17 00:00:00 2001 From: lp05arn Date: Sat, 14 Mar 2026 12:29:36 +0000 Subject: [PATCH 2/4] =?UTF-8?q?Implementation=20of=20a=20second-order=20Ru?= =?UTF-8?q?nge=E2=80=93Kutta=20method=20improved=20and=20dt=20calculation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/graph_slam/graph_slam_node.hpp | 3 +- src/slam/graph_slam/src/graph_slam_node.cpp | 46 +++++++++++++------ 2 files changed, 33 insertions(+), 16 deletions(-) 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 676aee2d..47836cdc 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 @@ -67,7 +67,8 @@ class GraphSlam : public rclcpp::Node double perception_noise_ = 0.05; double dt = 0.01; 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; diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index b7180a4a..fa8b6b67 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -190,27 +190,38 @@ GraphSlam::GraphSlam() : Node("graph_slam") ///////////////////////////////////////////////////////////////////////// void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg) -{ +{ + // Cálculo tiempo real y dt + double current_time = rclcpp::Time(msg->header.stamp).seconds(); + double dt = current_time - prev_t_.seconds(); + + // Si es el primer mensaje (prev_t_ es 0), o hay un error de tiempo (<=0), o el simulador da un tirón enorme (> 0.1s)... + if (prev_t_.seconds() == 0.0 || dt <= 0.0 || dt > 0.1) { + dt = 0.01; // ...usamos el fijo de 100Hz + } + prev_t_ = rclcpp::Time(msg->header.stamp); + // Update pose estimation from odometry double phi = vehicle_pose_(2); double r = msg->r; 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); + B.block(0, 0, 2, 2) << std::cos(phi_mid), -std::sin(phi_mid), std::sin(phi_mid), std::cos(phi_mid); u << msg->vx, msg->vy, r; + Eigen::Vector3d delta_pose = B * u * dt; + vehicle_pose_ += B * u * dt; // TODO check phi in range [-pi, pi] vehicle_pose_(2) = std::atan2(std::sin(vehicle_pose_(2)), std::cos(vehicle_pose_(2))); - driven_distance_ += (B * u * dt).norm(); + 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 @@ -255,7 +266,6 @@ void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg) } - void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { if (pose_vertices_.size() == 0) { @@ -556,17 +566,23 @@ void GraphSlam::update_pose_predictions(){ Eigen::Vector3d u2 = Eigen::Vector3d::Zero(); // 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); - double r = u2(2); // Velocidad angular (yaw rate) del vector u2 - double phi_mid = phi + (r * dt / 2.0); - B2.block(0, 0, 2, 2) << std::cos(phi_mid), -std::sin(phi_mid), std::sin(phi_mid), std::cos(phi_mid); - updated_pose += B2 * u2 * dt; - updated_pose(2) = std::atan2(std::sin(updated_pose(2)), std::cos(updated_pose(2))); // Mantenemos el ángulo en el rango [-pi, pi] + double r = u2(2); + 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); + + updated_pose += B2 * u2 * dt_hist; + updated_pose(2) = std::atan2(std::sin(updated_pose(2)), std::cos(updated_pose(2))); 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])); @@ -593,7 +609,7 @@ void GraphSlam::update_pose_predictions(){ void GraphSlam::fill_graph(){ // Get last optimized pose from which predictions will be made - last_optimized_pose_ = pose_vertices_.back(); + last_optimized_pose_ = pose_vertices_.back(); // Add vertices and edges to the optimizer for (auto vertex: pose_vertices_to_add_){ From 00b49a908af9a971d41abed18250cffc22718f5c Mon Sep 17 00:00:00 2001 From: lp05arn Date: Thu, 26 Mar 2026 13:36:46 +0000 Subject: [PATCH 3/4] feat: add parameter to select integration method (Euler/RK2) in config --- .../graph_slam/config/graph_slam_config.yaml | 1 + .../include/graph_slam/graph_slam_node.hpp | 1 + src/slam/graph_slam/src/graph_slam_node.cpp | 60 ++++++++++++++----- 3 files changed, 46 insertions(+), 16 deletions(-) diff --git a/src/slam/graph_slam/config/graph_slam_config.yaml b/src/slam/graph_slam/config/graph_slam_config.yaml index 2fe55905..4d898ae2 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: 3.33333 # 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 47836cdc..fac26e27 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 @@ -111,6 +111,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 fa8b6b67..780b8481 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); @@ -191,7 +193,7 @@ GraphSlam::GraphSlam() : Node("graph_slam") void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg) { - // Cálculo tiempo real y dt + /// Cálculo tiempo real y dt double current_time = rclcpp::Time(msg->header.stamp).seconds(); double dt = current_time - prev_t_.seconds(); @@ -204,18 +206,37 @@ void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg) // Update pose estimation from odometry double phi = vehicle_pose_(2); double r = msg->r; - 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); u << msg->vx, msg->vy, r; - Eigen::Vector3d delta_pose = B * u * dt; - vehicle_pose_ += B * u * dt; // TODO check phi in range [-pi, pi] + if (kIntegrationMethod == "Euler") { + // EULER: Usamos el yaw actual (phi) directamente + B.block(0, 0, 2, 2) << std::cos(phi), -std::sin(phi), std::sin(phi), std::cos(phi); + + } else if (kIntegrationMethod == "RK2") { + // RK2: Usamos el yaw proyectado a la mitad del dt (phi_mid) + 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 { + // PROTECCIÓN: Por si hay un error en el config.yaml, avisamos y usamos RK2 por defecto + RCLCPP_WARN_ONCE(this->get_logger(), + "Método de integración '%s' no reconocido. Usando RK2 por defecto.", + kIntegrationMethod.c_str()); + + 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); + } + + Eigen::Vector3d delta_pose = B * u * dt; + vehicle_pose_ += delta_pose; + + // Normalizamos el ángulo phi para que se quede en el rango [-pi, pi] vehicle_pose_(2) = std::atan2(std::sin(vehicle_pose_(2)), std::cos(vehicle_pose_(2))); 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, dt}); - + if (vehicle_pose_(2) > M_PI) { vehicle_pose_(2) -= 2*M_PI; } else if (vehicle_pose_(2) < -M_PI) { @@ -565,7 +586,6 @@ void GraphSlam::update_pose_predictions(){ Eigen::Matrix3d B2 = Eigen::Matrix3d::Identity(); Eigen::Vector3d u2 = Eigen::Vector3d::Zero(); - // Re-predict the pose for each odometry measurement in the buffer // Re-predict the pose for each odometry measurement for (const auto& step : odom_buffer_) { Eigen::Vector3d u2 = step.u; @@ -573,21 +593,29 @@ void GraphSlam::update_pose_predictions(){ double phi = updated_pose(2); double r = u2(2); - 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); + 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 { + // PROTECCIÓN (usando RK2 por defecto) + 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); + } + // Actualizamos la pose y la normalizamos updated_pose += B2 * u2 * dt_hist; updated_pose(2) = std::atan2(std::sin(updated_pose(2)), std::cos(updated_pose(2))); 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(); // Update the vehicle pose with the last prediction @@ -626,8 +654,8 @@ void GraphSlam::fill_graph(){ edges_to_add_.clear(); // Deactivate edges exceding the maximum - if(pose_edges_.size()-pose_edges_deactivated_ > kMaxPoseEdges){ - for (int i=pose_edges_deactivated_; i kMaxPoseEdges) { + for (int i = pose_edges_deactivated_; i < (int)pose_edges_.size() - kMaxPoseEdges; i++) { if (pose_edges_[i]->graph()) pose_edges_[i]->setLevel(kLevelPoseEdges); // Deactivate edge } pose_edges_deactivated_ = pose_edges_.size()-kMaxPoseEdges; From a854a5e583c732125e6fb5c5101efca5a4b51406 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ignacio=20S=C3=A1nchez?= Date: Sun, 29 Mar 2026 21:26:23 +0200 Subject: [PATCH 4/4] refactor: change comments to meet ARUS coding standards --- src/slam/graph_slam/src/graph_slam_node.cpp | 29 ++++++--------------- 1 file changed, 8 insertions(+), 21 deletions(-) diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index 5af83e19..58d606a0 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -193,13 +193,12 @@ GraphSlam::GraphSlam() : Node("graph_slam") void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg) { - /// Cálculo tiempo real y dt + // Real time diff calculation double current_time = rclcpp::Time(msg->header.stamp).seconds(); double dt = current_time - prev_t_.seconds(); - // Si es el primer mensaje (prev_t_ es 0), o hay un error de tiempo (<=0), o el simulador da un tirón enorme (> 0.1s)... if (prev_t_.seconds() == 0.0 || dt <= 0.0 || dt > 0.1) { - dt = 0.01; // ...usamos el fijo de 100Hz + dt = 0.005; // Default value (200 Hz) } prev_t_ = rclcpp::Time(msg->header.stamp); @@ -209,29 +208,20 @@ void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg) u << msg->vx, msg->vy, r; if (kIntegrationMethod == "Euler") { - // EULER: Usamos el yaw actual (phi) directamente B.block(0, 0, 2, 2) << std::cos(phi), -std::sin(phi), std::sin(phi), std::cos(phi); } else if (kIntegrationMethod == "RK2") { - // RK2: Usamos el yaw proyectado a la mitad del dt (phi_mid) 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 { - // PROTECCIÓN: Por si hay un error en el config.yaml, avisamos y usamos RK2 por defecto - RCLCPP_WARN_ONCE(this->get_logger(), - "Método de integración '%s' no reconocido. Usando RK2 por defecto.", - kIntegrationMethod.c_str()); - - 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); + 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; - - // Normalizamos el ángulo phi para que se quede en el rango [-pi, pi] - vehicle_pose_(2) = std::atan2(std::sin(vehicle_pose_(2)), std::cos(vehicle_pose_(2))); driven_distance_ += delta_pose.head<2>().norm(); // Add the odometry to the buffer to re-predict the pose after optimization @@ -602,14 +592,11 @@ void GraphSlam::update_pose_predictions(){ B2.block(0, 0, 2, 2) << std::cos(phi_mid), -std::sin(phi_mid), std::sin(phi_mid), std::cos(phi_mid); } else { - // PROTECCIÓN (usando RK2 por defecto) - 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); + // EULER as fallback + B2.block(0, 0, 2, 2) << std::cos(phi), -std::sin(phi), std::sin(phi), std::cos(phi); } - // Actualizamos la pose y la normalizamos updated_pose += B2 * u2 * dt_hist; - updated_pose(2) = std::atan2(std::sin(updated_pose(2)), std::cos(updated_pose(2))); updated_poses.push_back(updated_pose); } @@ -635,7 +622,7 @@ void GraphSlam::update_pose_predictions(){ void GraphSlam::fill_graph(){ // Get last optimized pose from which predictions will be made - last_optimized_pose_ = pose_vertices_.back(); + last_optimized_pose_ = pose_vertices_.back(); // Add vertices and edges to the optimizer for (auto vertex: pose_vertices_to_add_){