From bf76c5bd0f8449adcdd28153c38964267646aaaf Mon Sep 17 00:00:00 2001 From: Marcos Valera Date: Sun, 8 Nov 2026 00:48:26 +0000 Subject: [PATCH 1/2] feat: add ARUSSim launch without SLAM using ground truth (ideal_slam) --- src/arussim/CMakeLists.txt | 16 +- src/arussim/config/ideal_slam_config.yaml | 5 + src/arussim/launch/sim_no_slam_launch.py | 85 ++++++++++ src/arussim/src/ideal_slam.cpp | 198 ++++++++++++++++++++++ src/arussim/src/main_interface.cpp | 14 +- 5 files changed, 311 insertions(+), 7 deletions(-) create mode 100644 src/arussim/config/ideal_slam_config.yaml create mode 100644 src/arussim/launch/sim_no_slam_launch.py create mode 100644 src/arussim/src/ideal_slam.cpp diff --git a/src/arussim/CMakeLists.txt b/src/arussim/CMakeLists.txt index d37acafa..934d6c59 100755 --- a/src/arussim/CMakeLists.txt +++ b/src/arussim/CMakeLists.txt @@ -168,11 +168,25 @@ ament_target_dependencies(supervisor_exec rclcpp tf2_ros tf2) -# Install both executables +# generate executable for ideal_slam.cpp (ground truth SLAM, replaces graph_slam in simulation) +add_executable(ideal_slam_exec src/ideal_slam.cpp) +target_include_directories(ideal_slam_exec PRIVATE include) +ament_target_dependencies(ideal_slam_exec rclcpp + std_msgs + sensor_msgs + geometry_msgs + common_msgs + tf2_ros + tf2 + PCL + pcl_conversions) + +# Install executables install(TARGETS arussim_exec sensors_exec supervisor_exec + ideal_slam_exec DESTINATION lib/${PROJECT_NAME}) # share folders location diff --git a/src/arussim/config/ideal_slam_config.yaml b/src/arussim/config/ideal_slam_config.yaml new file mode 100644 index 00000000..ea824c66 --- /dev/null +++ b/src/arussim/config/ideal_slam_config.yaml @@ -0,0 +1,5 @@ +ideal_slam: + ros__parameters: + sensor_max_dist: 40.0 # m Cones farther than this are not added to the map (lidar_max_dist) + sensor_fov: 120.0 # deg Field of view in which cones are added to the map (lidar_fov) + min_lap_distance: 30.0 # m Min driven distance to count a new lap diff --git a/src/arussim/launch/sim_no_slam_launch.py b/src/arussim/launch/sim_no_slam_launch.py new file mode 100644 index 00000000..af8e54c3 --- /dev/null +++ b/src/arussim/launch/sim_no_slam_launch.py @@ -0,0 +1,85 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_ros.actions import Node + + +def generate_launch_description(): + + launch_pub = ExecuteProcess( + cmd=["ros2 topic pub /arussim/launch std_msgs/msg/Bool 'data: true' -1" ], + shell=True + ) + + return LaunchDescription([ + # Ideal SLAM: publishes /slam/map, /slam/lap_count and TF slam/vehicle from the ARUSSim ground truth + create_node(pkg='arussim', + exec='ideal_slam_exec', + name='ideal_slam', + config='ideal_slam_config.yaml'), + # Same nodes and parameters as common_meta/simulation_launch.py, without graph_slam + create_node(pkg='pc_supervisor', + params=[{'mission_name': 'simulation', + 'sim_mode': False, + 'record': False}]), + create_node(pkg='path_planning', + params=[{'v_max': 10.0, + 'ax_max': 7.5, + 'ay_max': 7.5, + 'debug': False}]), + create_node(pkg='controller', + params=[{'min_cmd': -22.0, + 'max_cmd': 22.0, + 'look_ahead_distance': 6.0, + 'optimized_look_ahead_distance': 8.0, + 'use_optimized_trajectory': True, + 'first_lap_steer_control': "PP", + 'optimized_steer_control': "MPC", + 'use_variable_look_ahead_distance': True, + 'debug': False}]), + create_node(pkg='visualization'), + create_node(pkg='can_interface', + params=[{'simulation': True}]), + create_node(pkg='car_state', + params=[{'simulation': True, + 'mission': 'trackdrive', + 'trackdrive_laps': 1000, + 'debug': False, + 'get_arussim_ground_truth': False}]), + create_node(pkg='trajectory_optimization', + params=[{'v_max': 15.0, + 'd_min': 1.3, + 'mu_y': 0.8, + 'mu_throttle': 0.75, + 'mu_brake': 0.75, + 'first_lap_optimization_method': "smoothing", + 'use_ggv': False, + 'use_grip_estimation': False, + 'k_trust_lon': 0.8, + 'k_trust_lat': 0.9, + 'debug': False}]), + create_node(pkg='cube_mars_interface', + params=[{'simulation': True}]), + create_node(pkg='grip_estimation'), + launch_pub + ]) + + +def create_node(pkg, config=None, exec=None, params=[], name=None): + + if config is None: + config = pkg + "_config.yaml" + if exec is None: + exec = pkg + "_exec" + + package_share_directory = get_package_share_directory(pkg) + config_file = os.path.join(package_share_directory, "config", config) + + return Node( + package=pkg, + executable=exec, + name=name or pkg, + output="screen", + parameters=[config_file]+params + ) \ No newline at end of file diff --git a/src/arussim/src/ideal_slam.cpp b/src/arussim/src/ideal_slam.cpp new file mode 100644 index 00000000..7ccf7f0f --- /dev/null +++ b/src/arussim/src/ideal_slam.cpp @@ -0,0 +1,198 @@ +/** + * @file ideal_slam.cpp + * @brief Ideal SLAM for ARUSSim: replaces graph_slam using the simulator ground truth. + * + * Publishes the same interface as graph_slam: + * - TF arussim/world -> slam/vehicle (perfect pose) + * - /slam/map (cones already seen by the car, perfect position and color) + * - /slam/lap_count (laps completed) + */ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "arussim/ConeXYZColorScore.h" + +// Cone published in /slam/map. The stack reads the covariances, which are 0 here (ideal SLAM). +struct SlamCone { + PCL_ADD_POINT4D; + int color; + float score; + float cov_xx; + float cov_yy; + float cov_xy; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +POINT_CLOUD_REGISTER_POINT_STRUCT (SlamCone, + (float, x, x) + (float, y, y) + (float, z, z) + (int, color, color) + (float, score, score) + (float, cov_xx, cov_xx) + (float, cov_yy, cov_yy) + (float, cov_xy, cov_xy) +) + +constexpr double kMinConeDist = 0.1; // m Min distance between two cones of the map + +class IdealSlam : public rclcpp::Node +{ +public: + IdealSlam() : Node("ideal_slam") + { + this->declare_parameter("sensor_max_dist", 40.0); // m Max range to see a cone + this->declare_parameter("sensor_fov", 120.0); // deg Field of view to see a cone + this->declare_parameter("min_lap_distance", 30.0); // m Min distance to count a new lap + max_dist_ = this->get_parameter("sensor_max_dist").as_double(); + fov_ = this->get_parameter("sensor_fov").as_double() * M_PI / 180.0; + min_lap_distance_ = this->get_parameter("min_lap_distance").as_double(); + + tf_broadcaster_ = std::make_shared(this); + map_pub_ = this->create_publisher("/slam/map", 10); + lap_count_pub_ = this->create_publisher("/slam/lap_count", 10); + + track_sub_ = this->create_subscription( + "/arussim/track", 1, std::bind(&IdealSlam::track_callback, this, std::placeholders::_1)); + ground_truth_sub_ = this->create_subscription( + "/arussim/ground_truth", 1, std::bind(&IdealSlam::ground_truth_callback, this, std::placeholders::_1)); + tpl_sub_ = this->create_subscription( + "/arussim/tpl_signal", 10, std::bind(&IdealSlam::tpl_callback, this, std::placeholders::_1)); + reset_sub_ = this->create_subscription( + "/arussim/reset", 1, [this](std_msgs::msg::Bool::SharedPtr) { reset(); }); + + timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&IdealSlam::on_timer, this)); + } + +private: + void reset() + { + seen_.assign(track_.size(), false); + map_.clear(); + lap_count_ = 0; + driven_distance_ = 0.0; + } + + void track_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + pcl::PointCloud new_track; + pcl::fromROSMsg(*msg, new_track); + + // The simulator republishes the track periodically: only reset when it changes + bool changed = new_track.size() != track_.size(); + for (size_t i = 0; !changed && i < new_track.size(); ++i) { + changed = std::abs(new_track[i].x - track_[i].x) > 1e-3 + || std::abs(new_track[i].y - track_[i].y) > 1e-3; + } + if (changed) { + track_ = new_track; + reset(); + } + } + + void ground_truth_callback(const common_msgs::msg::State::SharedPtr msg) + { + if (has_pose_) driven_distance_ += std::hypot(msg->x - x_, msg->y - y_); + x_ = msg->x; + y_ = msg->y; + yaw_ = msg->yaw; + has_pose_ = true; + + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = this->now(); + tf.header.frame_id = "arussim/world"; + tf.child_frame_id = "slam/vehicle"; + tf.transform.translation.x = x_; + tf.transform.translation.y = y_; + tf2::Quaternion q; + q.setRPY(0, 0, yaw_); + tf.transform.rotation.x = q.x(); + tf.transform.rotation.y = q.y(); + tf.transform.rotation.z = q.z(); + tf.transform.rotation.w = q.w(); + tf_broadcaster_->sendTransform(tf); + } + + void tpl_callback(const std_msgs::msg::Bool::SharedPtr) + { + // Same criterion as graph_slam: a new lap needs a minimum driven distance + if (driven_distance_ > min_lap_distance_) { + driven_distance_ = 0.0; + lap_count_++; + } + } + + void on_timer() + { + // Add the cones inside the sensor range and field of view + for (size_t i = 0; i < track_.size(); ++i) { + const double dx = track_[i].x - x_; + const double dy = track_[i].y - y_; + const double angle = std::remainder(std::atan2(dy, dx) - yaw_, 2 * M_PI); + if (seen_[i] || std::hypot(dx, dy) > max_dist_ || std::abs(angle) > fov_ / 2.0) continue; + + seen_[i] = true; + + // Like graph_slam, do not add a cone if there is already one in the map at the same place + // (the triangulation in path_planning fails with duplicated points) + const bool duplicated = std::any_of(map_.begin(), map_.end(), [&](const SlamCone& c) { + return std::hypot(c.x - track_[i].x, c.y - track_[i].y) < kMinConeDist; + }); + if (duplicated) continue; + + SlamCone cone{}; + cone.x = track_[i].x; + cone.y = track_[i].y; + cone.z = track_[i].z; + cone.data[3] = 1.0f; + // Like the simulated perception, only blue and yellow are colored (orange and TPL cones are uncolored) + cone.color = (track_[i].color == 0 || track_[i].color == 1) ? track_[i].color : -1; + cone.score = track_[i].score; + map_.push_back(cone); + } + + sensor_msgs::msg::PointCloud2 map_msg; + pcl::toROSMsg(map_, map_msg); + map_msg.header.stamp = this->now(); + map_msg.header.frame_id = "arussim/world"; + map_pub_->publish(map_msg); + + std_msgs::msg::Int16 lap_msg; + lap_msg.data = lap_count_; + lap_count_pub_->publish(lap_msg); + } + + double max_dist_, fov_, min_lap_distance_; + pcl::PointCloud track_; + pcl::PointCloud map_; + std::vector seen_; + double x_ = 0.0, y_ = 0.0, yaw_ = 0.0, driven_distance_ = 0.0; + bool has_pose_ = false; + int16_t lap_count_ = 0; + + std::shared_ptr tf_broadcaster_; + rclcpp::Publisher::SharedPtr map_pub_; + rclcpp::Publisher::SharedPtr lap_count_pub_; + rclcpp::Subscription::SharedPtr track_sub_; + rclcpp::Subscription::SharedPtr ground_truth_sub_; + rclcpp::Subscription::SharedPtr tpl_sub_, reset_sub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/arussim/src/main_interface.cpp b/src/arussim/src/main_interface.cpp index 400a34ca..6920a14d 100644 --- a/src/arussim/src/main_interface.cpp +++ b/src/arussim/src/main_interface.cpp @@ -117,11 +117,13 @@ MainInterface::MainInterface(QWidget* parent) : Panel(parent) // Launch selection launch_select_ = new QComboBox(this); launch_select_->setFixedHeight(rviz_height_ * 0.025); - std::string package_path_launch = ament_index_cpp::get_package_share_directory("common_meta"); - QString launch_path = QString::fromStdString(package_path_launch + "/launch/"); - QDir launch_dir(launch_path); - QStringList launch_files = launch_dir.entryList(QStringList() << "*sim*.py", QDir::Files); - launch_select_->addItems(launch_files); + // Launch files of the stack (common_meta) and of ARUSSim itself (e.g. sim_no_slam_launch.py) + for (const char* pkg : {"common_meta", "arussim"}) { + QDir launch_dir(QString::fromStdString(ament_index_cpp::get_package_share_directory(pkg) + "/launch/")); + for (const QString& file : launch_dir.entryList(QStringList() << "*sim*.py", QDir::Files)) { + launch_select_->addItem(file, pkg); + } + } launch_select_->setCurrentText("simulation_launch.py"); button_grid->addWidget(launch_select_, 0, 0, 1, 2); @@ -219,7 +221,7 @@ void MainInterface::launch_button_clicked() QString launch_file = launch_select_->currentText(); QStringList args; - args << "launch" << "common_meta" << launch_file; + args << "launch" << launch_select_->currentData().toString() << launch_file; simulation_process_->start("ros2", args); } } From 1088c8068e3e50268db4a5941595213365f789f9 Mon Sep 17 00:00:00 2001 From: Marcos Valera Date: Fri, 13 Nov 2026 23:22:49 +0000 Subject: [PATCH 2/2] refactor: parameters in config yaml, uncolored map and launch moved to common_meta --- src/arussim/config/ideal_slam_config.yaml | 21 +++- src/arussim/launch/sim_no_slam_launch.py | 85 ------------- src/arussim/src/ideal_slam.cpp | 144 +++++++++++++++------- src/arussim/src/main_interface.cpp | 14 +-- 4 files changed, 122 insertions(+), 142 deletions(-) delete mode 100644 src/arussim/launch/sim_no_slam_launch.py diff --git a/src/arussim/config/ideal_slam_config.yaml b/src/arussim/config/ideal_slam_config.yaml index ea824c66..66c45bc1 100644 --- a/src/arussim/config/ideal_slam_config.yaml +++ b/src/arussim/config/ideal_slam_config.yaml @@ -1,5 +1,20 @@ ideal_slam: ros__parameters: - sensor_max_dist: 40.0 # m Cones farther than this are not added to the map (lidar_max_dist) - sensor_fov: 120.0 # deg Field of view in which cones are added to the map (lidar_fov) - min_lap_distance: 30.0 # m Min driven distance to count a new lap + # TOPICS + map_topic: "/slam/map" # Output: map of cones + lap_count_topic: "/slam/lap_count" # Output: laps completed + track_topic: "/arussim/track" # Input: real cones of the track (ARUSSim) + perception_topic: "/arussim/perception" # Input: cones detected by the simulated lidar + ground_truth_topic: "/arussim/ground_truth" # Input: real state of the car (ARUSSim) + tpl_signal_topic: "/arussim/tpl_signal" # Input: the car crosses the finish line (ARUSSim) + reset_topic: "/arussim/reset" # Input: simulation reset (ARUSSim) + global_frame: "arussim/world" # Frame of the map and parent of the car TF + local_frame: "slam/vehicle" # Frame of the car TF + + # SLAM PARAMETERS + sensor_max_dist: 30.0 # m Detections farther than this are ignored + match_dist: 1.0 # m Max distance between a detection and the real cone it comes from + track_tolerance: 0.001 # m Min change in a cone position to consider that the track has changed + publish_freq: 10.0 # Hz Frequency at which the map and the lap count are published + min_observations: 12 # Times a cone must be detected to be added to the map + min_lap_distance: 30.0 # m Min driven distance to count a new lap diff --git a/src/arussim/launch/sim_no_slam_launch.py b/src/arussim/launch/sim_no_slam_launch.py deleted file mode 100644 index af8e54c3..00000000 --- a/src/arussim/launch/sim_no_slam_launch.py +++ /dev/null @@ -1,85 +0,0 @@ -import os -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import ExecuteProcess -from launch_ros.actions import Node - - -def generate_launch_description(): - - launch_pub = ExecuteProcess( - cmd=["ros2 topic pub /arussim/launch std_msgs/msg/Bool 'data: true' -1" ], - shell=True - ) - - return LaunchDescription([ - # Ideal SLAM: publishes /slam/map, /slam/lap_count and TF slam/vehicle from the ARUSSim ground truth - create_node(pkg='arussim', - exec='ideal_slam_exec', - name='ideal_slam', - config='ideal_slam_config.yaml'), - # Same nodes and parameters as common_meta/simulation_launch.py, without graph_slam - create_node(pkg='pc_supervisor', - params=[{'mission_name': 'simulation', - 'sim_mode': False, - 'record': False}]), - create_node(pkg='path_planning', - params=[{'v_max': 10.0, - 'ax_max': 7.5, - 'ay_max': 7.5, - 'debug': False}]), - create_node(pkg='controller', - params=[{'min_cmd': -22.0, - 'max_cmd': 22.0, - 'look_ahead_distance': 6.0, - 'optimized_look_ahead_distance': 8.0, - 'use_optimized_trajectory': True, - 'first_lap_steer_control': "PP", - 'optimized_steer_control': "MPC", - 'use_variable_look_ahead_distance': True, - 'debug': False}]), - create_node(pkg='visualization'), - create_node(pkg='can_interface', - params=[{'simulation': True}]), - create_node(pkg='car_state', - params=[{'simulation': True, - 'mission': 'trackdrive', - 'trackdrive_laps': 1000, - 'debug': False, - 'get_arussim_ground_truth': False}]), - create_node(pkg='trajectory_optimization', - params=[{'v_max': 15.0, - 'd_min': 1.3, - 'mu_y': 0.8, - 'mu_throttle': 0.75, - 'mu_brake': 0.75, - 'first_lap_optimization_method': "smoothing", - 'use_ggv': False, - 'use_grip_estimation': False, - 'k_trust_lon': 0.8, - 'k_trust_lat': 0.9, - 'debug': False}]), - create_node(pkg='cube_mars_interface', - params=[{'simulation': True}]), - create_node(pkg='grip_estimation'), - launch_pub - ]) - - -def create_node(pkg, config=None, exec=None, params=[], name=None): - - if config is None: - config = pkg + "_config.yaml" - if exec is None: - exec = pkg + "_exec" - - package_share_directory = get_package_share_directory(pkg) - config_file = os.path.join(package_share_directory, "config", config) - - return Node( - package=pkg, - executable=exec, - name=name or pkg, - output="screen", - parameters=[config_file]+params - ) \ No newline at end of file diff --git a/src/arussim/src/ideal_slam.cpp b/src/arussim/src/ideal_slam.cpp index 7ccf7f0f..70e1428a 100644 --- a/src/arussim/src/ideal_slam.cpp +++ b/src/arussim/src/ideal_slam.cpp @@ -4,13 +4,13 @@ * * Publishes the same interface as graph_slam: * - TF arussim/world -> slam/vehicle (perfect pose) - * - /slam/map (cones already seen by the car, perfect position and color) + * - /slam/map (cones detected by the simulated lidar, with perfect position) * - /slam/lap_count (laps completed) */ -#include #include #include +#include #include #include #include @@ -44,41 +44,65 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (SlamCone, (float, cov_xy, cov_xy) ) -constexpr double kMinConeDist = 0.1; // m Min distance between two cones of the map + +// Cone colors, same values as in the rest of the stack +constexpr int kBlue = 0; +constexpr int kYellow = 1; +constexpr int kUncolored = -1; class IdealSlam : public rclcpp::Node { public: IdealSlam() : Node("ideal_slam") { - this->declare_parameter("sensor_max_dist", 40.0); // m Max range to see a cone - this->declare_parameter("sensor_fov", 120.0); // deg Field of view to see a cone - this->declare_parameter("min_lap_distance", 30.0); // m Min distance to count a new lap + this->declare_parameter("map_topic", "/slam/map"); + this->declare_parameter("lap_count_topic", "/slam/lap_count"); + this->declare_parameter("track_topic", "/arussim/track"); + this->declare_parameter("perception_topic", "/arussim/perception"); + this->declare_parameter("ground_truth_topic", "/arussim/ground_truth"); + this->declare_parameter("tpl_signal_topic", "/arussim/tpl_signal"); + this->declare_parameter("reset_topic", "/arussim/reset"); + this->declare_parameter("global_frame", "arussim/world"); + this->declare_parameter("local_frame", "slam/vehicle"); + this->declare_parameter("sensor_max_dist", 30.0); + this->declare_parameter("match_dist", 1.0); + this->declare_parameter("track_tolerance", 0.001); + this->declare_parameter("publish_freq", 10.0); + this->declare_parameter("min_observations", 12); + this->declare_parameter("min_lap_distance", 30.0); + global_frame_ = this->get_parameter("global_frame").as_string(); + local_frame_ = this->get_parameter("local_frame").as_string(); max_dist_ = this->get_parameter("sensor_max_dist").as_double(); - fov_ = this->get_parameter("sensor_fov").as_double() * M_PI / 180.0; + match_dist_ = this->get_parameter("match_dist").as_double(); + track_tolerance_ = this->get_parameter("track_tolerance").as_double(); + min_observations_ = this->get_parameter("min_observations").as_int(); min_lap_distance_ = this->get_parameter("min_lap_distance").as_double(); tf_broadcaster_ = std::make_shared(this); - map_pub_ = this->create_publisher("/slam/map", 10); - lap_count_pub_ = this->create_publisher("/slam/lap_count", 10); + map_pub_ = this->create_publisher(this->get_parameter("map_topic").as_string(), 10); + lap_count_pub_ = this->create_publisher(this->get_parameter("lap_count_topic").as_string(), 10); track_sub_ = this->create_subscription( - "/arussim/track", 1, std::bind(&IdealSlam::track_callback, this, std::placeholders::_1)); + this->get_parameter("track_topic").as_string(), 1, std::bind(&IdealSlam::track_callback, this, std::placeholders::_1)); + perception_sub_ = this->create_subscription( + this->get_parameter("perception_topic").as_string(), 1, std::bind(&IdealSlam::perception_callback, this, std::placeholders::_1)); ground_truth_sub_ = this->create_subscription( - "/arussim/ground_truth", 1, std::bind(&IdealSlam::ground_truth_callback, this, std::placeholders::_1)); + this->get_parameter("ground_truth_topic").as_string(), 1, std::bind(&IdealSlam::ground_truth_callback, this, std::placeholders::_1)); tpl_sub_ = this->create_subscription( - "/arussim/tpl_signal", 10, std::bind(&IdealSlam::tpl_callback, this, std::placeholders::_1)); + this->get_parameter("tpl_signal_topic").as_string(), 10, std::bind(&IdealSlam::tpl_callback, this, std::placeholders::_1)); reset_sub_ = this->create_subscription( - "/arussim/reset", 1, [this](std_msgs::msg::Bool::SharedPtr) { reset(); }); + this->get_parameter("reset_topic").as_string(), 1, [this](std_msgs::msg::Bool::SharedPtr) { reset(); }); - timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&IdealSlam::on_timer, this)); + timer_ = this->create_wall_timer(std::chrono::duration(1.0 / this->get_parameter("publish_freq").as_double()), std::bind(&IdealSlam::on_timer, this)); } private: void reset() { - seen_.assign(track_.size(), false); + observations_.assign(track_.size(), 0); + in_map_.assign(track_.size(), false); map_.clear(); + passed_.clear(); lap_count_ = 0; driven_distance_ = 0.0; } @@ -91,8 +115,8 @@ class IdealSlam : public rclcpp::Node // The simulator republishes the track periodically: only reset when it changes bool changed = new_track.size() != track_.size(); for (size_t i = 0; !changed && i < new_track.size(); ++i) { - changed = std::abs(new_track[i].x - track_[i].x) > 1e-3 - || std::abs(new_track[i].y - track_[i].y) > 1e-3; + changed = std::abs(new_track[i].x - track_[i].x) > track_tolerance_ + || std::abs(new_track[i].y - track_[i].y) > track_tolerance_; } if (changed) { track_ = new_track; @@ -110,8 +134,8 @@ class IdealSlam : public rclcpp::Node geometry_msgs::msg::TransformStamped tf; tf.header.stamp = this->now(); - tf.header.frame_id = "arussim/world"; - tf.child_frame_id = "slam/vehicle"; + tf.header.frame_id = global_frame_; + tf.child_frame_id = local_frame_; tf.transform.translation.x = x_; tf.transform.translation.y = y_; tf2::Quaternion q; @@ -125,46 +149,69 @@ class IdealSlam : public rclcpp::Node void tpl_callback(const std_msgs::msg::Bool::SharedPtr) { - // Same criterion as graph_slam: a new lap needs a minimum driven distance + // Same criteria as graph_slam: a new lap needs a minimum driven distance if (driven_distance_ > min_lap_distance_) { driven_distance_ = 0.0; lap_count_++; } } - void on_timer() + void perception_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { - // Add the cones inside the sensor range and field of view - for (size_t i = 0; i < track_.size(); ++i) { - const double dx = track_[i].x - x_; - const double dy = track_[i].y - y_; - const double angle = std::remainder(std::atan2(dy, dx) - yaw_, 2 * M_PI); - if (seen_[i] || std::hypot(dx, dy) > max_dist_ || std::abs(angle) > fov_ / 2.0) continue; - - seen_[i] = true; - - // Like graph_slam, do not add a cone if there is already one in the map at the same place - // (the triangulation in path_planning fails with duplicated points) - const bool duplicated = std::any_of(map_.begin(), map_.end(), [&](const SlamCone& c) { - return std::hypot(c.x - track_[i].x, c.y - track_[i].y) < kMinConeDist; - }); - if (duplicated) continue; + if (!has_pose_) return; + pcl::PointCloud scan; + pcl::fromROSMsg(*msg, scan); + const double c = std::cos(yaw_), s = std::sin(yaw_); + + std::vector counted(track_.size(), false); + for (const auto& p : scan) { + if (std::hypot(p.x, p.y) > max_dist_) continue; // Farther than the sensor range we want to simulate + const double wx = x_ + p.x * c - p.y * s; + const double wy = y_ + p.x * s + p.y * c; + + size_t best = track_.size(); + double best_dist = match_dist_; + for (size_t i = 0; i < track_.size(); ++i) { + const double d = std::hypot(track_[i].x - wx, track_[i].y - wy); + if (d < best_dist) { best_dist = d; best = i; } + } + if (best == track_.size() || counted[best]) continue; + counted[best] = true; + observations_[best]++; + + // Like graph_slam, a cone is published once it has been seen several times + if (in_map_[best] || observations_[best] < min_observations_) continue; + in_map_[best] = true; SlamCone cone{}; - cone.x = track_[i].x; - cone.y = track_[i].y; - cone.z = track_[i].z; - cone.data[3] = 1.0f; - // Like the simulated perception, only blue and yellow are colored (orange and TPL cones are uncolored) - cone.color = (track_[i].color == 0 || track_[i].color == 1) ? track_[i].color : -1; - cone.score = track_[i].score; + cone.x = track_[best].x; + cone.y = track_[best].y; + cone.z = 0; + cone.color = kUncolored; // Uncolored until the car passes it (see on_timer) + cone.score = track_[best].score; map_.push_back(cone); + passed_.push_back(false); + } + } + + void on_timer() + { + // Same as graph_slam (pass_coloring): a cone is colored when the car has just passed it + const double c = std::cos(yaw_), s = std::sin(yaw_); + for (size_t i = 0; i < map_.size(); ++i) { + if (passed_[i]) continue; + const double dx = map_[i].x - x_, dy = map_[i].y - y_; + const double lx = dx * c + dy * s, ly = -dx * s + dy * c; + if (std::hypot(lx, ly) < 6.0 && lx < 0.0 && lx > -2.0) { + map_[i].color = ly > 0.0 ? kBlue : kYellow; + passed_[i] = true; + } } sensor_msgs::msg::PointCloud2 map_msg; pcl::toROSMsg(map_, map_msg); map_msg.header.stamp = this->now(); - map_msg.header.frame_id = "arussim/world"; + map_msg.header.frame_id = global_frame_; map_pub_->publish(map_msg); std_msgs::msg::Int16 lap_msg; @@ -172,10 +219,14 @@ class IdealSlam : public rclcpp::Node lap_count_pub_->publish(lap_msg); } - double max_dist_, fov_, min_lap_distance_; + std::string global_frame_, local_frame_; + int min_observations_; + double max_dist_, match_dist_, track_tolerance_, min_lap_distance_; pcl::PointCloud track_; pcl::PointCloud map_; - std::vector seen_; + std::vector passed_; // Cone already colored because the car passed it + std::vector observations_; + std::vector in_map_; double x_ = 0.0, y_ = 0.0, yaw_ = 0.0, driven_distance_ = 0.0; bool has_pose_ = false; int16_t lap_count_ = 0; @@ -185,6 +236,7 @@ class IdealSlam : public rclcpp::Node rclcpp::Publisher::SharedPtr lap_count_pub_; rclcpp::Subscription::SharedPtr track_sub_; rclcpp::Subscription::SharedPtr ground_truth_sub_; + rclcpp::Subscription::SharedPtr perception_sub_; rclcpp::Subscription::SharedPtr tpl_sub_, reset_sub_; rclcpp::TimerBase::SharedPtr timer_; }; diff --git a/src/arussim/src/main_interface.cpp b/src/arussim/src/main_interface.cpp index 6920a14d..400a34ca 100644 --- a/src/arussim/src/main_interface.cpp +++ b/src/arussim/src/main_interface.cpp @@ -117,13 +117,11 @@ MainInterface::MainInterface(QWidget* parent) : Panel(parent) // Launch selection launch_select_ = new QComboBox(this); launch_select_->setFixedHeight(rviz_height_ * 0.025); - // Launch files of the stack (common_meta) and of ARUSSim itself (e.g. sim_no_slam_launch.py) - for (const char* pkg : {"common_meta", "arussim"}) { - QDir launch_dir(QString::fromStdString(ament_index_cpp::get_package_share_directory(pkg) + "/launch/")); - for (const QString& file : launch_dir.entryList(QStringList() << "*sim*.py", QDir::Files)) { - launch_select_->addItem(file, pkg); - } - } + std::string package_path_launch = ament_index_cpp::get_package_share_directory("common_meta"); + QString launch_path = QString::fromStdString(package_path_launch + "/launch/"); + QDir launch_dir(launch_path); + QStringList launch_files = launch_dir.entryList(QStringList() << "*sim*.py", QDir::Files); + launch_select_->addItems(launch_files); launch_select_->setCurrentText("simulation_launch.py"); button_grid->addWidget(launch_select_, 0, 0, 1, 2); @@ -221,7 +219,7 @@ void MainInterface::launch_button_clicked() QString launch_file = launch_select_->currentText(); QStringList args; - args << "launch" << launch_select_->currentData().toString() << launch_file; + args << "launch" << "common_meta" << launch_file; simulation_process_->start("ros2", args); } }