From 6154934c88cfc7e3cb681d591544c575b3744719 Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Sun, 1 Nov 2026 19:22:33 +0100 Subject: [PATCH 1/7] feat: add configurable topic, mean, max and 50 ms objective to slam stats panel --- .../include/arussim/stats_interface.hpp | 23 ++++++ src/arussim/src/stats_interface.cpp | 76 +++++++++++++++---- 2 files changed, 85 insertions(+), 14 deletions(-) diff --git a/src/arussim/include/arussim/stats_interface.hpp b/src/arussim/include/arussim/stats_interface.hpp index 2bbe50a..59c7d5a 100644 --- a/src/arussim/include/arussim/stats_interface.hpp +++ b/src/arussim/include/arussim/stats_interface.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -46,6 +47,18 @@ class StatsInterface : public rviz_common::Panel void onInitialize() override; + /** + * @brief Loads the panel settings from the .rviz file (SlamStatsTopic). + * @param config Panel configuration. + */ + void load(const rviz_common::Config & config) override; + + /** + * @brief Saves the panel settings to the .rviz file (SlamStatsTopic). + * @param config Panel configuration. + */ + void save(rviz_common::Config config) const override; + protected: std::shared_ptr node_ptr_; rclcpp::Publisher::SharedPtr publisher_; @@ -126,6 +139,12 @@ private Q_SLOTS: rclcpp::Subscription::SharedPtr state_sub_; rclcpp::Subscription::SharedPtr gt_state_sub_; rclcpp::Subscription::SharedPtr stats_sub_; + QString slam_stats_topic_ = "/slam/stats"; // /slam/stats2 in rosbag_simulation + + /** + * @brief (Re)creates the SlamStats subscription with slam_stats_topic_. + */ + void subscribe_slam_stats(); rclcpp::Subscription::SharedPtr lap_time_sub_; rclcpp::Subscription::SharedPtr hit_cones_bool_sub_; rclcpp::Subscription::SharedPtr target_speed_sub_; @@ -140,6 +159,10 @@ private Q_SLOTS: double observed_landmarks_ = 0.0; double unmatched_landmarks_ = 0.0; double optimization_time_ = 0.0; + double optimization_time_sum_ = 0.0; + double optimization_time_max_ = 0.0; + int optimization_time_count_ = 0; + static constexpr double kOptimizationTimeObjective = 0.050; // s, SLAM objective (max) double data_association_time_ = 0.0; double bar_size_; diff --git a/src/arussim/src/stats_interface.cpp b/src/arussim/src/stats_interface.cpp index ae00e53..9a17d0c 100644 --- a/src/arussim/src/stats_interface.cpp +++ b/src/arussim/src/stats_interface.cpp @@ -172,15 +172,7 @@ void StatsInterface::onInitialize() rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); // Subscribers - stats_sub_ = node->create_subscription( - "/slam/stats", 1, - [this](const common_msgs::msg::SlamStats::SharedPtr msg) { - QMetaObject::invokeMethod(this, [this, msg]() { - stats_callback(msg->active_vertices, msg->active_edges, msg->observed_landmarks, - msg->unmatched_landmarks,msg->optimization_time, msg->data_association_time); - }, Qt::QueuedConnection); - } - ); + subscribe_slam_stats(); // Subscribe to car state to compute slam jumps using member callback state_sub_ = node->create_subscription( @@ -215,6 +207,40 @@ void StatsInterface::onInitialize() slam_graph_label_->installEventFilter(this); } +void StatsInterface::subscribe_slam_stats() +{ + if (!node_ptr_) return; + rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); + stats_sub_ = node->create_subscription( + slam_stats_topic_.toStdString(), 1, + [this](const common_msgs::msg::SlamStats::SharedPtr msg) { + QMetaObject::invokeMethod(this, [this, msg]() { + stats_callback(msg->active_vertices, msg->active_edges, msg->observed_landmarks, + msg->unmatched_landmarks, msg->optimization_time, + msg->data_association_time); + }, Qt::QueuedConnection); + } + ); +} + +void StatsInterface::load(const rviz_common::Config & config) +{ + rviz_common::Panel::load(config); + QString topic; + if (config.mapGetString("SlamStatsTopic", &topic) && !topic.isEmpty() && + topic != slam_stats_topic_) { + slam_stats_topic_ = topic; + reset_callback(); + subscribe_slam_stats(); + } +} + +void StatsInterface::save(rviz_common::Config config) const +{ + rviz_common::Panel::save(config); + config.mapSetValue("SlamStatsTopic", slam_stats_topic_); +} + bool StatsInterface::eventFilter(QObject* obj, QEvent* event) { if (obj == slam_graph_label_) { @@ -359,6 +385,9 @@ void StatsInterface::stats_callback(double active_vertices, double active_edges, unmatched_landmarks_ = unmatched_landmarks; optimization_time_ = optimization_time; data_association_time_ = data_association_time; + optimization_time_sum_ += optimization_time; + optimization_time_max_ = std::max(optimization_time_max_, optimization_time); + optimization_time_count_++; // Collect the sample now (cheap); defer the repaint to the render timer. collect_optimizer_sample(); @@ -584,7 +613,8 @@ void StatsInterface::collect_optimizer_sample() optimization_time_history_.append(qMakePair(current_time, optimization_time_)); // Calculate max data - double current_max = 0.025; // Mínimo una división para evitar división por cero + // At least one division above the 50 ms objective, so its line is always visible + double current_max = kOptimizationTimeObjective + 0.01; for (const auto &point : optimization_time_history_) { current_max = std::max(current_max, point.second); } @@ -631,6 +661,15 @@ void StatsInterface::rebuild_optimizer_background(int w, int h) int text_y = (j == num_rows) ? y_pos + 12 : y_pos; painter.drawText(5, text_y, QString::number(val, 'f', 3)); } + + // Objective line: max. optimization time 50 ms + const double objective_norm = kOptimizationTimeObjective / max_optimization_time_; + if (objective_norm > 0.0 && objective_norm < 1.0) { + int y_objective = h - qRound(objective_norm * h); + painter.setPen(QPen(QColor(200, 0, 0), graph_grid_width_ * 3, Qt::DashLine)); + painter.drawLine(0, y_objective, w, y_objective); + painter.drawText(w - 60, y_objective - 4, tr("50 ms")); + } } void StatsInterface::update_optimizer_time_graph() @@ -712,8 +751,14 @@ void StatsInterface::update_telemetry_labels() active_edges_label_->setText("Active Edges: " + QString::number(active_edges_, 'f', 0)); observed_landmarks_label_->setText("Observed Landmarks: " + QString::number(observed_landmarks_, 'f', 0)); unmatched_landmarks_label_->setText("Unmatched Landmarks: " + QString::number(unmatched_landmarks_, 'f', 0)); - optimization_time_label_->setText("Optimization Time: " + QString::number(optimization_time_, 'f', 5)); - data_association_time_label_->setText("Data Association Time: " + QString::number(data_association_time_, 'f', 5)); + const double mean_optimization_time = optimization_time_count_ > 0 ? + optimization_time_sum_ / optimization_time_count_ : 0.0; + optimization_time_label_->setText( + "Optimization Time: " + QString::number(optimization_time_ * 1e3, 'f', 1) + " ms" + + " | mean " + QString::number(mean_optimization_time * 1e3, 'f', 1) + " ms" + + " | max " + QString::number(optimization_time_max_ * 1e3, 'f', 1) + " ms"); + data_association_time_label_->setText( + "Data Association Time: " + QString::number(data_association_time_ * 1e3, 'f', 1) + " ms"); } /** @@ -724,6 +769,9 @@ void StatsInterface::reset_callback() { // reset graphs optimization_time_history_.clear(); + optimization_time_sum_ = 0.0; + optimization_time_max_ = 0.0; + optimization_time_count_ = 0; slam_graph_label_->clear(); slam_jump_history_.clear(); slam_accuracy_history_.clear(); @@ -753,8 +801,8 @@ void StatsInterface::reset_callback() active_edges_label_->setText("Active Edges: " + QString::number(0.0, 'f', 0)); observed_landmarks_label_->setText("Observed Landmarks: " + QString::number(0.0, 'f', 0)); unmatched_landmarks_label_->setText("Unmatched Landmarks: " + QString::number(0.0, 'f', 0)); - optimization_time_label_->setText("Optimization Time: " + QString::number(0.0, 'f', 5)); - data_association_time_label_->setText("Data Association Time: " + QString::number(0.0, 'f', 5)); + optimization_time_label_->setText("Optimization Time: 0.0 ms | mean 0.0 ms | max 0.0 ms"); + data_association_time_label_->setText("Data Association Time: 0.0 ms"); if (slam_jump_value_label_) { slam_jump_value_label_->setText("Slam Jump (Precision): " + QString::number(0.0, 'f', 4)); } From f21856e71ee008163546b036e7969b4faebf4599 Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Sun, 1 Nov 2026 19:22:33 +0100 Subject: [PATCH 2/7] feat: add node argument to arussim launch with slam rviz config --- .../config/arussim_slam_rviz_config.rviz | 420 ++++++++++++++++++ src/arussim/launch/arussim_launch.py | 50 ++- 2 files changed, 460 insertions(+), 10 deletions(-) create mode 100644 src/arussim/config/arussim_slam_rviz_config.rviz diff --git a/src/arussim/config/arussim_slam_rviz_config.rviz b/src/arussim/config/arussim_slam_rviz_config.rviz new file mode 100644 index 0000000..b1d9e26 --- /dev/null +++ b/src/arussim/config/arussim_slam_rviz_config.rviz @@ -0,0 +1,420 @@ +# RViz for arussim_launch.py node:=slam (SLAM layers + StatsInterface) +Panels: +- Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 242 +- Class: rviz_common/Selection + Name: Selection +- Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 +- Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +- Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +- Class: main_interface::MainInterface + Name: MainInterface +- Class: plot_interface::PlotInterface + Name: PlotInterface +- Class: stats_interface::StatsInterface + Name: StatsInterface + SlamStatsTopic: /slam/stats +Visualization Manager: + Class: '' + Displays: + - Alpha: 0.5 + Cell Size: 5 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 50 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1.5 + Name: Axes + Radius: 0.25 + Reference Frame: slam/vehicle + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + ? '' + : true + Topic: + Depth: 1 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /arussim/vehicle_visualization + Value: true + - Alpha: 0.75 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: prob_yellow + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 0 + Max Intensity: 1 + Min Color: 0; 0; 255 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 100 + Size (m): 0.4000000059604645 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /arussim/perception + Use Fixed Frame: true + Use rainbow: false + Value: false + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Marker + Namespaces: {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visualization/arussim_fixed_trajectory + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + triangulation: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visualization/triangulation + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + arussim/cones: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /arussim/cone_visualization + Value: true + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Marker + Namespaces: {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visualization/pursuit_point + Value: true + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Marker + Namespaces: + trajectory: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visualization/trajectory + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_slam/marker + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: color + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 0 + Max Intensity: 1 + Min Color: 0; 0; 255 + Min Intensity: 0 + Name: 'SLAM map (Select: score, cov_*, num_observations)' + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.5 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /slam/map + Use Fixed Frame: true + Use rainbow: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + left_track_limits: true + right_track_limits: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visualization/track_limits + Value: true + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Marker + Namespaces: + trajectory: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visualization/trajectory_optimized + Value: true + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Marker + Namespaces: + trajectory: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visualization/optimized_trajectory_3d + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: SLAM covariance (3 sigma) + Namespaces: {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visualization/slam/covariance + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: SLAM score + Namespaces: {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visualization/slam/score + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 0 + Max Intensity: 1 + Min Color: 0; 0; 255 + Min Intensity: 0 + Name: LiDAR raw (/rslidar_points) + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.5 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rslidar_points + Use Fixed Frame: false + Use rainbow: false + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Name: TF + Frame Timeout: 15 + Marker Scale: 2 + Show Arrows: true + Show Axes: true + Show Names: true + Update Interval: 0 + Frames: + All Enabled: false + arussim/world: + Value: true + slam/vehicle: + Value: true + rslidar: + Value: true + Tree: {} + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Fixed Frame: arussim/world + Frame Rate: 120 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 5.535693645477295 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -1.406264305114746 + Y: -0.013922890648245811 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.18662911653518677 + Target Frame: arussim/vehicle_cog + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 3.1387863159179688 + Saved: null +Window Geometry: + Displays: + collapsed: false + Height: 1892 + Hide Left Dock: false + Hide Right Dock: false + MainInterface: + collapsed: false + PlotInterface: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + StatsInterface: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3840 + X: 4096 + Y: 54 diff --git a/src/arussim/launch/arussim_launch.py b/src/arussim/launch/arussim_launch.py index b526277..7cfbda9 100755 --- a/src/arussim/launch/arussim_launch.py +++ b/src/arussim/launch/arussim_launch.py @@ -1,19 +1,40 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ExecuteProcess +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -def generate_launch_description(): - +# node:= opens RViz with config/arussim__rviz_config.rviz +VISUALIZATION_NODES = ['slam'] + + +def generate_launch_description(): + + return LaunchDescription([ + DeclareLaunchArgument( + 'node', default_value='', + description='Open RViz with the layers of this node (options: ' + + ', '.join(VISUALIZATION_NODES) + '). Empty: default RViz.'), + OpaqueFunction(function=launch_setup), + ]) + + +def launch_setup(context): + + node = LaunchConfiguration('node').perform(context) + if node and node not in VISUALIZATION_NODES: + raise RuntimeError(f"Unknown node '{node}'. Options: {', '.join(VISUALIZATION_NODES)}") + + rviz_config_name = f'arussim_{node}_rviz_config.rviz' if node else 'arussim_rviz_config.rviz' rviz_config_file = os.path.join( get_package_share_directory('arussim'), 'config', - 'arussim_rviz_config.rviz' + rviz_config_name ) - - return LaunchDescription([ + + actions = [ Node( package='rviz2', executable='rviz2', @@ -31,10 +52,19 @@ def generate_launch_description(): create_node(pkg='arussim', exec='supervisor_exec', config='supervisor_config.yaml') - ]) - + ] + + if node == 'slam': + # Only what the SLAM layers need: it reads /slam/map, publishes the static TF + # slam/vehicle -> rslidar and asks graph_slam for the marginal covariances + actions.append(create_node(pkg='visualization', + exec='slam_visualization_exec', + config='slam_visualization_config.yaml', + name='slam_visualization')) + return actions + -def create_node(pkg, config=None, exec=None, params=[]): +def create_node(pkg, config=None, exec=None, params=[], name=None): if config is None: config = pkg + "_config.yaml" @@ -47,7 +77,7 @@ def create_node(pkg, config=None, exec=None, params=[]): return Node( package=pkg, executable=exec, - name=pkg, + name=name if name is not None else pkg, output="screen", parameters=[config_file]+params ) \ No newline at end of file From 1330823136e6f8cb23f4bd6f725d30109918a9a3 Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Mon, 2 Nov 2026 00:03:29 +0100 Subject: [PATCH 3/7] feat: add selected slam cone layer to slam rviz config --- src/arussim/config/arussim_slam_rviz_config.rviz | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/arussim/config/arussim_slam_rviz_config.rviz b/src/arussim/config/arussim_slam_rviz_config.rviz index b1d9e26..90072f7 100644 --- a/src/arussim/config/arussim_slam_rviz_config.rviz +++ b/src/arussim/config/arussim_slam_rviz_config.rviz @@ -199,7 +199,7 @@ Visualization Manager: Max Intensity: 1 Min Color: 0; 0; 255 Min Intensity: 0 - Name: 'SLAM map (Select: score, cov_*, num_observations)' + Name: SLAM map Position Transformer: XYZ Selectable: true Size (Pixels): 3 @@ -276,6 +276,17 @@ Visualization Manager: Reliability Policy: Reliable Value: /visualization/slam/score Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: SLAM selected cone (click it with Publish Point) + Namespaces: {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visualization/slam/selected + Value: true - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: From 736375346321afafff275517bc529fce92149dd8 Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Tue, 3 Nov 2026 14:17:14 +0100 Subject: [PATCH 4/7] feat: add configurable state topic to slam stats panel --- src/arussim/include/arussim/stats_interface.hpp | 5 +++-- src/arussim/src/stats_interface.cpp | 12 +++++++++++- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/arussim/include/arussim/stats_interface.hpp b/src/arussim/include/arussim/stats_interface.hpp index 59c7d5a..f2e3ef8 100644 --- a/src/arussim/include/arussim/stats_interface.hpp +++ b/src/arussim/include/arussim/stats_interface.hpp @@ -48,13 +48,13 @@ class StatsInterface : public rviz_common::Panel void onInitialize() override; /** - * @brief Loads the panel settings from the .rviz file (SlamStatsTopic). + * @brief Loads the panel settings from the .rviz file (SlamStatsTopic, StateTopic). * @param config Panel configuration. */ void load(const rviz_common::Config & config) override; /** - * @brief Saves the panel settings to the .rviz file (SlamStatsTopic). + * @brief Saves the panel settings to the .rviz file (SlamStatsTopic, StateTopic). * @param config Panel configuration. */ void save(rviz_common::Config config) const override; @@ -140,6 +140,7 @@ private Q_SLOTS: rclcpp::Subscription::SharedPtr gt_state_sub_; rclcpp::Subscription::SharedPtr stats_sub_; QString slam_stats_topic_ = "/slam/stats"; // /slam/stats2 in rosbag_simulation + QString state_topic_ = "/car_state/state"; // /car_state/state2 in rosbag_simulation /** * @brief (Re)creates the SlamStats subscription with slam_stats_topic_. diff --git a/src/arussim/src/stats_interface.cpp b/src/arussim/src/stats_interface.cpp index 9a17d0c..8d53ed0 100644 --- a/src/arussim/src/stats_interface.cpp +++ b/src/arussim/src/stats_interface.cpp @@ -176,7 +176,8 @@ void StatsInterface::onInitialize() // Subscribe to car state to compute slam jumps using member callback state_sub_ = node->create_subscription( - "/car_state/state", 10, std::bind(&StatsInterface::state_callback, this, std::placeholders::_1)); + state_topic_.toStdString(), 10, + std::bind(&StatsInterface::state_callback, this, std::placeholders::_1)); // Subscribe to the simulator ground-truth pose to compute SLAM accuracy. // Buffers each pose with its header timestamp; state_callback then matches @@ -233,12 +234,21 @@ void StatsInterface::load(const rviz_common::Config & config) reset_callback(); subscribe_slam_stats(); } + if (config.mapGetString("StateTopic", &topic) && !topic.isEmpty() && topic != state_topic_ && + node_ptr_) { + state_topic_ = topic; + reset_callback(); + state_sub_ = node_ptr_->get_raw_node()->create_subscription( + state_topic_.toStdString(), 10, + std::bind(&StatsInterface::state_callback, this, std::placeholders::_1)); + } } void StatsInterface::save(rviz_common::Config config) const { rviz_common::Panel::save(config); config.mapSetValue("SlamStatsTopic", slam_stats_topic_); + config.mapSetValue("StateTopic", state_topic_); } bool StatsInterface::eventFilter(QObject* obj, QEvent* event) From 708959e14e80d7c0bf3e440be9bab6cc2c6bbb16 Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Wed, 4 Nov 2026 19:08:57 +0100 Subject: [PATCH 5/7] feat: add configurable car state and target speed topics to the plot panel --- src/arussim/CMakeLists.txt | 1 + .../include/arussim/plot_interface.hpp | 28 +++++++ src/arussim/src/plot_interface.cpp | 83 +++++++++++++++---- 3 files changed, 95 insertions(+), 17 deletions(-) diff --git a/src/arussim/CMakeLists.txt b/src/arussim/CMakeLists.txt index d37acaf..7aaca75 100755 --- a/src/arussim/CMakeLists.txt +++ b/src/arussim/CMakeLists.txt @@ -78,6 +78,7 @@ ament_target_dependencies(plot_interface_exec pluginlib rviz_common arussim_msgs + common_msgs ) ament_target_dependencies(stats_interface_exec pluginlib diff --git a/src/arussim/include/arussim/plot_interface.hpp b/src/arussim/include/arussim/plot_interface.hpp index 88d7ab5..254160d 100644 --- a/src/arussim/include/arussim/plot_interface.hpp +++ b/src/arussim/include/arussim/plot_interface.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -30,6 +31,7 @@ #include #include "arussim_msgs/msg/four_wheel_drive.hpp" #include "arussim_msgs/msg/state.hpp" +#include "common_msgs/msg/state.hpp" namespace plot_interface @@ -43,6 +45,18 @@ class PlotInterface : public rviz_common::Panel void onInitialize() override; + /** + * @brief Loads the panel settings from the .rviz file (CarStateTopic, TargetSpeedTopic). + * @param config Panel configuration. + */ + void load(const rviz_common::Config & config) override; + + /** + * @brief Saves the panel settings to the .rviz file (CarStateTopic, TargetSpeedTopic). + * @param config Panel configuration. + */ + void save(rviz_common::Config config) const override; + protected: std::shared_ptr node_ptr_; rclcpp::Publisher::SharedPtr publisher_; @@ -110,6 +124,20 @@ private Q_SLOTS: rclcpp::Subscription::SharedPtr reset_sub_; rclcpp::Subscription::SharedPtr torque_sub_; rclcpp::Subscription::SharedPtr state_sub_; + rclcpp::Subscription::SharedPtr car_state_sub_; + // Empty: ARUSSim ground truth (/arussim/state). In rosbag_simulation, /car_state/state2 + QString car_state_topic_; + QString target_speed_topic_ = "/controller/target_speed"; + + /** + * @brief (Re)creates the state subscription: car_state_topic_ if set, else /arussim/state. + */ + void subscribe_state(); + + /** + * @brief (Re)creates the target speed subscription with target_speed_topic_. + */ + void subscribe_target_speed(); rclcpp::Subscription::SharedPtr lap_time_sub_; rclcpp::Subscription::SharedPtr hit_cones_bool_sub_; rclcpp::Subscription::SharedPtr target_speed_sub_; diff --git a/src/arussim/src/plot_interface.cpp b/src/arussim/src/plot_interface.cpp index 8c11a17..8b443a3 100644 --- a/src/arussim/src/plot_interface.cpp +++ b/src/arussim/src/plot_interface.cpp @@ -215,23 +215,8 @@ void PlotInterface::onInitialize() } ); - state_sub_ = node->create_subscription( - "/arussim/state", 1, - [this](const arussim_msgs::msg::State::SharedPtr msg) { - QMetaObject::invokeMethod(this, [this, msg]() { - state_callback(msg->vx, msg->vy, msg->r, msg->ax, msg->ay, msg->delta); - }, Qt::QueuedConnection); - } - ); - - target_speed_sub_ = node->create_subscription( - "/controller/target_speed", 1, - [this](const std_msgs::msg::Float32::SharedPtr msg) { - QMetaObject::invokeMethod(this, [this, msg]() { - target_speed_ = msg->data; - }, Qt::QueuedConnection); - } - ); + subscribe_state(); + subscribe_target_speed(); reset_sub_ = node->create_subscription( "/arussim/reset", 1, @@ -628,6 +613,70 @@ void PlotInterface::update_telemetry_labels() delta_label_->setText("Delta: " + QString::number(delta_, 'f', 2)); } +void PlotInterface::subscribe_state() +{ + if (!node_ptr_) return; + rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); + state_sub_.reset(); + car_state_sub_.reset(); + if (car_state_topic_.isEmpty()) { + state_sub_ = node->create_subscription( + "/arussim/state", 1, + [this](const arussim_msgs::msg::State::SharedPtr msg) { + QMetaObject::invokeMethod(this, [this, msg]() { + state_callback(msg->vx, msg->vy, msg->r, msg->ax, msg->ay, msg->delta); + }, Qt::QueuedConnection); + } + ); + return; + } + // Without ARUSSim (rosbag_simulation): the car_state estimate + car_state_sub_ = node->create_subscription( + car_state_topic_.toStdString(), 1, + [this](const common_msgs::msg::State::SharedPtr msg) { + QMetaObject::invokeMethod(this, [this, msg]() { + state_callback(msg->vx, msg->vy, msg->r, msg->ax, msg->ay, msg->delta); + }, Qt::QueuedConnection); + } + ); +} + +void PlotInterface::subscribe_target_speed() +{ + if (!node_ptr_) return; + target_speed_sub_ = node_ptr_->get_raw_node()->create_subscription( + target_speed_topic_.toStdString(), 1, + [this](const std_msgs::msg::Float32::SharedPtr msg) { + QMetaObject::invokeMethod(this, [this, msg]() { + target_speed_ = msg->data; + }, Qt::QueuedConnection); + } + ); +} + +void PlotInterface::load(const rviz_common::Config & config) +{ + rviz_common::Panel::load(config); + QString topic; + if (config.mapGetString("CarStateTopic", &topic) && topic != car_state_topic_) { + car_state_topic_ = topic; + reset_callback(); + subscribe_state(); + } + if (config.mapGetString("TargetSpeedTopic", &topic) && !topic.isEmpty() && + topic != target_speed_topic_) { + target_speed_topic_ = topic; + subscribe_target_speed(); + } +} + +void PlotInterface::save(rviz_common::Config config) const +{ + rviz_common::Panel::save(config); + config.mapSetValue("CarStateTopic", car_state_topic_); + config.mapSetValue("TargetSpeedTopic", target_speed_topic_); +} + /** * @brief Callback for the reset button * From 844276004a613e662730d51b453fa47e387136ae Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Wed, 4 Nov 2026 19:08:57 +0100 Subject: [PATCH 6/7] chore: show the drawing scale of the slam covariance layer in the slam rviz --- src/arussim/config/arussim_slam_rviz_config.rviz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/arussim/config/arussim_slam_rviz_config.rviz b/src/arussim/config/arussim_slam_rviz_config.rviz index 90072f7..cf71d3d 100644 --- a/src/arussim/config/arussim_slam_rviz_config.rviz +++ b/src/arussim/config/arussim_slam_rviz_config.rviz @@ -256,7 +256,7 @@ Visualization Manager: Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: SLAM covariance (3 sigma) + Name: SLAM covariance (3 sigma x20) Namespaces: {} Topic: Depth: 5 From 212e0972bc550e1c64c7ad9ffc97452565227432 Mon Sep 17 00:00:00 2001 From: RubenValdayo Date: Wed, 4 Nov 2026 19:35:38 +0100 Subject: [PATCH 7/7] fix: redraw the plot panel with the car state when there are no torques --- src/arussim/include/arussim/plot_interface.hpp | 8 ++++++++ src/arussim/src/plot_interface.cpp | 11 +++++++++++ 2 files changed, 19 insertions(+) diff --git a/src/arussim/include/arussim/plot_interface.hpp b/src/arussim/include/arussim/plot_interface.hpp index 254160d..139cca0 100644 --- a/src/arussim/include/arussim/plot_interface.hpp +++ b/src/arussim/include/arussim/plot_interface.hpp @@ -138,6 +138,14 @@ private Q_SLOTS: * @brief (Re)creates the target speed subscription with target_speed_topic_. */ void subscribe_target_speed(); + + /** + * @brief Redraws the labels and plots at most every kRefreshPeriodMs. Without ARUSSim nothing + * publishes the torques, which is what redraws them otherwise. + */ + void refresh_from_car_state(); + qint64 last_refresh_ms_ = -1; + static constexpr qint64 kRefreshPeriodMs = 40; // 25 Hz rclcpp::Subscription::SharedPtr lap_time_sub_; rclcpp::Subscription::SharedPtr hit_cones_bool_sub_; rclcpp::Subscription::SharedPtr target_speed_sub_; diff --git a/src/arussim/src/plot_interface.cpp b/src/arussim/src/plot_interface.cpp index 8b443a3..6c4e5ff 100644 --- a/src/arussim/src/plot_interface.cpp +++ b/src/arussim/src/plot_interface.cpp @@ -636,11 +636,22 @@ void PlotInterface::subscribe_state() [this](const common_msgs::msg::State::SharedPtr msg) { QMetaObject::invokeMethod(this, [this, msg]() { state_callback(msg->vx, msg->vy, msg->r, msg->ax, msg->ay, msg->delta); + refresh_from_car_state(); }, Qt::QueuedConnection); } ); } +void PlotInterface::refresh_from_car_state() +{ + const qint64 now_ms = timer_.elapsed(); + if (last_refresh_ms_ >= 0 && now_ms - last_refresh_ms_ < kRefreshPeriodMs) return; + last_refresh_ms_ = now_ms; + update_telemetry_labels(); + update_vx_target_graph(); + update_gg_graph(); +} + void PlotInterface::subscribe_target_speed() { if (!node_ptr_) return;