From 6d008bc61eeb5779993ec5de78fc2c0d715395fb Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 29 Jan 2025 16:15:13 +0000 Subject: [PATCH 1/5] Fix rviz crash if start later Signed-off-by: mini-1235 --- nav2_bringup/launch/rviz_launch.py | 4 +- .../include/nav2_rviz_plugins/selector.hpp | 9 +-- nav2_rviz_plugins/src/selector.cpp | 61 +++++++++---------- nav2_rviz_plugins/src/utils.cpp | 6 +- 4 files changed, 37 insertions(+), 43 deletions(-) diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 425dac4edee..002a8554a96 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( 'namespace', - default_value='navigation', + default_value='', description=( 'Top-level namespace. The value will be used to replace the ' ' keyword on the rviz config file.' @@ -60,7 +60,7 @@ def generate_launch_description(): executable='rviz2', namespace=namespace, arguments=['-d', rviz_config_file], - output='screen', + output='log', parameters=[{'use_sim_time': use_sim_time}], remappings=[ ('/tf', 'tf'), 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..bb4b3e13d92 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,35 @@ 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.1); + 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; + } + 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)); } From 6e50a9f4ac8b45c8f8085b42d2bfb03d7167760b Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 29 Jan 2025 16:22:29 +0000 Subject: [PATCH 2/5] Bring back default rviz launch file Signed-off-by: mini-1235 --- nav2_bringup/launch/rviz_launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 002a8554a96..0e57c803ac1 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -36,7 +36,7 @@ def generate_launch_description(): # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( 'namespace', - default_value='', + default_value='navigation', description=( 'Top-level namespace. The value will be used to replace the ' ' keyword on the rviz config file.' @@ -60,7 +60,7 @@ def generate_launch_description(): executable='rviz2', namespace=namespace, arguments=['-d', rviz_config_file], - output='log', + output='screen', parameters=[{'use_sim_time': use_sim_time}], remappings=[ ('/tf', 'tf'), @@ -89,4 +89,4 @@ def generate_launch_description(): # Add other nodes and processes we need ld.add_action(exit_event_handler) - return ld + return ld \ No newline at end of file From b67d41f4ca867a5ef3f3b38dd6a18d59b66372e7 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 29 Jan 2025 16:25:57 +0000 Subject: [PATCH 3/5] New line Signed-off-by: mini-1235 --- nav2_bringup/launch/rviz_launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 0e57c803ac1..59fe618ddd2 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -89,4 +89,5 @@ def generate_launch_description(): # Add other nodes and processes we need ld.add_action(exit_event_handler) - return ld \ No newline at end of file + return ld + \ No newline at end of file From 8a08da54417be2a7e9a664cec800776d8dc8d021 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 29 Jan 2025 16:30:06 +0000 Subject: [PATCH 4/5] Whitespace Signed-off-by: mini-1235 --- nav2_bringup/launch/rviz_launch.py | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index 59fe618ddd2..425dac4edee 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -90,4 +90,3 @@ def generate_launch_description(): ld.add_action(exit_event_handler) return ld - \ No newline at end of file From c97b8e038694e7667aa04efe476aae50e0fb9eda Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Wed, 29 Jan 2025 18:47:06 +0000 Subject: [PATCH 5/5] Add log when failed Signed-off-by: mini-1235 --- nav2_rviz_plugins/src/selector.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp index bb4b3e13d92..086554573d4 100644 --- a/nav2_rviz_plugins/src/selector.cpp +++ b/nav2_rviz_plugins/src/selector.cpp @@ -150,7 +150,7 @@ void Selector::loadPlugins() { load_plugins_thread_ = std::thread([this]() { - rclcpp::Rate rate(0.1); + rclcpp::Rate rate(0.2); while (rclcpp::ok() && !plugins_loaded_) { RCLCPP_INFO(client_node_->get_logger(), "Trying to load plugins..."); nav2_rviz_plugins::pluginLoader( @@ -172,6 +172,8 @@ Selector::loadPlugins() progress_checker_->count() > 0) { plugins_loaded_ = true; + } else { + RCLCPP_INFO(client_node_->get_logger(), "Failed to load plugins. Retrying..."); } rate.sleep(); }