adding goal checker and smoother selectors to the plugins

Signed-off-by: PRP <[email protected]>

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<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::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<std_msgs::msg::String>::SharedPtr publisher);
 
 protected:
   QVBoxLayout * layout1 = new QVBoxLayout;

nav2_rviz_plugins/src/selector.cpp

@@ -29,17 +29,26 @@ Selector::Selector(QWidget * parent)
   pub_controller_ =
     client_node_->create_publisher<std_msgs::msg::String>("controller_selector", qos);
   pub_planner_ = client_node_->create_publisher<std_msgs::msg::String>("planner_selector", qos);
+  pub_goal_checker_ =
+    client_node_->create_publisher<std_msgs::msg::String>("goal_checker_selector", qos);
+  pub_smoother_ = client_node_->create_publisher<std_msgs::msg::String>("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);
 
   main_layout_->addWidget(new QLabel("Controller"));
   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<int>::of(&QComboBox::activated), this,
     &Selector::setPlanner);
+
+  connect(
+    goal_checker_, QOverload<int>::of(&QComboBox::activated), this,
+    &Selector::setGoalChecker);
+
+  connect(
+    smoother_, QOverload<int>::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<std_msgs::msg::String>::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<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(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();