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..6ff842f1564 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,7 @@ 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_; 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..cfb8ae6a503 100644 --- a/nav2_rviz_plugins/src/utils.cpp +++ b/nav2_rviz_plugins/src/utils.cpp @@ -21,35 +21,19 @@ 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) { 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}); auto str_arr = parameters[0].as_string_array(); for (auto str : str_arr) {