diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp index 1f6596e9cc8..3e55aae4d1c 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp @@ -16,7 +16,6 @@ #define NAV2_RVIZ_PLUGINS__SELECTOR_HPP_ #include -#include #include #include #include @@ -43,8 +42,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_; @@ -52,13 +50,12 @@ class Selector : public rviz_common::Panel rclcpp::Publisher::SharedPtr pub_goal_checker_; rclcpp::Publisher::SharedPtr pub_smoother_; rclcpp::Publisher::SharedPtr pub_progress_checker_; - rclcpp::TimerBase::SharedPtr rclcpp_timer_; bool plugins_loaded_ = false; bool server_failed_ = false; - bool tried_once_ = false; - QBasicTimer timer_; + std::thread load_plugins_thread_; + QVBoxLayout * main_layout_; QHBoxLayout * row_1_layout_; QHBoxLayout * row_2_layout_; diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp index 102e123e271..086554573d4 100644 --- a/nav2_rviz_plugins/src/selector.cpp +++ b/nav2_rviz_plugins/src/selector.cpp @@ -70,7 +70,7 @@ Selector::Selector(QWidget * parent) main_layout_->addLayout(row_3_layout_); setLayout(main_layout_); - timer_.start(200, this); + loadPlugins(); connect( controller_, QOverload::of(&QComboBox::activated), this, @@ -115,7 +115,6 @@ void Selector::setSelection( msg.data = combo_box->currentText().toStdString(); publisher->publish(msg); - timer_.start(200, this); } // Call setSelection() for controller @@ -148,37 +147,37 @@ 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]() { + rclcpp::Rate rate(0.2); + while (rclcpp::ok() && !plugins_loaded_) { + RCLCPP_INFO(client_node_->get_logger(), "Trying to load plugins..."); + 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_); + if (controller_->count() > 0 && + planner_->count() > 0 && + goal_checker_->count() > 0 && + smoother_->count() > 0 && + progress_checker_->count() > 0) + { + plugins_loaded_ = true; + } else { + RCLCPP_INFO(client_node_->get_logger(), "Failed to load plugins. Retrying..."); + } + rate.sleep(); + } + }); } } // namespace nav2_rviz_plugins diff --git a/nav2_rviz_plugins/src/utils.cpp b/nav2_rviz_plugins/src/utils.cpp index 0053e404683..ee291c5c5e3 100644 --- a/nav2_rviz_plugins/src/utils.cpp +++ b/nav2_rviz_plugins/src/utils.cpp @@ -24,13 +24,13 @@ void pluginLoader( rclcpp::Node::SharedPtr node, bool & server_failed, 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; } + auto parameter_client = std::make_shared(node, server_name); + // Wait for the service to be available before calling it bool server_unavailable = false; while (!parameter_client->wait_for_service(std::chrono::seconds(1))) { @@ -49,9 +49,9 @@ void pluginLoader( if (server_unavailable) { return; } - combo_box->addItem("Default"); auto parameters = parameter_client->get_parameters({plugin_type}); auto str_arr = parameters[0].as_string_array(); + combo_box->addItem("Default"); for (auto str : str_arr) { combo_box->addItem(QString::fromStdString(str)); }