Feature/slam/fix localization errors

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

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;

src/car_state/src/car_state_node.cpp

@@ -123,10 +123,12 @@ CarState::CarState(): Node("car_state")
     this->declare_parameter<double>("lf", 0.84315);
     this->declare_parameter<double>("tf", 1.22);
     this->declare_parameter<double>("tr", 1.22);
+    this->declare_parameter<double>("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<double>("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<int>(1000.0 / 100)),
+        std::chrono::milliseconds(static_cast<int>(1000.0 / kStateFrequency)),
         std::bind(&CarState::on_timer, this));
 
     // CPU monitor timer

src/common/common_meta/launch/rosbag_simulation_launch.py

@@ -13,7 +13,9 @@ 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',
+                             'stats_topic': '/perception/stats2',
+                             'use_artificial_timestamp': True}]),
         create_node(pkg='path_planning',
                     params=[{'map_topic': '/slam/map2',
                              'car_info_topic': '/car_state/car_info2',
@@ -31,10 +33,14 @@ def generate_launch_description():
                              'global_frame': 'arussim/world2',
                              'local_frame': 'slam/vehicle2',
                              'debug': False}]),
+        create_node(pkg='pc_supervisor',
+                    params=[{'node_prefix': "/pcart2",
+                             'record': 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',
+                             'slam_stats_topic': '/slam/stats2',
                              'global_frame': 'arussim/world2',
                              'local_frame': 'slam/vehicle2',
                              'finish_line_offset': 3.0,

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

src/pc_supervisor/config/pc_supervisor_config.yaml

@@ -6,5 +6,6 @@ pc_supervisor:
     as_status_topic : "/can_interface/AS_status"
     discovery_time: 2000
     monitor_time: 500
-    record_pcap: true
-    ignore_rslidar: false
+    record_pcap: false
+    ignore_rslidar: false
+    record: true

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
   /**

src/pc_supervisor/src/pc_supervisor_node.cpp

@@ -22,6 +22,7 @@ PcSupervisor::PcSupervisor() : Node("pc_supervisor") {
     kMonitorTime = this->declare_parameter<int>("monitor_time", 500);
     kRecordPcap = this->declare_parameter<bool>("record_pcap", true);
     kIgnoreRSLiDAR = this->declare_parameter<bool>("ignore_rslidar", false);
+    kRecord = this->declare_parameter<bool>("record", true);
 
     // Topics
     std::string cpu_total_topic = kNodePrefix + "/cpu_total";
@@ -247,7 +248,7 @@ void PcSupervisor::generic_topic_callback(std::shared_ptr<rclcpp::SerializedMess
 
 void PcSupervisor::as_status_callback(const std_msgs::msg::Float32::SharedPtr msg) {
     int as_status = static_cast<int>(msg->data);
-    bool should_record = (as_status >= 1 && as_status <= 3); 
+    bool should_record = ((as_status >= 1 && as_status <= 3) && kRecord); 
 
     std::lock_guard<std::mutex> lock(bag_mutex_);
 

src/perception/config/perception_config.yaml

@@ -11,11 +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
-    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
+
+    # Deskewing
+    deskewing: true               # Motion compensation within each LiDAR scan using point timestamps
+    buffer_size: 100
 
     # Cropping parameters
     crop: false
@@ -25,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

src/perception/include/perception/perception_node.hpp

@@ -58,8 +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 lidar_timestamp_;
         std::vector<double> final_times;
         std::vector<pcl::PointCloud<PointXYZIRingTime>::Ptr> cloud_buffer_;
         pcl::PointCloud<PointXYZIRingTime>::Ptr prev_cones_;
@@ -90,7 +91,14 @@ class Perception : public rclcpp::Node
         std::string kClippingBoxValuesTopic;
 
         bool kWaitDriving;
-        bool kDeskew;
+
+        // Deskewing
+        bool kDeskewing;
+        int kBufferSize;
+        std::vector<rclcpp::Time> pose_timestamps_;
+        std::vector<double> vx_buffer_;
+        std::vector<double> vy_buffer_;
+        std::vector<double> r_buffer_;
 
         // Cropping
         bool kCrop;
@@ -152,6 +160,7 @@ class Perception : public rclcpp::Node
 
         bool kDebug;
         bool kDebugClouds;
+        bool kUseArtificialTimestamp;
 
 
         // Subscribers

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<PointXYZIRingTime>::Ptr cloud, double vx, double vy, double yaw_rate)
-    {
+    void motion_correction(pcl::PointCloud<PointXYZIRingTime>::Ptr cloud, std::vector<double> vx_buffer, std::vector<double> vy_buffer, 
+        std::vector<double> r_buffer, std::vector<rclcpp::Time> 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<int>(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<size_t>(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);

src/perception/src/perception_node.cpp

@@ -25,7 +25,8 @@ Perception::Perception() : Node("Perception")
     this->declare_parameter<std::string>("stats_topic", "/perception/stats");
     this->declare_parameter<std::string>("clipping_box_values_topic", "/perception/clipping_box_values");
     this->declare_parameter<bool>("wait_driving", false);
-    this->declare_parameter<bool>("deskew", true);
+    this->declare_parameter<bool>("deskewing", true);
+    this->declare_parameter<int>("buffer_size", 100);
     this->declare_parameter<bool>("crop", true);
     this->declare_parameter<bool>("clipping", true);
     this->declare_parameter<double>("min_x_clip", -30.0);
@@ -68,6 +69,7 @@ Perception::Perception() : Node("Perception")
     this->declare_parameter<double>("coloring_threshold", 15.0);
     this->declare_parameter<bool>("debug", true);
     this->declare_parameter<bool>("debug_clouds", true);
+    this->declare_parameter<bool>("use_artificial_timestamp", true);
 
     // Get the parameters
     this->get_parameter("lidar_topic", kLidarTopic);
@@ -82,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);
@@ -125,6 +128,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;
@@ -176,12 +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) lidar_timestamp_ = this->now();
+    else lidar_timestamp_ = lidar_msg->header.stamp;
 
     if (kWaitDriving && as_status_ != 3) return;
     common_msgs::msg::PerceptionStats stats_msg;
@@ -191,10 +208,12 @@ void Perception::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr l
     pcl::PointCloud<PointXYZIRingTime>::Ptr cloud(new pcl::PointCloud<PointXYZIRingTime>);
     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<PointXYZIRingTime> crop_box;
@@ -351,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();
@@ -416,7 +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 = "/rslidar";
-    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

src/slam/graph_slam/config/graph_slam_config.yaml

@@ -12,31 +12,31 @@ 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: 10000                #        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: 10000         #        Max number of landmark edges given to 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
+    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
     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

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<pcl::PointXYZ> kdtree;
                 kdtree.setInputCloud(obs_pcl);
                 std::vector<int> 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<pcl::PointXYZ> 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<int> indices(1);
                 std::vector<float> distances(1);
@@ -199,7 +197,7 @@ class DataAssociation{
             mrpt::math::CMatrixDouble predictions_cov(2 * map_.size(), 2);
             std::vector<int> 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_;