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 b87129c5..62809565 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 / 100)), + std::chrono::milliseconds(static_cast(1000.0 / kStateFrequency)), 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..2c9396ac 100644 --- a/src/common/common_meta/launch/rosbag_simulation_launch.py +++ b/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, 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/pc_supervisor/config/pc_supervisor_config.yaml b/src/pc_supervisor/config/pc_supervisor_config.yaml index 30a55f7d..3bd978c3 100644 --- a/src/pc_supervisor/config/pc_supervisor_config.yaml +++ b/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 \ No newline at end of file + record_pcap: false + ignore_rslidar: false + record: true 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 807aca4c..c70a924a 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_); diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index f1f5e3c9..a9700455 100644 --- a/src/perception/config/perception_config.yaml +++ b/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 diff --git a/src/perception/include/perception/perception_node.hpp b/src/perception/include/perception/perception_node.hpp index 094d52a4..5f3c17f5 100644 --- a/src/perception/include/perception/perception_node.hpp +++ b/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 final_times; std::vector::Ptr> cloud_buffer_; pcl::PointCloud::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 pose_timestamps_; + std::vector vx_buffer_; + std::vector vy_buffer_; + std::vector r_buffer_; // Cropping bool kCrop; @@ -152,6 +160,7 @@ class Perception : public rclcpp::Node bool kDebug; bool kDebugClouds; + bool kUseArtificialTimestamp; // Subscribers diff --git a/src/perception/include/perception/utils.h b/src/perception/include/perception/utils.h index ef980828..e85fead9 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 4754684a..45e190f6 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); @@ -68,6 +69,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); @@ -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::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; @@ -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 diff --git a/src/slam/graph_slam/config/graph_slam_config.yaml b/src/slam/graph_slam/config/graph_slam_config.yaml index 5ab43d40..794f73ee 100644 --- a/src/slam/graph_slam/config/graph_slam_config.yaml +++ b/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 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..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 @@ -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); @@ -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_; 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..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 @@ -44,6 +44,8 @@ #include #include +#include +#include class GraphSlam : public rclcpp::Node @@ -65,7 +67,7 @@ class GraphSlam : public rclcpp::Node double y_noise_ = 5.0E-2; double yaw_noise_ = 5.0E-3; double perception_noise_ = 0.05; - double dt = 0.01; + double dt = 0.005; g2o::VertexSE2* last_optimized_pose_ = nullptr; std::vector odom_buffer_; @@ -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; @@ -90,7 +94,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 edfd8069..4fa19b1d 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -247,24 +247,32 @@ void GraphSlam::state_callback(const common_msgs::msg::State::SharedPtr msg) pose_edges_counter_ = 0; } pose_edges_counter_++; - pose_counter_++; - } void GraphSlam::lidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - if (pose_vertices_.size() == 0) { - return; - } - std::vector observed_landmarks; std::vector unmatched_landmarks; rclcpp::Time obs_time = msg->header.stamp; + Eigen::Vector3d pose_at_scan; + + if (pose_vertices_.size() == 0) { + return; + } - // adjust_perception_delay(obs_time); + adjust_perception_delay(obs_time); + + if (!vertex_at_scan_time_) { + RCLCPP_WARN(this->get_logger(), "No pose vertex available for the scan timestamp."); + return; + } + + 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); @@ -273,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)); } @@ -310,15 +318,13 @@ 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]; - 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_; @@ -328,28 +334,23 @@ 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(); 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 RCLCPP_WARN(this->get_logger(), "Unknown data_association_method='%s'. Falling back to 'mrpt'.", kDataAssociationMethod); + 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.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 @@ -365,7 +366,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()); @@ -377,7 +378,6 @@ 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) { @@ -386,7 +386,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()); @@ -402,8 +402,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,7 +422,7 @@ void GraphSlam::camera_callback(const sensor_msgs::msg::PointCloud2::SharedPtr m 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_); @@ -440,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_); @@ -449,17 +449,16 @@ void GraphSlam::camera_callback(const sensor_msgs::msg::PointCloud2::SharedPtr m void GraphSlam::optimizer_callback(){ double t0 = this->now().seconds(); - update_pose_predictions(); - fill_graph(); - + optimizer_.initializeOptimization(); - optimizer_.optimize(kOptimize); - 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_pub_->publish(lap_count_msg); @@ -467,8 +466,8 @@ void GraphSlam::optimizer_callback(){ 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_); } } @@ -550,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_){ @@ -604,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; @@ -613,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; @@ -635,8 +633,14 @@ void GraphSlam::fix_map(){ void GraphSlam::publish_map(){ + Eigen::Vector3d current_pose; + { + + current_pose = vehicle_pose_; + } + 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); } @@ -669,7 +673,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; @@ -760,7 +764,7 @@ void GraphSlam::check_finish_line(){ } if (lap_count_ == 1 && !map_fixed_) { - fix_map(); + fix_map(); } } @@ -937,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] << "," @@ -961,9 +965,10 @@ 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]; + 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; } } @@ -973,7 +978,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]; }