Fix rviz crash if launch later (#4882)

* Fix rviz crash if start later Signed-off-by: mini-1235 <[email protected]> * Bring back default rviz launch file Signed-off-by: mini-1235 <[email protected]> * New line Signed-off-by: mini-1235 <[email protected]> * Whitespace Signed-off-by: mini-1235 <[email protected]> * Add log when failed Signed-off-by: mini-1235 <[email protected]> --------- Signed-off-by: mini-1235 <[email protected]> (cherry picked from commit 4192372)
ros-navigation · Jan 29, 2025 · 9e0bee5 · 9e0bee5
1 parent 9a5689e
commit 9e0bee5
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 41 deletions.
diff --git 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 <QtWidgets>
-#include <QBasicTimer>
 #include <QFrame>
 #include <QGridLayout>
 #include <QScrollArea>
@@ -43,22 +42,20 @@ 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<std_msgs::msg::String>::SharedPtr pub_controller_;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_planner_;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_goal_checker_;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_smoother_;
   rclcpp::Publisher<std_msgs::msg::String>::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
@@ -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<int>::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
@@ -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<rclcpp::SyncParametersClient>(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<rclcpp::SyncParametersClient>(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));
   }