diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp index c5339898337..a67f8bc16cf 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp @@ -49,6 +49,8 @@ class Selector : public rviz_common::Panel rclcpp::Node::SharedPtr client_node_; rclcpp::Publisher::SharedPtr pub_controller_; rclcpp::Publisher::SharedPtr pub_planner_; + rclcpp::Publisher::SharedPtr pub_goal_checker_; + rclcpp::Publisher::SharedPtr pub_smoother_; rclcpp::TimerBase::SharedPtr rclcpp_timer_; bool plugins_loaded_ = false; @@ -57,9 +59,13 @@ class Selector : public rviz_common::Panel QVBoxLayout * main_layout_; QComboBox * controller_; QComboBox * planner_; + QComboBox * goal_checker_; + QComboBox * smoother_; void setController(); void setPlanner(); + void setGoalChecker(); + void setSmoother(); /* * @brief Load the avaialble plugins into the combo box @@ -75,10 +81,13 @@ class Selector : public rviz_common::Panel QComboBox * combo_box); /* - * @brief Remove the item from the combo box - * @param combo_box The combo box to remove the item from + * @brief Set the selection from the combo box + * @param combo_box The combo box to set the selection for + * @param publisher Publish the selected plugin */ - void removeItem(QComboBox * combo_box); + void setSelection( + QComboBox * combo_box, + rclcpp::Publisher::SharedPtr publisher); protected: QVBoxLayout * layout1 = new QVBoxLayout; diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp index 6a1cea29ba7..613ee1aef19 100644 --- a/nav2_rviz_plugins/src/selector.cpp +++ b/nav2_rviz_plugins/src/selector.cpp @@ -29,10 +29,15 @@ Selector::Selector(QWidget * parent) pub_controller_ = client_node_->create_publisher("controller_selector", qos); pub_planner_ = client_node_->create_publisher("planner_selector", qos); + pub_goal_checker_ = + client_node_->create_publisher("goal_checker_selector", qos); + pub_smoother_ = client_node_->create_publisher("smoother_selector", qos); main_layout_ = new QVBoxLayout; controller_ = new QComboBox; planner_ = new QComboBox; + goal_checker_ = new QComboBox; + smoother_ = new QComboBox; main_layout_->setContentsMargins(10, 10, 10, 10); @@ -40,6 +45,10 @@ Selector::Selector(QWidget * parent) main_layout_->addWidget(controller_); main_layout_->addWidget(new QLabel("Planner")); main_layout_->addWidget(planner_); + main_layout_->addWidget(new QLabel("Goal Checker")); + main_layout_->addWidget(goal_checker_); + main_layout_->addWidget(new QLabel("Smoother")); + main_layout_->addWidget(smoother_); setLayout(main_layout_); timer_.start(200, this); @@ -51,44 +60,63 @@ Selector::Selector(QWidget * parent) connect( planner_, QOverload::of(&QComboBox::activated), this, &Selector::setPlanner); + + connect( + goal_checker_, QOverload::of(&QComboBox::activated), this, + &Selector::setGoalChecker); + + connect( + smoother_, QOverload::of(&QComboBox::activated), this, + &Selector::setSmoother); } Selector::~Selector() { } -// Function to remove the option "default" from the combo box -void Selector::removeItem(QComboBox * combo_box) +// 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); } -} -// Publish the selected controller -void Selector::setController() -{ - // If "default" option is selected, it get's removed and the next item is selected - removeItem(controller_); + // if there are no plugins available, return + if (combo_box->count() == 0) { + return; + } std_msgs::msg::String msg; - msg.data = controller_->currentText().toStdString(); + msg.data = combo_box->currentText().toStdString(); - pub_controller_->publish(msg); + publisher->publish(msg); timer_.start(200, this); } -// Publish the selected planner +// Call setSelection() for controller +void Selector::setController() +{ + setSelection(controller_, pub_controller_); +} + +// Call setSelection() for planner void Selector::setPlanner() { - // If "default" option is selected, it get's removed and the next item is selected - removeItem(planner_); + setSelection(planner_, pub_planner_); +} - std_msgs::msg::String msg; - msg.data = planner_->currentText().toStdString(); +// Call setSelection() for goal checker +void Selector::setGoalChecker() +{ + setSelection(goal_checker_, pub_goal_checker_); +} - pub_planner_->publish(msg); - timer_.start(200, this); +// Call setSelection() for smoother +void Selector::setSmoother() +{ + setSelection(smoother_, pub_smoother_); } // Load the available plugins into the combo box @@ -101,16 +129,25 @@ void Selector::pluginLoader( 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(1s)) { if (!rclcpp::ok()) { RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting."); rclcpp::shutdown(); } - RCLCPP_INFO(node->get_logger(), "service not available, waiting again..."); + RCLCPP_INFO( + node->get_logger(), + (server_name + " service not available").c_str()); + server_unavailable = true; + break; } // Loading the plugins into the combo box if (!plugins_loaded_) { + // 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(); @@ -128,6 +165,9 @@ Selector::timerEvent(QTimerEvent * event) if (!plugins_loaded_) { pluginLoader(client_node_, "controller_server", "controller_plugins", controller_); pluginLoader(client_node_, "planner_server", "planner_plugins", planner_); + pluginLoader(client_node_, "controller_server", "goal_checker_plugins", goal_checker_); + pluginLoader(client_node_, "smoother_server", "smoother_plugins", smoother_); + plugins_loaded_ = true; } timer_.stop();