From d959788a4b4416f42615647f461c2ed43c9f9757 Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Wed, 11 Mar 2026 13:05:29 +0100 Subject: [PATCH 01/14] fix: modify parameters and add new functionalities to solve localization issues --- .../graph_slam/config/graph_slam_config.yaml | 10 +++---- .../include/graph_slam/graph_slam_node.hpp | 1 - src/slam/graph_slam/src/graph_slam_node.cpp | 26 ++++++++++++------- 3 files changed, 21 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..2178fb61 100644 --- a/src/slam/graph_slam/config/graph_slam_config.yaml +++ b/src/slam/graph_slam/config/graph_slam_config.yaml @@ -12,18 +12,18 @@ graph_slam: local_frame: "slam/vehicle" set_user_lambda_init: 10.0 # Initial lambda of the optimizer - optimizer_freq: 3.33333 # Hz Frequency at which the optimizer works + optimizer_freq: 7.0 # Hz Frequency at which the optimizer works finish_line_offset: 6.0 # m Offset of the finishing line regarding the beginning track_width: 3.0 # m Width of the track min_lap_distance: 30.0 # m Min distance to consider a new lap - max_pose_edges: 10000 # Max number of pose edges given to the optimizer + max_pose_edges: 7000 # Max number of pose edges given to the optimizer level_pose_edges: 2 # Change pose edge to level _ in the optimizer level_landmark_edges: 3 # Change landmark edge to level _ in the optimizer - max_landmark_edges: 10000 # Max number of landmark edges given to the optimizer + max_landmark_edges: 6000 # Max number of landmark edges given to the optimizer pose_edges_spacing: 1 # Only register every Nth pose edge - landmark_edges_spacing: 1 # Only register every Nth landmark edge + landmark_edges_spacing: 2 # Only register every Nth landmark edge delta_landmark: 0.3 # Confidence threshold for values from data association - optimize: 10 # Number of optimizations to realize + optimize: 3 # Number of optimizations to realize xy_max_offset: 5.0 # m Max jump of vehicle position in a single optimization yaw_max_offset: 12 # Max angle of the vehicle to correct in a single optimization dist_append_point_to_map: 0.1 # m Max distance to append a cone to one already on the map 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..c2b6f5af 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 @@ -90,7 +90,6 @@ class GraphSlam : public rclcpp::Node int landmark_edges_deactivated_ = 0; rclcpp::Time prev_t_; int pose_edges_counter_ = 1; - int pose_counter_ = 0; int landmark_edges_counter_ = 1; pcl::PointCloud loaded_map_; bool map_stored_ = false; diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index fdaf6ffd..873ee3b6 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -247,8 +247,6 @@ void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg) pose_edges_counter_ = 0; } pose_edges_counter_++; - pose_counter_++; - } @@ -263,8 +261,16 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms std::vector unmatched_landmarks; rclcpp::Time obs_time = msg->header.stamp; + Eigen::Vector3d pose_at_scan; + adjust_perception_delay(obs_time); + if (!vertex_at_scan_time_) { + RCLCPP_WARN(this->get_logger(), "No pose vertex available for the scan timestamp."); + return; + } - // adjust_perception_delay(obs_time); + pose_at_scan(0)=vertex_at_scan_time_->estimate().translation().x(); + pose_at_scan(1)=vertex_at_scan_time_->estimate().translation().y(); + pose_at_scan(2)=vertex_at_scan_time_->estimate().rotation().angle(); pcl::PointCloud::Ptr cloud(new pcl::PointCloud); pcl::fromROSMsg(*msg, *cloud); @@ -318,7 +324,7 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms //Extract the cone position PointXYZProbColorScore cone = cloud->points[i]; - Landmark l = Landmark(Eigen::Vector2d(cone.x, cone.y), vehicle_pose_); + Landmark l = Landmark(Eigen::Vector2d(cone.x, cone.y), pose_at_scan); // Measurement covariance (XY) for this observation: l.covariance_ = min_perception_cov_; @@ -342,8 +348,8 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms if (kDataAssociationMethod == "ICP") DA.data_association_ICP(observed_landmarks, unmatched_landmarks, true, kLidarColoring, kMaxDistToMatch, kMinObsTimeToDisable, kMaxNumObsToDisable); else if (kDataAssociationMethod == "MRPT") DA.data_association_MRPT(observed_landmarks, unmatched_landmarks, kMinDistToMatch, kMinObsTimeToDisable, kMaxNumObsToDisable, kChi2Quantile); - else if (kDataAssociationMethod == "JCBB") DA.data_association_JCBB(vehicle_pose_, observed_landmarks, unmatched_landmarks, kMinObsTimeToDisable, kMaxNumObsToDisable, kChi2Quantile); - else if (kDataAssociationMethod == "KNN") DA.data_association_KNN(vehicle_pose_, observed_landmarks, unmatched_landmarks, kMinObsTimeToDisable, kMaxNumObsToDisable, kMahalanobisThershold); + else if (kDataAssociationMethod == "JCBB") DA.data_association_JCBB(pose_at_scan, observed_landmarks, unmatched_landmarks, kMinObsTimeToDisable, kMaxNumObsToDisable, kChi2Quantile); + else if (kDataAssociationMethod == "KNN") DA.data_association_KNN(pose_at_scan, observed_landmarks, unmatched_landmarks, kMinObsTimeToDisable, kMaxNumObsToDisable, kMahalanobisThershold); else RCLCPP_WARN(this->get_logger(), "Unknown data_association_method='%s'. Falling back to 'mrpt'.", kDataAssociationMethod); last_data_association_time_ = this->now().seconds() - t0; @@ -365,7 +371,7 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms // Add an edge between the last pose vertex and the new landmark vertex g2o::EdgeSE2PointXY* edge = new g2o::EdgeSE2PointXY(); - edge->vertices()[0] = pose_vertices_.back(); + edge->vertices()[0] = vertex_at_scan_time_; edge->vertices()[1] = landmark_vertex; edge->setMeasurement(landmark.local_position_); edge->setInformation(R.inverse()); @@ -386,7 +392,7 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms // Add an edge between the last pose vertex and the matched landmark vertex g2o::VertexPointXY* landmark_vertex = landmark_vertices_[landmark.id_/2]; g2o::EdgeSE2PointXY* edge = new g2o::EdgeSE2PointXY(); - edge->vertices()[0] = pose_vertices_.back(); + edge->vertices()[0] = vertex_at_scan_time_; edge->vertices()[1] = landmark_vertex; edge->setMeasurement(landmark.local_position_); edge->setInformation(R.inverse()); @@ -961,7 +967,7 @@ void GraphSlam::adjust_perception_delay(rclcpp::Time scan_stamp) const double scan_time_sec = scan_stamp.seconds(); - for (int i = static_cast(pose_vertices_.size()) - 1; i >= 0; --i) { + for (int i = static_cast(pose_vertices_.size()) - 1; i >= 0; i--) { if (pose_timestamps_[i].seconds() <= scan_time_sec) { vertex_at_scan_time_ = pose_vertices_[i]; return; @@ -973,7 +979,7 @@ void GraphSlam::adjust_perception_delay(rclcpp::Time scan_stamp) } g2o::VertexSE2* GraphSlam::get_last_valid_pose() { - for (int i = pose_vertices_.size() - 1; i >= 0; --i) { + for (int i = pose_vertices_.size() - 1; i >= 0; i--) { if (pose_vertices_[i]->hessianIndex() >= 0) { return pose_vertices_[i]; } From ffb439ba42507804caf2656f0cf56a7ef224ec26 Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Sun, 15 Mar 2026 21:52:07 +0100 Subject: [PATCH 02/14] feat: add mutex to solve lists dropping to zero and preventing future segmentation faults --- .../include/graph_slam/graph_slam_node.hpp | 6 ++ src/slam/graph_slam/src/graph_slam_node.cpp | 70 +++++++++++++------ 2 files changed, 56 insertions(+), 20 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 c2b6f5af..e735ed18 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 @@ -44,6 +44,8 @@ #include #include +#include +#include class GraphSlam : public rclcpp::Node @@ -81,6 +83,8 @@ class GraphSlam : public rclcpp::Node std::vector pose_vertices_to_add_; // Vertices to add to the optimizer std::vector landmark_vertices_to_add_; // Vertices to add to the optimizer std::vector edges_to_add_; // Edges to add to the optimizer + std::atomic stats_active_vertices_{0}; + std::atomic stats_active_edges_{0}; // Other variables double driven_distance_ = 0.0; @@ -96,6 +100,8 @@ class GraphSlam : public rclcpp::Node common_msgs::msg::Trajectory opt_traj_msg_; double last_optimization_time_ = 0.0; double last_data_association_time_ = 0.0; + std::mutex graph_mutex_; + std::mutex buffer_mutex_; // Parameters std::string kLidarTopic; diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index 873ee3b6..92ab52ab 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -191,6 +191,8 @@ GraphSlam::GraphSlam() : Node("graph_slam") void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg) { + std::lock_guard buffer_lock(buffer_mutex_); + // 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); @@ -253,16 +255,23 @@ 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) { - return; - } + std::lock_guard graph_lock(graph_mutex_); std::vector observed_landmarks; std::vector unmatched_landmarks; rclcpp::Time obs_time = msg->header.stamp; Eigen::Vector3d pose_at_scan; - adjust_perception_delay(obs_time); + + { + std::lock_guard buffer_lock(buffer_mutex_); + if (pose_vertices_.size() == 0) { + return; + } + + adjust_perception_delay(obs_time); + } + if (!vertex_at_scan_time_) { RCLCPP_WARN(this->get_logger(), "No pose vertex available for the scan timestamp."); return; @@ -357,6 +366,8 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms if(!map_fixed_){ + std::lock_guard buffer_lock(buffer_mutex_); + for (auto landmark : unmatched_landmarks) { landmark.id_ = 2*DA.map_.size(); // Use even ids for landmark vertices auto new_landmark = std::make_shared(landmark); // Copy to avoid memory issues @@ -385,6 +396,8 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms if(landmark_edges_counter_ % kLandmarkEdgesSpacing == 0){ + std::lock_guard buffer_lock(buffer_mutex_); + for (auto landmark : observed_landmarks) { if (landmark.id_ == Landmark::UNMATCHED_ID) { continue; @@ -408,8 +421,8 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms // Publish SLAM statistics common_msgs::msg::SlamStats stats_msg; - stats_msg.active_vertices = optimizer_.activeVertices().size(); - stats_msg.active_edges = optimizer_.activeEdges().size(); + stats_msg.active_vertices = stats_active_vertices_.load(); + stats_msg.active_edges = stats_active_edges_.load(); stats_msg.optimization_time = last_optimization_time_; stats_msg.data_association_time = last_data_association_time_; stats_msg.observed_landmarks = observed_landmarks.size(); @@ -422,6 +435,8 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms void GraphSlam::camera_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + std::lock_guard graph_lock(graph_mutex_); + std::vector observed_camera_landmarks; std::vector unmatched_landmarks; @@ -455,26 +470,39 @@ void GraphSlam::camera_callback(const sensor_msgs::msg::PointCloud2::SharedPtr m void GraphSlam::optimizer_callback(){ double t0 = this->now().seconds(); + int current_lap = 0; - update_pose_predictions(); - - fill_graph(); + { + std::lock_guard graph_lock(graph_mutex_); - optimizer_.initializeOptimization(); + { + std::lock_guard buffer_lock(buffer_mutex_); + update_pose_predictions(); + fill_graph(); - optimizer_.optimize(kOptimize); + if (lap_count_ == 1 && !map_fixed_) { + fix_map(); + } + current_lap = lap_count_; + } + + optimizer_.initializeOptimization(); + optimizer_.optimize(kOptimize); + update_data_association_map(); - update_data_association_map(); + stats_active_vertices_.store(optimizer_.activeVertices().size()); + stats_active_edges_.store(optimizer_.activeEdges().size()); + } std_msgs::msg::Int16 lap_count_msg; - lap_count_msg.data = lap_count_; + lap_count_msg.data = current_lap; lap_count_pub_->publish(lap_count_msg); last_optimization_time_ = this->now().seconds() - t0; if(kDebug){ - RCLCPP_INFO(this->get_logger(), "Number of vertices: %d", optimizer_.activeVertices().size()); - RCLCPP_INFO(this->get_logger(), "Number of edges: %d", optimizer_.activeEdges().size()); + RCLCPP_INFO(this->get_logger(), "Number of vertices: %d", stats_active_vertices_.load()); + RCLCPP_INFO(this->get_logger(), "Number of edges: %d", stats_active_edges_.load()); RCLCPP_INFO(this->get_logger(), "Optimization time: %f", last_optimization_time_); } } @@ -641,6 +669,12 @@ void GraphSlam::fix_map(){ void GraphSlam::publish_map(){ + Eigen::Vector3d current_pose; + { + std::lock_guard buffer_lock(buffer_mutex_); + current_pose = vehicle_pose_; + } + std::vector pub_indices; for (int i = 0; i < DA.map_.size(); i++) { pub_indices.push_back(i); @@ -675,7 +709,7 @@ void GraphSlam::publish_map(){ double min_dist = std::numeric_limits::max(); for (int index : indices.indices) { auto point = map_pcl->points[index]; - min_dist = std::min(min_dist, std::pow(point.x - vehicle_pose_(0), 2) + std::pow(point.y - vehicle_pose_(1), 2)); + min_dist = std::min(min_dist, std::pow(point.x - current_pose(0), 2) + std::pow(point.y - current_pose(1), 2)); } if (min_dist < closest_cluster_dist) { pub_indices = indices.indices; @@ -764,10 +798,6 @@ void GraphSlam::check_finish_line(){ driven_distance_ = 0.0; lap_count_++; } - - if (lap_count_ == 1 && !map_fixed_) { - fix_map(); - } } From 3d3823044e863e9f4074947b4bf336205d043bb2 Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Wed, 18 Mar 2026 13:58:45 +0100 Subject: [PATCH 03/14] fix: solve adjust_perception_delay, adjust parameters, remove mutex and increase odometry freq to 200Hz --- src/car_state/src/car_state_node.cpp | 2 +- .../launch/rosbag_simulation_launch.py | 2 +- src/perception/config/perception_config.yaml | 2 +- src/perception/src/perception_node.cpp | 4 +-- .../graph_slam/config/graph_slam_config.yaml | 10 +++---- .../include/graph_slam/graph_slam_node.hpp | 4 +-- src/slam/graph_slam/src/graph_slam_node.cpp | 28 ++++++++++--------- 7 files changed, 27 insertions(+), 25 deletions(-) diff --git a/src/car_state/src/car_state_node.cpp b/src/car_state/src/car_state_node.cpp index af1beffb..9068dffe 100644 --- a/src/car_state/src/car_state_node.cpp +++ b/src/car_state/src/car_state_node.cpp @@ -247,7 +247,7 @@ CarState::CarState(): Node("car_state") // Configure timer once in the constructor based on the selected controller and frequency timer_ = this->create_wall_timer( - std::chrono::milliseconds(static_cast(1000.0 / 100)), + std::chrono::milliseconds(static_cast(1000.0 / 200)), std::bind(&CarState::on_timer, this)); // CPU monitor timer diff --git a/src/common/common_meta/launch/rosbag_simulation_launch.py b/src/common/common_meta/launch/rosbag_simulation_launch.py index e9b76a02..768bc4fe 100644 --- a/src/common/common_meta/launch/rosbag_simulation_launch.py +++ b/src/common/common_meta/launch/rosbag_simulation_launch.py @@ -32,7 +32,7 @@ def generate_launch_description(): 'local_frame': 'slam/vehicle2', 'debug': False}]), create_node(pkg='graph_slam', - params=[{'lidar_topic': '/perception/map', + params=[{'lidar_topic': '/perception/map2', 'map_topic': '/slam/map2', 'car_state_topic': '/car_state/state2', 'global_frame': 'arussim/world2', diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index b7393033..e21bfd58 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -62,7 +62,7 @@ perception: # Reconstruction parameters reconstruction: true - reconstruction_radius: 0.3 + reconstruction_radius: 0.15 # Scoring parameters threshold_scoring: 0.0 diff --git a/src/perception/src/perception_node.cpp b/src/perception/src/perception_node.cpp index 2f6669b4..78a68717 100644 --- a/src/perception/src/perception_node.cpp +++ b/src/perception/src/perception_node.cpp @@ -179,7 +179,7 @@ void Perception::state_callback(const common_msgs::msg::CarInfo::SharedPtr state void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_msg) { - + auto timestamp = this->now(); if (kWaitDriving && as_status_ != 3) return; common_msgs::msg::PerceptionStats stats_msg; @@ -408,7 +408,7 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l sensor_msgs::msg::PointCloud2 map_msg; pcl::toROSMsg(*final_map, map_msg); map_msg.header.frame_id = "/rslidar"; - map_msg.header.stamp = lidar_msg->header.stamp; + map_msg.header.stamp = timestamp; map_pub_->publish(map_msg); // Publish clipping box values for visualization node diff --git a/src/slam/graph_slam/config/graph_slam_config.yaml b/src/slam/graph_slam/config/graph_slam_config.yaml index 2178fb61..87f3941b 100644 --- a/src/slam/graph_slam/config/graph_slam_config.yaml +++ b/src/slam/graph_slam/config/graph_slam_config.yaml @@ -12,18 +12,18 @@ graph_slam: local_frame: "slam/vehicle" set_user_lambda_init: 10.0 # Initial lambda of the optimizer - optimizer_freq: 7.0 # Hz Frequency at which the optimizer works + optimizer_freq: 5.0 # Hz Frequency at which the optimizer works finish_line_offset: 6.0 # m Offset of the finishing line regarding the beginning track_width: 3.0 # m Width of the track min_lap_distance: 30.0 # m Min distance to consider a new lap - max_pose_edges: 7000 # Max number of pose edges given to the optimizer + max_pose_edges: 8000 # Max number of pose edges given to the optimizer level_pose_edges: 2 # Change pose edge to level _ in the optimizer level_landmark_edges: 3 # Change landmark edge to level _ in the optimizer - max_landmark_edges: 6000 # Max number of landmark edges given to the optimizer + max_landmark_edges: 8000 # Max number of landmark edges given to the optimizer pose_edges_spacing: 1 # Only register every Nth pose edge - landmark_edges_spacing: 2 # Only register every Nth landmark edge + landmark_edges_spacing: 1 # Only register every Nth landmark edge delta_landmark: 0.3 # Confidence threshold for values from data association - optimize: 3 # Number of optimizations to realize + optimize: 5 # Number of optimizations to realize xy_max_offset: 5.0 # m Max jump of vehicle position in a single optimization yaw_max_offset: 12 # Max angle of the vehicle to correct in a single optimization dist_append_point_to_map: 0.1 # m Max distance to append a cone to one already on the map 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 e735ed18..7e53567d 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 @@ -66,8 +66,8 @@ class GraphSlam : public rclcpp::Node double x_noise_ = 5.0E-2; double y_noise_ = 5.0E-2; double yaw_noise_ = 5.0E-3; - double perception_noise_ = 0.05; - double dt = 0.01; + double perception_noise_ = 0.10; + double dt = 0.005; g2o::VertexSE2* last_optimized_pose_ = nullptr; std::vector odom_buffer_; diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index 92ab52ab..8471f9cf 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -191,7 +191,7 @@ GraphSlam::GraphSlam() : Node("graph_slam") void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg) { - std::lock_guard buffer_lock(buffer_mutex_); + // Update pose estimation from odometry double phi = vehicle_pose_(2); @@ -255,8 +255,6 @@ void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg) void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - std::lock_guard graph_lock(graph_mutex_); - std::vector observed_landmarks; std::vector unmatched_landmarks; @@ -264,7 +262,7 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms Eigen::Vector3d pose_at_scan; { - std::lock_guard buffer_lock(buffer_mutex_); + if (pose_vertices_.size() == 0) { return; } @@ -366,7 +364,7 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms if(!map_fixed_){ - std::lock_guard buffer_lock(buffer_mutex_); + for (auto landmark : unmatched_landmarks) { landmark.id_ = 2*DA.map_.size(); // Use even ids for landmark vertices @@ -396,7 +394,7 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms if(landmark_edges_counter_ % kLandmarkEdgesSpacing == 0){ - std::lock_guard buffer_lock(buffer_mutex_); + for (auto landmark : observed_landmarks) { if (landmark.id_ == Landmark::UNMATCHED_ID) { @@ -435,8 +433,7 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms void GraphSlam::camera_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - std::lock_guard graph_lock(graph_mutex_); - + std::vector observed_camera_landmarks; std::vector unmatched_landmarks; @@ -473,10 +470,8 @@ void GraphSlam::optimizer_callback(){ int current_lap = 0; { - std::lock_guard graph_lock(graph_mutex_); - - { - std::lock_guard buffer_lock(buffer_mutex_); + { + update_pose_predictions(); fill_graph(); @@ -671,7 +666,7 @@ void GraphSlam::publish_map(){ Eigen::Vector3d current_pose; { - std::lock_guard buffer_lock(buffer_mutex_); + current_pose = vehicle_pose_; } @@ -996,6 +991,13 @@ void GraphSlam::adjust_perception_delay(rclcpp::Time scan_stamp) } const double scan_time_sec = scan_stamp.seconds(); + const double latest_pose_time = pose_timestamps_.back().seconds(); + const double time_diff = latest_pose_time - scan_time_sec; + + // EL CHIVATO: Imprime los tiempos con 6 decimales (microsegundos) + RCLCPP_INFO(this->get_logger(), + "⏱️ [TIMESTAMPS] Lidar: %.6f | Odom: %.6f | Diff: %.6f seg", + scan_time_sec, latest_pose_time, time_diff); for (int i = static_cast(pose_vertices_.size()) - 1; i >= 0; i--) { if (pose_timestamps_[i].seconds() <= scan_time_sec) { From 5908abbd57d0e1e07cad5da999c2eb55696dc6bd Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Wed, 18 Mar 2026 16:43:30 +0100 Subject: [PATCH 04/14] refactor: fix warnings and unnecessary variables and spaces --- .../graph_slam/data_association_ICP_NN.hpp | 14 ++-- src/slam/graph_slam/src/graph_slam_node.cpp | 81 +++++++------------ 2 files changed, 33 insertions(+), 62 deletions(-) diff --git a/src/slam/graph_slam/include/graph_slam/data_association_ICP_NN.hpp b/src/slam/graph_slam/include/graph_slam/data_association_ICP_NN.hpp index 1b588745..0901ddc9 100644 --- a/src/slam/graph_slam/include/graph_slam/data_association_ICP_NN.hpp +++ b/src/slam/graph_slam/include/graph_slam/data_association_ICP_NN.hpp @@ -71,10 +71,8 @@ class DataAssociation{ } // Merge observations that are too close together - int i = 0; + size_t i = 0; while (i < obs_pcl->size() && obs_pcl->size() >= 2) { - pcl::PointXYZ obs_point = obs_pcl->points[i]; - pcl::KdTreeFLANN kdtree; kdtree.setInputCloud(obs_pcl); std::vector indices(2); @@ -114,7 +112,7 @@ class DataAssociation{ icp_.align(*corrected_obs); if (icp_.hasConverged() && icp_.getFitnessScore() < 1){ - for (int i = 0; i < observed_landmarks.size(); i++){ + for (size_t i = 0; i < observed_landmarks.size(); i++){ Landmark& obs = observed_landmarks[i]; obs.world_position_ = Eigen::Vector2d(corrected_obs->points[i].x, corrected_obs->points[i].y); } @@ -126,7 +124,7 @@ class DataAssociation{ pcl::KdTreeFLANN kdtree; kdtree.setInputCloud(map_pcl); - for (int i = 0; i < corrected_obs->size(); i++) { + for (size_t i = 0; i < corrected_obs->size(); i++) { pcl::PointXYZ obs_point = corrected_obs->points[i]; std::vector indices(1); std::vector distances(1); @@ -170,7 +168,7 @@ class DataAssociation{ observed_landmarks.end()); } } - void data_association_JCBB(Eigen::Vector3d vehicle_pose, + void data_association_JCBB(Eigen::Vector3d /*vehicle_pose*/, std::vector& observed_landmarks, std::vector& unmatched_landmarks, double min_obs_time_to_disable = 1.0, double max_num_obs_to_disable = 3, double chi2quantile = 0.999) { @@ -199,7 +197,7 @@ class DataAssociation{ mrpt::math::CMatrixDouble predictions_cov(2 * map_.size(), 2); std::vector predictions_map_ids; int predictions_ctr = 0; - for (int i = 0; i < map_.size(); i++) { + for (size_t i = 0; i < map_.size(); i++) { auto mapped_lm = map_[i]; predictions.block<1, 2>(predictions_ctr, 0) = mapped_lm->world_position_; predictions_cov.block<2, 2>(2 * predictions_ctr, 0) = mapped_lm->covariance_; @@ -409,7 +407,7 @@ class DataAssociation{ ); } - void data_association_KNN(Eigen::Vector3d vehicle_pose, std::vector& observed_landmarks, + void data_association_KNN(Eigen::Vector3d /*vehicle_pose*/, std::vector& observed_landmarks, std::vector& unmatched_landmarks, double min_obs_time_to_disable, double max_num_obs_to_disable, double mahalanobis_threshold) { diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index 8471f9cf..bde3e802 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -191,8 +191,6 @@ GraphSlam::GraphSlam() : Node("graph_slam") 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); @@ -261,15 +259,12 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms rclcpp::Time obs_time = msg->header.stamp; Eigen::Vector3d pose_at_scan; - { - - if (pose_vertices_.size() == 0) { - return; - } - - adjust_perception_delay(obs_time); + if (pose_vertices_.size() == 0) { + return; } + adjust_perception_delay(obs_time); + if (!vertex_at_scan_time_) { RCLCPP_WARN(this->get_logger(), "No pose vertex available for the scan timestamp."); return; @@ -286,7 +281,7 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms if (need_localization_ && !localized_ && loaded_map_.points.size() > 10 && cloud->points.size() > 10) { // Build scan cloud (points in vehicle frame) pcl::PointCloud::Ptr scan_pcl(new pcl::PointCloud); - for (int i = 0; i < msg->width; i++) { + for (unsigned int i = 0; i < msg->width; i++) { PointXYZProbColorScore &cone = cloud->points[i]; scan_pcl->push_back(pcl::PointXYZ(cone.x, cone.y, 0.0)); } @@ -323,11 +318,9 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms } else { RCLCPP_WARN(this->get_logger(), "ICP localization on loaded map did not converge; will retry on next scan."); } - } - - for (int i = 0; i < msg->width; i++) { + for (unsigned int i = 0; i < msg->width; i++) { //Extract the cone position PointXYZProbColorScore cone = cloud->points[i]; @@ -341,15 +334,11 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms l.update_color(YELLOW, cone.prob_yellow); } observed_landmarks.push_back(l); - - - } if (observed_landmarks.size() == 0) { return; } - double t0 = this->now().seconds(); @@ -357,15 +346,12 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms else if (kDataAssociationMethod == "MRPT") DA.data_association_MRPT(observed_landmarks, unmatched_landmarks, kMinDistToMatch, kMinObsTimeToDisable, kMaxNumObsToDisable, kChi2Quantile); else if (kDataAssociationMethod == "JCBB") DA.data_association_JCBB(pose_at_scan, observed_landmarks, unmatched_landmarks, kMinObsTimeToDisable, kMaxNumObsToDisable, kChi2Quantile); else if (kDataAssociationMethod == "KNN") DA.data_association_KNN(pose_at_scan, observed_landmarks, unmatched_landmarks, kMinObsTimeToDisable, kMaxNumObsToDisable, kMahalanobisThershold); - else RCLCPP_WARN(this->get_logger(), "Unknown data_association_method='%s'. Falling back to 'mrpt'.", kDataAssociationMethod); + else RCLCPP_WARN(this->get_logger(), "Unknown data_association_method='%s'. Falling back to 'mrpt'.", kDataAssociationMethod.c_str()); last_data_association_time_ = this->now().seconds() - t0; if (kDebug) RCLCPP_INFO(this->get_logger(), "Data Association time: %f", last_data_association_time_); - if(!map_fixed_){ - - for (auto landmark : unmatched_landmarks) { landmark.id_ = 2*DA.map_.size(); // Use even ids for landmark vertices auto new_landmark = std::make_shared(landmark); // Copy to avoid memory issues @@ -392,10 +378,7 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms } } - if(landmark_edges_counter_ % kLandmarkEdgesSpacing == 0){ - - for (auto landmark : observed_landmarks) { if (landmark.id_ == Landmark::UNMATCHED_ID) { continue; @@ -433,14 +416,13 @@ void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr ms void GraphSlam::camera_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - std::vector observed_camera_landmarks; std::vector unmatched_landmarks; pcl::PointCloud::Ptr cloud(new pcl::PointCloud); pcl::fromROSMsg(*msg, *cloud); - for (int i = 0; i < msg->width; i++) { + for (unsigned int i = 0; i < msg->width; i++) { ConeXYZColorScore cone = cloud->points[i]; Landmark l = Landmark(Eigen::Vector2d(cone.x, cone.y), vehicle_pose_); @@ -458,7 +440,7 @@ void GraphSlam::camera_callback(const sensor_msgs::msg::PointCloud2::SharedPtr m else if (kDataAssociationMethod == "MRPT") DA.data_association_MRPT(observed_camera_landmarks, unmatched_landmarks, kMinDistToMatch, kMinObsTimeToDisable, kMaxNumObsToDisable, kChi2Quantile); else if (kDataAssociationMethod == "JCBB") DA.data_association_JCBB(vehicle_pose_, observed_camera_landmarks, unmatched_landmarks, kMinObsTimeToDisable, kMaxNumObsToDisable, kChi2Quantile); else if (kDataAssociationMethod == "KNN") DA.data_association_KNN(vehicle_pose_, observed_camera_landmarks, unmatched_landmarks, kMinObsTimeToDisable, kMaxNumObsToDisable, kMahalanobisThershold); - else RCLCPP_WARN(this->get_logger(), "Unknown data_association_method='%s'. Falling back to 'mrpt'.", kDataAssociationMethod); + else RCLCPP_WARN(this->get_logger(), "Unknown data_association_method='%s'. Falling back to 'mrpt'.", kDataAssociationMethod.c_str()); last_data_association_time_ = this->now().seconds() - t0; if (kDebug) RCLCPP_INFO(this->get_logger(), "Data Association time: %f", last_data_association_time_); @@ -467,30 +449,18 @@ void GraphSlam::camera_callback(const sensor_msgs::msg::PointCloud2::SharedPtr m void GraphSlam::optimizer_callback(){ double t0 = this->now().seconds(); - int current_lap = 0; - - { - { - - update_pose_predictions(); - fill_graph(); - - if (lap_count_ == 1 && !map_fixed_) { - fix_map(); - } - current_lap = lap_count_; - } + update_pose_predictions(); + fill_graph(); - optimizer_.initializeOptimization(); - optimizer_.optimize(kOptimize); - update_data_association_map(); + optimizer_.initializeOptimization(); + optimizer_.optimize(kOptimize); + update_data_association_map(); - stats_active_vertices_.store(optimizer_.activeVertices().size()); - stats_active_edges_.store(optimizer_.activeEdges().size()); - } + stats_active_vertices_.store(optimizer_.activeVertices().size()); + stats_active_edges_.store(optimizer_.activeEdges().size()); std_msgs::msg::Int16 lap_count_msg; - lap_count_msg.data = current_lap; + lap_count_msg.data = lap_count_; lap_count_pub_->publish(lap_count_msg); last_optimization_time_ = this->now().seconds() - t0; @@ -579,7 +549,6 @@ void GraphSlam::update_pose_predictions(){ int index = last_optimized_pose_->id(); Eigen::Matrix3d B2 = Eigen::Matrix3d::Identity(); - Eigen::Vector3d u2 = Eigen::Vector3d::Zero(); // Re-predict the pose for each odometry measurement in the buffer for (auto u2: odom_buffer_){ @@ -633,8 +602,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 static_cast(kMaxPoseEdges)){ + for (size_t i=pose_edges_deactivated_; igraph()) pose_edges_[i]->setLevel(kLevelPoseEdges); // Deactivate edge } pose_edges_deactivated_ = pose_edges_.size()-kMaxPoseEdges; @@ -642,8 +611,8 @@ void GraphSlam::fill_graph(){ g2o::VertexSE2* v0 = static_cast(pose_edges_[pose_edges_deactivated_]->vertex(0)); v0->setFixed(true); } - if(landmark_edges_.size()-landmark_edges_deactivated_ > kMaxLandmarkEdges){ - for (int i=landmark_edges_deactivated_; i static_cast(kMaxLandmarkEdges)){ + for (size_t i=landmark_edges_deactivated_; igraph()) landmark_edges_[i]->setLevel(kLevelLandmarkEdges); // Deactivate edge } landmark_edges_deactivated_ = landmark_edges_.size()-kMaxLandmarkEdges; @@ -671,7 +640,7 @@ void GraphSlam::publish_map(){ } std::vector pub_indices; - for (int i = 0; i < DA.map_.size(); i++) { + for (size_t i = 0; i < DA.map_.size(); i++) { pub_indices.push_back(i); } @@ -793,6 +762,10 @@ void GraphSlam::check_finish_line(){ driven_distance_ = 0.0; lap_count_++; } + + if (lap_count_ == 1 && !map_fixed_) { + fix_map(); + } } @@ -968,7 +941,7 @@ void GraphSlam::store_trajectory(const common_msgs::msg::Trajectory::SharedPtr m shared_file << "x,y,s,k,speed_profile,acc_profile\n"; // Write trajectory points - for (int i=0; ipoints.size(); i++) { + for (size_t i=0; ipoints.size(); i++) { file << msg->points[i].x << "," << msg->points[i].y << "," << msg->s[i] << "," << msg->k[i] << "," << msg->speed_profile[i] << "," << msg->acc_profile[i] << "\n"; shared_file << msg->points[i].x << "," << msg->points[i].y << "," << msg->s[i] << "," From 8e8983dd73b1f25e87dd35fd7b804192a7b37a64 Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Wed, 18 Mar 2026 17:31:48 +0100 Subject: [PATCH 05/14] refactor: remove unnecesary rclcpp info --- src/slam/graph_slam/src/graph_slam_node.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index 07d6ec93..b002f61a 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -964,13 +964,6 @@ void GraphSlam::adjust_perception_delay(rclcpp::Time scan_stamp) } const double scan_time_sec = scan_stamp.seconds(); - const double latest_pose_time = pose_timestamps_.back().seconds(); - const double time_diff = latest_pose_time - scan_time_sec; - - // EL CHIVATO: Imprime los tiempos con 6 decimales (microsegundos) - RCLCPP_INFO(this->get_logger(), - "⏱️ [TIMESTAMPS] Lidar: %.6f | Odom: %.6f | Diff: %.6f seg", - scan_time_sec, latest_pose_time, time_diff); for (int i = static_cast(pose_vertices_.size()) - 1; i >= 0; i--) { if (pose_timestamps_[i].seconds() <= scan_time_sec) { From bc398ec210b41529259b075b9ffd3972659bc204 Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Fri, 20 Mar 2026 11:19:33 +0100 Subject: [PATCH 06/14] fix: solve all comments and an error in list iterations in adjust_perception_delay() --- src/car_state/config/car_state_config.yaml | 1 + src/car_state/include/car_state/car_state_node.hpp | 1 + src/car_state/src/car_state_node.cpp | 4 +++- src/perception/config/perception_config.yaml | 1 + .../include/perception/perception_node.hpp | 2 ++ src/perception/src/perception_node.cpp | 12 ++++++++++-- src/slam/graph_slam/config/graph_slam_config.yaml | 4 ---- .../include/graph_slam/data_association_ICP_NN.hpp | 4 ++-- src/slam/graph_slam/src/graph_slam_node.cpp | 4 ++-- 9 files changed, 22 insertions(+), 11 deletions(-) diff --git a/src/car_state/config/car_state_config.yaml b/src/car_state/config/car_state_config.yaml index 9212733f..e89985c6 100644 --- a/src/car_state/config/car_state_config.yaml +++ b/src/car_state/config/car_state_config.yaml @@ -36,6 +36,7 @@ car_state: lf: 0.84315 tf: 1.22 tr: 1.22 + state_frequency: 200.0 # Error weights error_weight_imu: 2.0 diff --git a/src/car_state/include/car_state/car_state_node.hpp b/src/car_state/include/car_state/car_state_node.hpp index 26181838..a5d3786d 100644 --- a/src/car_state/include/car_state/car_state_node.hpp +++ b/src/car_state/include/car_state/car_state_node.hpp @@ -119,6 +119,7 @@ class CarState : public rclcpp::Node double kLf; double kTf; double kTr; + double kStateFrequency; double kErrorWeightIMU; double kErrorWeightExtensometer; diff --git a/src/car_state/src/car_state_node.cpp b/src/car_state/src/car_state_node.cpp index d99699f4..72251dc6 100644 --- a/src/car_state/src/car_state_node.cpp +++ b/src/car_state/src/car_state_node.cpp @@ -123,10 +123,12 @@ CarState::CarState(): Node("car_state") this->declare_parameter("lf", 0.84315); this->declare_parameter("tf", 1.22); this->declare_parameter("tr", 1.22); + this->declare_parameter("state_frequency", 200.0); this->get_parameter("lf", kLf); this->get_parameter("tf", kTf); this->get_parameter("tr", kTr); + this->get_parameter("state_frequency", kStateFrequency); // Error weights this->declare_parameter("error_weight_imu", 1.0); @@ -247,7 +249,7 @@ CarState::CarState(): Node("car_state") // Configure timer once in the constructor based on the selected controller and frequency timer_ = this->create_wall_timer( - std::chrono::milliseconds(static_cast(1000.0 / 200)), + std::chrono::milliseconds(static_cast(1000.0 / kStateFrequency)), std::bind(&CarState::on_timer, this)); // CPU monitor timer diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index e4c4cf0c..7cc03055 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -15,6 +15,7 @@ perception: clipping_box_values_topic: "/perception/clipping_box_values" wait_driving: false # Wait for AS_DRIVING status before processing + use_artificial_timestamp: true # Use the node's current time as timestamp instead of the one from the LiDAR messages # Cropping parameters crop: false diff --git a/src/perception/include/perception/perception_node.hpp b/src/perception/include/perception/perception_node.hpp index 3697e5bd..4888d2db 100644 --- a/src/perception/include/perception/perception_node.hpp +++ b/src/perception/include/perception/perception_node.hpp @@ -60,6 +60,7 @@ class Perception : public rclcpp::Node double y_; int as_status_=0; double dt; + rclcpp::Time timestamp_; std::vector final_times; std::vector::Ptr> cloud_buffer_; pcl::PointCloud::Ptr prev_cones_; @@ -151,6 +152,7 @@ class Perception : public rclcpp::Node bool kDebug; bool kDebugClouds; + bool kUseArtificialTimestamp; // Subscribers diff --git a/src/perception/src/perception_node.cpp b/src/perception/src/perception_node.cpp index 78a68717..18480b1e 100644 --- a/src/perception/src/perception_node.cpp +++ b/src/perception/src/perception_node.cpp @@ -67,6 +67,7 @@ Perception::Perception() : Node("Perception") this->declare_parameter("coloring_threshold", 15.0); this->declare_parameter("debug", true); this->declare_parameter("debug_clouds", true); + this->declare_parameter("use_artificial_timestamp", true); // Get the parameters this->get_parameter("lidar_topic", kLidarTopic); @@ -123,6 +124,7 @@ Perception::Perception() : Node("Perception") this->get_parameter("coloring_threshold", kColoringThreshold); this->get_parameter("debug", kDebug); this->get_parameter("debug_clouds", kDebugClouds); + this->get_parameter("use_artificial_timestamp", kUseArtificialTimestamp); // Initialize the variables vx_ = 0.0; @@ -179,7 +181,9 @@ void Perception::state_callback(const common_msgs::msg::CarInfo::SharedPtr state void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_msg) { - auto timestamp = this->now(); + if (kUseArtificialTimestamp) { + auto timestamp_ = this->now(); + } if (kWaitDriving && as_status_ != 3) return; common_msgs::msg::PerceptionStats stats_msg; @@ -408,7 +412,11 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l sensor_msgs::msg::PointCloud2 map_msg; pcl::toROSMsg(*final_map, map_msg); map_msg.header.frame_id = "/rslidar"; - map_msg.header.stamp = timestamp; + if (kUseArtificialTimestamp) { + map_msg.header.stamp = timestamp_; + } else { + map_msg.header.stamp = lidar_msg->header.stamp; + } map_pub_->publish(map_msg); // Publish clipping box values for visualization node diff --git a/src/slam/graph_slam/config/graph_slam_config.yaml b/src/slam/graph_slam/config/graph_slam_config.yaml index 70e08891..1b370c61 100644 --- a/src/slam/graph_slam/config/graph_slam_config.yaml +++ b/src/slam/graph_slam/config/graph_slam_config.yaml @@ -12,11 +12,7 @@ graph_slam: local_frame: "slam/vehicle" set_user_lambda_init: 10.0 # Initial lambda of the optimizer -<<<<<<< feature/slam/fix_localization_errors - optimizer_freq: 5.0 # Hz Frequency at which the optimizer works -======= optimizer_freq: 5.0 # Hz Frequency at which the optimizer works ->>>>>>> develop finish_line_offset: 6.0 # m Offset of the finishing line regarding the beginning track_width: 3.0 # m Width of the track min_lap_distance: 30.0 # m Min distance to consider a new lap diff --git a/src/slam/graph_slam/include/graph_slam/data_association_ICP_NN.hpp b/src/slam/graph_slam/include/graph_slam/data_association_ICP_NN.hpp index 0901ddc9..fbe5abc8 100644 --- a/src/slam/graph_slam/include/graph_slam/data_association_ICP_NN.hpp +++ b/src/slam/graph_slam/include/graph_slam/data_association_ICP_NN.hpp @@ -168,7 +168,7 @@ class DataAssociation{ observed_landmarks.end()); } } - void data_association_JCBB(Eigen::Vector3d /*vehicle_pose*/, + void data_association_JCBB(Eigen::Vector3d vehicle_pose, std::vector& observed_landmarks, std::vector& unmatched_landmarks, double min_obs_time_to_disable = 1.0, double max_num_obs_to_disable = 3, double chi2quantile = 0.999) { @@ -407,7 +407,7 @@ class DataAssociation{ ); } - void data_association_KNN(Eigen::Vector3d /*vehicle_pose*/, std::vector& observed_landmarks, + void data_association_KNN(Eigen::Vector3d vehicle_pose, std::vector& observed_landmarks, std::vector& unmatched_landmarks, double min_obs_time_to_disable, double max_num_obs_to_disable, double mahalanobis_threshold) { diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index b002f61a..b5c556ce 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -965,9 +965,9 @@ void GraphSlam::adjust_perception_delay(rclcpp::Time scan_stamp) const double scan_time_sec = scan_stamp.seconds(); - for (int i = static_cast(pose_vertices_.size()) - 1; i >= 0; i--) { + for (int i = static_cast(pose_timestamps_.size()) - 1; i >= 0; i--) { if (pose_timestamps_[i].seconds() <= scan_time_sec) { - vertex_at_scan_time_ = pose_vertices_[i]; + vertex_at_scan_time_ = pose_vertices_[i+1]; return; } } From 70b194a45e4a554d4f5ec300688dcfcc1712c045 Mon Sep 17 00:00:00 2001 From: root Date: Fri, 20 Mar 2026 16:16:44 +0000 Subject: [PATCH 07/14] feat: make SLAM great again --- src/common/common_meta/launch/rosbag_simulation_launch.py | 3 ++- src/perception/config/perception_config.yaml | 4 ++-- src/perception/src/perception_node.cpp | 2 +- src/slam/graph_slam/src/graph_slam_node.cpp | 1 + 4 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/common/common_meta/launch/rosbag_simulation_launch.py b/src/common/common_meta/launch/rosbag_simulation_launch.py index 768bc4fe..d52c1016 100644 --- a/src/common/common_meta/launch/rosbag_simulation_launch.py +++ b/src/common/common_meta/launch/rosbag_simulation_launch.py @@ -13,7 +13,8 @@ def generate_launch_description(): 'perception_topic': '/perception/map2', 'accum_cloud_topic': '/perception/accumulated_cloud2', 'filtered_cloud_topic': '/perception/filtered_cloud2', - 'clusters_cloud_topic': '/perception/clusters_cloud2'}]), + 'clusters_cloud_topic': '/perception/clusters_cloud2', + 'use_artificial_timestamp': True}]), create_node(pkg='path_planning', params=[{'map_topic': '/slam/map2', 'car_info_topic': '/car_state/car_info2', diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index 06a5f459..d019b18a 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -15,8 +15,8 @@ perception: clipping_box_values_topic: "/perception/clipping_box_values" wait_driving: false # Wait for AS_DRIVING status before processing - use_artificial_timestamp: true # Use the node's current time as timestamp instead of the one from the LiDAR messages - deskew: false # Motion compensation within each LiDAR scan using point timestamps + use_artificial_timestamp: false # Use the node's current time as timestamp instead of the one from the LiDAR messages + deskew: true # Motion compensation within each LiDAR scan using point timestamps # Cropping parameters crop: false diff --git a/src/perception/src/perception_node.cpp b/src/perception/src/perception_node.cpp index 1f36b03b..2c141c33 100644 --- a/src/perception/src/perception_node.cpp +++ b/src/perception/src/perception_node.cpp @@ -184,7 +184,7 @@ void Perception::state_callback(const common_msgs::msg::CarInfo::SharedPtr state void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_msg) { if (kUseArtificialTimestamp) { - auto timestamp_ = this->now(); + timestamp_ = this->now(); } if (kWaitDriving && as_status_ != 3) return; diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index b5c556ce..4fa19b1d 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -967,6 +967,7 @@ void GraphSlam::adjust_perception_delay(rclcpp::Time scan_stamp) for (int i = static_cast(pose_timestamps_.size()) - 1; i >= 0; i--) { if (pose_timestamps_[i].seconds() <= scan_time_sec) { + if (kDebug) RCLCPP_INFO(this->get_logger(), "[TIMESTAMPS] Lidar: %.6f | Odom: %.6f | Diff: %.6f seg", scan_time_sec, pose_timestamps_[i].seconds(), scan_time_sec - pose_timestamps_[i].seconds()); vertex_at_scan_time_ = pose_vertices_[i+1]; return; } From 0169f138cf887058c6f7b83d1910a9df20d79519 Mon Sep 17 00:00:00 2001 From: root Date: Sat, 21 Mar 2026 10:16:51 +0000 Subject: [PATCH 08/14] feat: adjust some params --- src/perception/config/perception_config.yaml | 4 ++-- src/perception/src/perception_node.cpp | 10 +++++----- src/slam/graph_slam/config/graph_slam_config.yaml | 10 +++++----- .../graph_slam/include/graph_slam/graph_slam_node.hpp | 2 +- 4 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index d019b18a..c93416b1 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -15,7 +15,7 @@ perception: clipping_box_values_topic: "/perception/clipping_box_values" wait_driving: false # Wait for AS_DRIVING status before processing - use_artificial_timestamp: false # Use the node's current time as timestamp instead of the one from the LiDAR messages + use_artificial_timestamp: true # Use the node's current time as timestamp instead of the one from the LiDAR messages deskew: true # Motion compensation within each LiDAR scan using point timestamps # Cropping parameters @@ -26,7 +26,7 @@ perception: slow_straight_cropping: false # Leave only a straight section in front of the car when moving slowly # Clipping parameters - clipping: false # Enable clipping of the point cloud + clipping: true # Enable clipping of the point cloud max_x_clip: 50.0 max_y_clip: 5.0 min_x_clip: -30.0 diff --git a/src/perception/src/perception_node.cpp b/src/perception/src/perception_node.cpp index 2c141c33..9d61caac 100644 --- a/src/perception/src/perception_node.cpp +++ b/src/perception/src/perception_node.cpp @@ -184,7 +184,7 @@ void Perception::state_callback(const common_msgs::msg::CarInfo::SharedPtr state void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_msg) { if (kUseArtificialTimestamp) { - timestamp_ = this->now(); + timestamp_ = this->now() - rclcpp::Duration::from_seconds(0.1); } if (kWaitDriving && as_status_ != 3) return; @@ -377,7 +377,7 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l // Publish the filtered cloud sensor_msgs::msg::PointCloud2 filtered_msg; pcl::toROSMsg(*cloud_filtered, filtered_msg); - filtered_msg.header.frame_id = "/rslidar"; + filtered_msg.header.frame_id = "slam/vehicle2"; filtered_pub_->publish(filtered_msg); // Publish ground cloud (only if not empty) @@ -387,7 +387,7 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l ground_cloud->is_dense = true; sensor_msgs::msg::PointCloud2 ground_msg; pcl::toROSMsg(*ground_cloud, ground_msg); - ground_msg.header.frame_id = "/rslidar"; + ground_msg.header.frame_id = "slam/vehicle2"; ground_cloud_pub_->publish(ground_msg); } @@ -411,7 +411,7 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l for (const auto& c : cluster_clouds) *all_clusters_cloud += *c; sensor_msgs::msg::PointCloud2 clusters_msg; pcl::toROSMsg(*all_clusters_cloud, clusters_msg); - clusters_msg.header.frame_id = "/rslidar"; + clusters_msg.header.frame_id = "slam/vehicle2"; clusters_pub_->publish(clusters_msg); } @@ -419,7 +419,7 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l // Publish the map cloud sensor_msgs::msg::PointCloud2 map_msg; pcl::toROSMsg(*final_map, map_msg); - map_msg.header.frame_id = "/rslidar"; + map_msg.header.frame_id = "slam/vehicle2"; if (kUseArtificialTimestamp) { map_msg.header.stamp = timestamp_; } else { diff --git a/src/slam/graph_slam/config/graph_slam_config.yaml b/src/slam/graph_slam/config/graph_slam_config.yaml index 1b370c61..27c60abb 100644 --- a/src/slam/graph_slam/config/graph_slam_config.yaml +++ b/src/slam/graph_slam/config/graph_slam_config.yaml @@ -12,13 +12,13 @@ graph_slam: local_frame: "slam/vehicle" set_user_lambda_init: 10.0 # Initial lambda of the optimizer - optimizer_freq: 5.0 # Hz Frequency at which the optimizer works + optimizer_freq: 10.0 # Hz Frequency at which the optimizer works finish_line_offset: 6.0 # m Offset of the finishing line regarding the beginning track_width: 3.0 # m Width of the track min_lap_distance: 30.0 # m Min distance to consider a new lap max_pose_edges: 8000 # Max number of pose edges given to the optimizer level_pose_edges: 2 # Change pose edge to level _ in the optimizer - level_landmark_edges: 3 # Change landmark edge to level _ in the optimizer + level_landmark_edges: 2 # Change landmark edge to level _ in the optimizer max_landmark_edges: 8000 # Max number of landmark edges given to the optimizer pose_edges_spacing: 1 # Only register every Nth pose edge landmark_edges_spacing: 1 # Only register every Nth landmark edge @@ -27,16 +27,16 @@ graph_slam: xy_max_offset: 5.0 # m Max jump of vehicle position in a single optimization yaw_max_offset: 12 # Max angle of the vehicle to correct in a single optimization dist_append_point_to_map: 0.1 # m Max distance to append a cone to one already on the map - min_landmark_covariance: 0.30 # Min covariance of a landmark + min_landmark_covariance: 0.15 # Min covariance of a landmark # Data association data_association_method: "MRPT" # "MRPT" or "ICP" mahalanobis_thershold: 15.0 # m (For KNN) Max mahalanobis to associate points in data association max_dist_to_match: 5.0 # m (For ICP) Max distance to associate points in data association min_dist_to_match: 1.0 # m (For MRPT and KNN) Min distance to unify points in data association - chi2quantile: 0.9999 # (For MRPT and JCBB) Chi-square confidence quantile used for Mahalanobis gating + chi2quantile: 0.995 # (For MRPT and JCBB) Chi-square confidence quantile used for Mahalanobis gating min_obs_time_to_disable: 1.0 # s Min time whithout seing a cone to disable it - max_num_obs_to_disable: 3 # Max number of times seing a cone to disable it + max_num_obs_to_disable: 10 # Max number of times seing a cone to disable it # Coloring parameters lidar_coloring: false # Activate lidar coloring 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 7e53567d..3da7b9fa 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 @@ -66,7 +66,7 @@ class GraphSlam : public rclcpp::Node double x_noise_ = 5.0E-2; double y_noise_ = 5.0E-2; double yaw_noise_ = 5.0E-3; - double perception_noise_ = 0.10; + double perception_noise_ = 0.05; double dt = 0.005; g2o::VertexSE2* last_optimized_pose_ = nullptr; std::vector odom_buffer_; From f70aca2910a447c36647236f63900e4c66b5fd6c Mon Sep 17 00:00:00 2001 From: root Date: Sat, 21 Mar 2026 10:18:40 +0000 Subject: [PATCH 09/14] fix: change params for testing --- src/perception/config/perception_config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index c93416b1..d019b18a 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -15,7 +15,7 @@ perception: clipping_box_values_topic: "/perception/clipping_box_values" wait_driving: false # Wait for AS_DRIVING status before processing - use_artificial_timestamp: true # Use the node's current time as timestamp instead of the one from the LiDAR messages + use_artificial_timestamp: false # Use the node's current time as timestamp instead of the one from the LiDAR messages deskew: true # Motion compensation within each LiDAR scan using point timestamps # Cropping parameters @@ -26,7 +26,7 @@ perception: slow_straight_cropping: false # Leave only a straight section in front of the car when moving slowly # Clipping parameters - clipping: true # Enable clipping of the point cloud + clipping: false # Enable clipping of the point cloud max_x_clip: 50.0 max_y_clip: 5.0 min_x_clip: -30.0 From 6cd276c73b9c728eefd1e4c8da214faf315c64d6 Mon Sep 17 00:00:00 2001 From: root Date: Sat, 21 Mar 2026 21:42:39 +0000 Subject: [PATCH 10/14] feat: use the nearest state in deskewing --- .../launch/rosbag_simulation_launch.py | 4 ++ .../common_msgs/msg/PerceptionStats.msg | 2 +- src/perception/config/perception_config.yaml | 7 ++- .../include/perception/perception_node.hpp | 13 ++++-- src/perception/include/perception/utils.h | 26 ++++++++++- src/perception/src/perception_node.cpp | 46 ++++++++++--------- 6 files changed, 69 insertions(+), 29 deletions(-) diff --git a/src/common/common_meta/launch/rosbag_simulation_launch.py b/src/common/common_meta/launch/rosbag_simulation_launch.py index d52c1016..9a6c85ba 100644 --- a/src/common/common_meta/launch/rosbag_simulation_launch.py +++ b/src/common/common_meta/launch/rosbag_simulation_launch.py @@ -14,6 +14,7 @@ def generate_launch_description(): 'accum_cloud_topic': '/perception/accumulated_cloud2', 'filtered_cloud_topic': '/perception/filtered_cloud2', 'clusters_cloud_topic': '/perception/clusters_cloud2', + 'stats_topic': '/perception/stats2', 'use_artificial_timestamp': True}]), create_node(pkg='path_planning', params=[{'map_topic': '/slam/map2', @@ -32,10 +33,13 @@ def generate_launch_description(): 'global_frame': 'arussim/world2', 'local_frame': 'slam/vehicle2', 'debug': False}]), + create_node(pkg='pc_supervisor', + params=[{'node_prefix': "/pcart2"}]), create_node(pkg='graph_slam', params=[{'lidar_topic': '/perception/map2', 'map_topic': '/slam/map2', 'car_state_topic': '/car_state/state2', + 'slam_stats_topic': '/slam/stats2', 'global_frame': 'arussim/world2', 'local_frame': 'slam/vehicle2', 'finish_line_offset': 3.0, diff --git a/src/common/common_msgs/msg/PerceptionStats.msg b/src/common/common_msgs/msg/PerceptionStats.msg index fa2268b5..4d39cbff 100644 --- a/src/common/common_msgs/msg/PerceptionStats.msg +++ b/src/common/common_msgs/msg/PerceptionStats.msg @@ -1,5 +1,5 @@ float64 clipping_time -float64 deskew_time +float64 deskewing_time float64 cropping_time float64 slow_cropping_time float64 ground_filter_time diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index d019b18a..45bcb91f 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -11,12 +11,15 @@ perception: max_dist_cone_topic: "/perception/max_dist_cone" final_map_size_topic: "/perception/final_map_size" ground_cloud_topic: "/perception/ground_cloud" - clipping_time_topic: "/perception/stats" + stats_topic: "/perception/stats" clipping_box_values_topic: "/perception/clipping_box_values" wait_driving: false # Wait for AS_DRIVING status before processing use_artificial_timestamp: false # Use the node's current time as timestamp instead of the one from the LiDAR messages - deskew: true # Motion compensation within each LiDAR scan using point timestamps + + # Deskewing + deskewing: true # Motion compensation within each LiDAR scan using point timestamps + buffer_size: 100 # Cropping parameters crop: false diff --git a/src/perception/include/perception/perception_node.hpp b/src/perception/include/perception/perception_node.hpp index a2d92afc..5f3c17f5 100644 --- a/src/perception/include/perception/perception_node.hpp +++ b/src/perception/include/perception/perception_node.hpp @@ -58,9 +58,9 @@ class Perception : public rclcpp::Node double yaw_; double x_; double y_; - int as_status_=0; + int as_status_ = 0; double dt; - rclcpp::Time timestamp_; + rclcpp::Time lidar_timestamp_; std::vector final_times; std::vector::Ptr> cloud_buffer_; pcl::PointCloud::Ptr prev_cones_; @@ -91,7 +91,14 @@ class Perception : public rclcpp::Node std::string kClippingBoxValuesTopic; bool kWaitDriving; - bool kDeskew; + + // Deskewing + bool kDeskewing; + int kBufferSize; + std::vector pose_timestamps_; + std::vector vx_buffer_; + std::vector vy_buffer_; + std::vector r_buffer_; // Cropping bool kCrop; diff --git a/src/perception/include/perception/utils.h b/src/perception/include/perception/utils.h index 7437bfee..241d138b 100644 --- a/src/perception/include/perception/utils.h +++ b/src/perception/include/perception/utils.h @@ -200,23 +200,45 @@ namespace Utils /** * @brief Correct the motion of the points in the final map. */ - void motion_correction(pcl::PointCloud::Ptr cloud, double vx, double vy, double yaw_rate) - { + void motion_correction(pcl::PointCloud::Ptr cloud, std::vector vx_buffer, std::vector vy_buffer, + std::vector r_buffer, std::vector pose_timestamps, rclcpp::Time lidar_timestamp) { if (!cloud || cloud->empty()) { return; } + size_t state_idx = 0; + + const double lidar_timestamp_sec = lidar_timestamp.seconds(); + + for (int i = static_cast(pose_timestamps.size()) - 1; i >= 0; i--) { + if (pose_timestamps[i].seconds() <= lidar_timestamp_sec) { + state_idx = i; + break; + } + } + double reference_timestamp = cloud->points.front().timestamp; for (const auto& point : cloud->points) { reference_timestamp = std::max(reference_timestamp, point.timestamp); } + double state_dt; + if (pose_timestamps.size() <= 2) state_dt = 0.005; + else state_dt = pose_timestamps.back().seconds() - pose_timestamps[pose_timestamps.size() - 2].seconds(); + for (auto& point : cloud->points) { const double dt = reference_timestamp - point.timestamp; if (dt <= 0.0) { continue; } + const size_t offset = static_cast(dt / state_dt); + const size_t idx = std::min(state_idx + offset, vx_buffer.size() - 1); + + const double vx = vx_buffer[idx]; + const double vy = vy_buffer[idx]; + const double yaw_rate = r_buffer[idx]; + const double theta = -yaw_rate * dt; const double cos_theta = std::cos(theta); const double sin_theta = std::sin(theta); diff --git a/src/perception/src/perception_node.cpp b/src/perception/src/perception_node.cpp index 9d61caac..ccefb65d 100644 --- a/src/perception/src/perception_node.cpp +++ b/src/perception/src/perception_node.cpp @@ -25,7 +25,8 @@ Perception::Perception() : Node("Perception") this->declare_parameter("stats_topic", "/perception/stats"); this->declare_parameter("clipping_box_values_topic", "/perception/clipping_box_values"); this->declare_parameter("wait_driving", false); - this->declare_parameter("deskew", true); + this->declare_parameter("deskewing", true); + this->declare_parameter("buffer_size", 100); this->declare_parameter("crop", true); this->declare_parameter("clipping", true); this->declare_parameter("min_x_clip", -30.0); @@ -83,7 +84,8 @@ Perception::Perception() : Node("Perception") this->get_parameter("stats_topic", kStatsTopic); this->get_parameter("clipping_box_values_topic", kClippingBoxValuesTopic); this->get_parameter("wait_driving", kWaitDriving); - this->get_parameter("deskew", kDeskew); + this->get_parameter("deskewing", kDeskewing); + this->get_parameter("buffer_size", kBufferSize); this->get_parameter("crop", kCrop); this->get_parameter("min_x_clip", kMinXClip); this->get_parameter("min_y_clip", kMinYClip); @@ -178,14 +180,25 @@ void Perception::state_callback(const common_msgs::msg::CarInfo::SharedPtr state yaw_ = state_msg->yaw; x_ = state_msg->x; y_ = state_msg->y; + + pose_timestamps_.push_back(state_msg->header.stamp); + vx_buffer_.push_back(state_msg->vx); + vy_buffer_.push_back(state_msg->vy); + r_buffer_.push_back(state_msg->r); + + if (pose_timestamps_.size() > kBufferSize) { + pose_timestamps_.erase(pose_timestamps_.begin()); + vx_buffer_.erase(vx_buffer_.begin()); + vy_buffer_.erase(vy_buffer_.begin()); + r_buffer_.erase(r_buffer_.begin()); + } } void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr lidar_msg) { - if (kUseArtificialTimestamp) { - timestamp_ = this->now() - rclcpp::Duration::from_seconds(0.1); - } + if (kUseArtificialTimestamp) lidar_timestamp_ = this->now(); + else lidar_timestamp_ = lidar_msg->header.stamp; if (kWaitDriving && as_status_ != 3) return; common_msgs::msg::PerceptionStats stats_msg; @@ -195,10 +208,12 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l pcl::PointCloud::Ptr cloud(new pcl::PointCloud); pcl::fromROSMsg(*lidar_msg, *cloud); - if (kDeskew) { - double deskew_start = this->now().seconds(); - Utils::motion_correction(cloud, vx_, vy_, r_); - stats_msg.deskew_time = this->now().seconds() - deskew_start; + if (kDeskewing) { + double deskewing_start = this->now().seconds(); + Utils::motion_correction(cloud, vx_buffer_, vy_buffer_, r_buffer_, pose_timestamps_, lidar_timestamp_); + double deskewing_time = this->now().seconds() - deskewing_start; + stats_msg.deskewing_time = deskewing_time; + if (kDebug) RCLCPP_INFO(this->get_logger(), "Clipping Time: %f", deskewing_time); } pcl::CropBox crop_box; @@ -355,13 +370,6 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l } - // Motion correction - // double dt = this->now().seconds() - start_time; // Estimate SDK delay - // Remove or adapt this if Utils::motion_correction expects a different point type - // Utils::motion_correction(final_map, vx_, vy_, r_, dt); - // if (kDebug) RCLCPP_INFO(this->get_logger(), "Motion correction time: %f", this->now().seconds() - start_time); - - final_times.push_back(this->now().seconds() - start_time); double average_time = std::accumulate(final_times.begin(), final_times.end(), 0.0) / final_times.size(); stats_msg.number_cones = final_map->size(); @@ -420,11 +428,7 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l sensor_msgs::msg::PointCloud2 map_msg; pcl::toROSMsg(*final_map, map_msg); map_msg.header.frame_id = "slam/vehicle2"; - if (kUseArtificialTimestamp) { - map_msg.header.stamp = timestamp_; - } else { - map_msg.header.stamp = lidar_msg->header.stamp; - } + map_msg.header.stamp = lidar_timestamp_; map_pub_->publish(map_msg); // Publish clipping box values for visualization node From 0356384be7b3038d232e776af29c254d34a9ac0c Mon Sep 17 00:00:00 2001 From: root Date: Sat, 21 Mar 2026 22:13:20 +0000 Subject: [PATCH 11/14] feat: adapt pc_supervisor for debugging --- src/common/common_meta/launch/rosbag_simulation_launch.py | 3 ++- src/pc_supervisor/config/pc_supervisor_config.yaml | 3 ++- src/pc_supervisor/include/pc_supervisor/pc_supervisor_node.hpp | 1 + src/pc_supervisor/src/pc_supervisor_node.cpp | 3 ++- 4 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/common/common_meta/launch/rosbag_simulation_launch.py b/src/common/common_meta/launch/rosbag_simulation_launch.py index 9a6c85ba..2c9396ac 100644 --- a/src/common/common_meta/launch/rosbag_simulation_launch.py +++ b/src/common/common_meta/launch/rosbag_simulation_launch.py @@ -34,7 +34,8 @@ def generate_launch_description(): 'local_frame': 'slam/vehicle2', 'debug': False}]), create_node(pkg='pc_supervisor', - params=[{'node_prefix': "/pcart2"}]), + params=[{'node_prefix': "/pcart2", + 'record': False}]), create_node(pkg='graph_slam', params=[{'lidar_topic': '/perception/map2', 'map_topic': '/slam/map2', diff --git a/src/pc_supervisor/config/pc_supervisor_config.yaml b/src/pc_supervisor/config/pc_supervisor_config.yaml index 1f5a1a0e..bfddda62 100644 --- a/src/pc_supervisor/config/pc_supervisor_config.yaml +++ b/src/pc_supervisor/config/pc_supervisor_config.yaml @@ -7,4 +7,5 @@ pc_supervisor: discovery_time: 2000 monitor_time: 500 record_pcap: false - ignore_rslidar: false \ No newline at end of file + ignore_rslidar: false + record: true \ No newline at end of file diff --git a/src/pc_supervisor/include/pc_supervisor/pc_supervisor_node.hpp b/src/pc_supervisor/include/pc_supervisor/pc_supervisor_node.hpp index 78be62fc..3a22542c 100644 --- a/src/pc_supervisor/include/pc_supervisor/pc_supervisor_node.hpp +++ b/src/pc_supervisor/include/pc_supervisor/pc_supervisor_node.hpp @@ -99,6 +99,7 @@ class PcSupervisor : public rclcpp::Node int kMonitorTime; bool kRecordPcap; bool kIgnoreRSLiDAR; + bool kRecord; // Methods /** diff --git a/src/pc_supervisor/src/pc_supervisor_node.cpp b/src/pc_supervisor/src/pc_supervisor_node.cpp index 5fe3ecd9..5555fc97 100644 --- a/src/pc_supervisor/src/pc_supervisor_node.cpp +++ b/src/pc_supervisor/src/pc_supervisor_node.cpp @@ -22,6 +22,7 @@ PcSupervisor::PcSupervisor() : Node("pc_supervisor") { kMonitorTime = this->declare_parameter("monitor_time", 500); kRecordPcap = this->declare_parameter("record_pcap", true); kIgnoreRSLiDAR = this->declare_parameter("ignore_rslidar", false); + kRecord = this->declare_parameter("record", true); // Topics std::string cpu_total_topic = kNodePrefix + "/cpu_total"; @@ -247,7 +248,7 @@ void PcSupervisor::generic_topic_callback(std::shared_ptr(msg->data); - bool should_record = (as_status >= 1 && as_status <= 3); + bool should_record = ((as_status >= 1 && as_status <= 3) && kRecord); std::lock_guard lock(bag_mutex_); From da43267878ebafb7d90aeb17d10346e18de6adea Mon Sep 17 00:00:00 2001 From: root Date: Sat, 21 Mar 2026 22:14:02 +0000 Subject: [PATCH 12/14] fix: increase landmark edge spacing --- src/slam/graph_slam/config/graph_slam_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/slam/graph_slam/config/graph_slam_config.yaml b/src/slam/graph_slam/config/graph_slam_config.yaml index 27c60abb..794f73ee 100644 --- a/src/slam/graph_slam/config/graph_slam_config.yaml +++ b/src/slam/graph_slam/config/graph_slam_config.yaml @@ -21,7 +21,7 @@ graph_slam: level_landmark_edges: 2 # Change landmark edge to level _ in the optimizer max_landmark_edges: 8000 # Max number of landmark edges given to the optimizer pose_edges_spacing: 1 # Only register every Nth pose edge - landmark_edges_spacing: 1 # Only register every Nth landmark edge + landmark_edges_spacing: 2 # Only register every Nth landmark edge delta_landmark: 0.3 # Confidence threshold for values from data association optimize: 5 # Number of optimizations to realize xy_max_offset: 5.0 # m Max jump of vehicle position in a single optimization From 9a27f285dda5e73ae7d37c8c84c1f2c18ceaa5ed Mon Sep 17 00:00:00 2001 From: root Date: Sat, 21 Mar 2026 22:19:44 +0000 Subject: [PATCH 13/14] fix: set clipping in true and restore publishers frames --- src/perception/config/perception_config.yaml | 2 +- src/perception/src/perception_node.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index 45bcb91f..9ae29db9 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -29,7 +29,7 @@ perception: slow_straight_cropping: false # Leave only a straight section in front of the car when moving slowly # Clipping parameters - clipping: false # Enable clipping of the point cloud + clipping: true # Enable clipping of the point cloud max_x_clip: 50.0 max_y_clip: 5.0 min_x_clip: -30.0 diff --git a/src/perception/src/perception_node.cpp b/src/perception/src/perception_node.cpp index ccefb65d..dfba5f79 100644 --- a/src/perception/src/perception_node.cpp +++ b/src/perception/src/perception_node.cpp @@ -385,7 +385,7 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l // Publish the filtered cloud sensor_msgs::msg::PointCloud2 filtered_msg; pcl::toROSMsg(*cloud_filtered, filtered_msg); - filtered_msg.header.frame_id = "slam/vehicle2"; + filtered_msg.header.frame_id = "/rslidar"; filtered_pub_->publish(filtered_msg); // Publish ground cloud (only if not empty) @@ -395,7 +395,7 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l ground_cloud->is_dense = true; sensor_msgs::msg::PointCloud2 ground_msg; pcl::toROSMsg(*ground_cloud, ground_msg); - ground_msg.header.frame_id = "slam/vehicle2"; + ground_msg.header.frame_id = "/rslidar"; ground_cloud_pub_->publish(ground_msg); } @@ -419,7 +419,7 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l for (const auto& c : cluster_clouds) *all_clusters_cloud += *c; sensor_msgs::msg::PointCloud2 clusters_msg; pcl::toROSMsg(*all_clusters_cloud, clusters_msg); - clusters_msg.header.frame_id = "slam/vehicle2"; + clusters_msg.header.frame_id = "/rslidar"; clusters_pub_->publish(clusters_msg); } @@ -427,7 +427,7 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l // Publish the map cloud sensor_msgs::msg::PointCloud2 map_msg; pcl::toROSMsg(*final_map, map_msg); - map_msg.header.frame_id = "slam/vehicle2"; + map_msg.header.frame_id = "/rslidar"; map_msg.header.stamp = lidar_timestamp_; map_pub_->publish(map_msg); From 8d4e8936b423a44040d57a139a1af19aeaadf0b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ignacio=20S=C3=A1nchez?= Date: Sun, 22 Mar 2026 01:39:43 +0100 Subject: [PATCH 14/14] feat: delete unused mutex initialization variable --- src/slam/graph_slam/include/graph_slam/graph_slam_node.hpp | 2 -- 1 file changed, 2 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 3da7b9fa..629249e6 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 @@ -100,8 +100,6 @@ class GraphSlam : public rclcpp::Node common_msgs::msg::Trajectory opt_traj_msg_; double last_optimization_time_ = 0.0; double last_data_association_time_ = 0.0; - std::mutex graph_mutex_; - std::mutex buffer_mutex_; // Parameters std::string kLidarTopic;