From 091237e6208784025151a16a2b864d8f96a41ea7 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 24 Jan 2025 02:42:06 +0800 Subject: [PATCH 1/2] Fix rviz crash issue if rviz is run later --- .../nav2_rviz_plugins/docking_panel.hpp | 1 - .../include/nav2_rviz_plugins/selector.hpp | 8 +-- .../include/nav2_rviz_plugins/utils.hpp | 2 +- nav2_rviz_plugins/src/docking_panel.cpp | 3 +- nav2_rviz_plugins/src/selector.cpp | 61 ++++++++----------- nav2_rviz_plugins/src/utils.cpp | 20 +----- 6 files changed, 34 insertions(+), 61 deletions(-) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp index fe4acfff350..d8300410f26 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp @@ -110,7 +110,6 @@ private Q_SLOTS: // Flags to indicate if the plugins have been loaded bool plugins_loaded_ = false; - bool server_failed_ = false; QVBoxLayout * main_layout_{nullptr}; QHBoxLayout * info_layout_{nullptr}; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp index 1f6596e9cc8..f6f02bf7187 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp @@ -43,8 +43,7 @@ class Selector : public rviz_common::Panel ~Selector(); private: - // The (non-spinning) client node used to invoke the action client - void timerEvent(QTimerEvent * event) override; + void loadPlugins(); rclcpp::Node::SharedPtr client_node_; rclcpp::Publisher::SharedPtr pub_controller_; @@ -54,9 +53,8 @@ class Selector : public rviz_common::Panel rclcpp::Publisher::SharedPtr pub_progress_checker_; rclcpp::TimerBase::SharedPtr rclcpp_timer_; - bool plugins_loaded_ = false; - bool server_failed_ = false; - bool tried_once_ = false; + std::thread load_plugins_thread_; + std::mutex combo_box_mutex; QBasicTimer timer_; QVBoxLayout * main_layout_; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp index 1f286060f3c..8c8b5fe8a35 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/utils.hpp @@ -35,7 +35,7 @@ namespace nav2_rviz_plugins * @param combo_box The combo box to add the loaded plugins to */ void pluginLoader( - rclcpp::Node::SharedPtr node, bool & server_failed, const std::string & server_name, + rclcpp::Node::SharedPtr node, const std::string & server_name, const std::string & plugin_type, QComboBox * combo_box); // Create label string from goal status msg diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 62304a825f0..89911f59e1b 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -245,9 +245,10 @@ DockingPanel::DockingPanel(QWidget * parent) [this] { // Load the plugins if not already loaded if (!plugins_loaded_) { + dock_type_->addItem("Default"); RCLCPP_INFO(client_node_->get_logger(), "Loading dock plugins"); nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "docking_server", "dock_plugins", dock_type_); + client_node_, "docking_server", "dock_plugins", dock_type_); plugins_loaded_ = true; } }); diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp index 102e123e271..cd406fec697 100644 --- a/nav2_rviz_plugins/src/selector.cpp +++ b/nav2_rviz_plugins/src/selector.cpp @@ -69,8 +69,15 @@ Selector::Selector(QWidget * parent) main_layout_->addLayout(row_3_label_layout_); main_layout_->addLayout(row_3_layout_); + controller_->addItem("Default"); + planner_->addItem("Default"); + goal_checker_->addItem("Default"); + smoother_->addItem("Default"); + progress_checker_->addItem("Default"); + + setLayout(main_layout_); - timer_.start(200, this); + loadPlugins(); connect( controller_, QOverload::of(&QComboBox::activated), this, @@ -95,16 +102,15 @@ Selector::Selector(QWidget * parent) Selector::~Selector() { + if (load_plugins_thread_.joinable()) { + load_plugins_thread_.join(); + } } // Publish the selected controller or planner void Selector::setSelection( QComboBox * combo_box, rclcpp::Publisher::SharedPtr publisher) { - // If "default" option is selected, it gets removed and the next item is selected - if (combo_box->findText("Default") != -1) { - combo_box->removeItem(0); - } // if there are no plugins available, return if (combo_box->count() == 0) { @@ -115,7 +121,6 @@ void Selector::setSelection( msg.data = combo_box->currentText().toStdString(); publisher->publish(msg); - timer_.start(200, this); } // Call setSelection() for controller @@ -148,37 +153,21 @@ void Selector::setProgressChecker() } void -Selector::timerEvent(QTimerEvent * event) +Selector::loadPlugins() { - if (event->timerId() == timer_.timerId()) { - if (!plugins_loaded_) { - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "controller_server", "controller_plugins", controller_); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "planner_server", "planner_plugins", planner_); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "controller_server", "goal_checker_plugins", goal_checker_); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "smoother_server", "smoother_plugins", smoother_); - nav2_rviz_plugins::pluginLoader( - client_node_, server_failed_, "controller_server", "progress_checker_plugins", - progress_checker_); - - plugins_loaded_ = true; - } - - // Restart the timer if the one of the server fails - if (server_failed_ && !tried_once_) { - RCLCPP_INFO(client_node_->get_logger(), "Retrying to connect to the failed server."); - server_failed_ = false; - plugins_loaded_ = false; - tried_once_ = true; - timer_.start(200, this); - return; - } - - timer_.stop(); - } + load_plugins_thread_ = std::thread([this]() { + nav2_rviz_plugins::pluginLoader( + client_node_, "controller_server", "controller_plugins", controller_); + nav2_rviz_plugins::pluginLoader( + client_node_, "planner_server", "planner_plugins", planner_); + nav2_rviz_plugins::pluginLoader( + client_node_, "controller_server", "goal_checker_plugins", goal_checker_); + nav2_rviz_plugins::pluginLoader( + client_node_, "smoother_server", "smoother_plugins", smoother_); + nav2_rviz_plugins::pluginLoader( + client_node_, "controller_server", "progress_checker_plugins", + progress_checker_); + }); } } // namespace nav2_rviz_plugins diff --git a/nav2_rviz_plugins/src/utils.cpp b/nav2_rviz_plugins/src/utils.cpp index 96cf7784bf1..fc3074e034a 100644 --- a/nav2_rviz_plugins/src/utils.cpp +++ b/nav2_rviz_plugins/src/utils.cpp @@ -21,36 +21,22 @@ namespace nav2_rviz_plugins { void pluginLoader( - rclcpp::Node::SharedPtr node, bool & server_failed, const std::string & server_name, + rclcpp::Node::SharedPtr node, const std::string & server_name, const std::string & plugin_type, QComboBox * combo_box) { + RCLCPP_ERROR(node->get_logger(), "Loading plugins for %s", plugin_type.c_str()); auto parameter_client = std::make_shared(node, server_name); - // Do not load the plugins if the combo box is already populated - if (combo_box->count() > 0) { - return; - } - // Wait for the service to be available before calling it - bool server_unavailable = false; while (!parameter_client->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting."); rclcpp::shutdown(); } - RCLCPP_INFO(node->get_logger(), "%s service not available", server_name.c_str()); - server_unavailable = true; - server_failed = true; - break; } - // Loading the plugins into the combo box - // If server unavaialble, let the combo box be empty - if (server_unavailable) { - return; - } - combo_box->addItem("Default"); auto parameters = parameter_client->get_parameters({plugin_type}); + RCLCPP_ERROR(node->get_logger(), "Loaded %s plugins", plugin_type.c_str()); auto str_arr = parameters[0].as_string_array(); for (auto str : str_arr) { combo_box->addItem(QString::fromStdString(str)); From 5fd86616f7c04ad545b5f473c1ba23762219b8ff Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 24 Jan 2025 02:58:05 +0800 Subject: [PATCH 2/2] Fix rviz crash issue if rviz is run later --- nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp | 1 - nav2_rviz_plugins/src/utils.cpp | 2 -- 2 files changed, 3 deletions(-) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp index f6f02bf7187..6ff842f1564 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp @@ -54,7 +54,6 @@ class Selector : public rviz_common::Panel rclcpp::TimerBase::SharedPtr rclcpp_timer_; std::thread load_plugins_thread_; - std::mutex combo_box_mutex; QBasicTimer timer_; QVBoxLayout * main_layout_; diff --git a/nav2_rviz_plugins/src/utils.cpp b/nav2_rviz_plugins/src/utils.cpp index fc3074e034a..cfb8ae6a503 100644 --- a/nav2_rviz_plugins/src/utils.cpp +++ b/nav2_rviz_plugins/src/utils.cpp @@ -24,7 +24,6 @@ void pluginLoader( rclcpp::Node::SharedPtr node, const std::string & server_name, const std::string & plugin_type, QComboBox * combo_box) { - RCLCPP_ERROR(node->get_logger(), "Loading plugins for %s", plugin_type.c_str()); auto parameter_client = std::make_shared(node, server_name); // Wait for the service to be available before calling it @@ -36,7 +35,6 @@ void pluginLoader( } auto parameters = parameter_client->get_parameters({plugin_type}); - RCLCPP_ERROR(node->get_logger(), "Loaded %s plugins", plugin_type.c_str()); auto str_arr = parameters[0].as_string_array(); for (auto str : str_arr) { combo_box->addItem(QString::fromStdString(str));