From 42561c5214bbc755bafc7300d47160aa3a69d8a4 Mon Sep 17 00:00:00 2001
From: PRP <padhupradheep@gmail.com>
Date: Thu, 8 Feb 2024 11:25:19 +0100
Subject: [PATCH] adding progress checker to the selector plugin

Signed-off-by: PRP <padhupradheep@gmail.com>
---
 .../include/nav2_rviz_plugins/selector.hpp    | 13 ++++-
 nav2_rviz_plugins/src/selector.cpp            | 55 ++++++++++++++-----
 2 files changed, 50 insertions(+), 18 deletions(-)

diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp
index edfcbef96e7..0301b893995 100644
--- a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp
+++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp
@@ -51,6 +51,7 @@ class Selector : public rviz_common::Panel
   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;
@@ -58,18 +59,24 @@ class Selector : public rviz_common::Panel
   bool tried_once_ = false;
 
   QBasicTimer timer_;
-  QHBoxLayout * main_layout_;
-  QVBoxLayout * left_layout_;
-  QVBoxLayout * right_layout_;
+  QVBoxLayout * main_layout_;
+  QHBoxLayout * row_1_layout_;
+  QHBoxLayout * row_2_layout_;
+  QHBoxLayout * row_3_layout_;
+  QHBoxLayout * row_1_label_layout_;
+  QHBoxLayout * row_2_label_layout_;
+  QHBoxLayout * row_3_label_layout_;
   QComboBox * controller_;
   QComboBox * planner_;
   QComboBox * goal_checker_;
   QComboBox * smoother_;
+  QComboBox * progress_checker_;
 
   void setController();
   void setPlanner();
   void setGoalChecker();
   void setSmoother();
+  void setProgressChecker();
 
   /*
     * @brief Load the avaialble plugins into the combo box
diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp
index cc394c0f13e..f0f3d4e3358 100644
--- a/nav2_rviz_plugins/src/selector.cpp
+++ b/nav2_rviz_plugins/src/selector.cpp
@@ -32,28 +32,41 @@ Selector::Selector(QWidget * parent)
   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 QHBoxLayout;
-  left_layout_ = new QVBoxLayout;
-  right_layout_ = new QVBoxLayout;
+  pub_progress_checker_ =
+    client_node_->create_publisher<std_msgs::msg::String>("progress_checker_selector", qos);
+
+  main_layout_ = new QVBoxLayout;
+  row_1_label_layout_ = new QHBoxLayout;
+  row_2_label_layout_ = new QHBoxLayout;
+  row_3_label_layout_ = new QHBoxLayout;
+  row_1_layout_ = new QHBoxLayout;
+  row_2_layout_ = new QHBoxLayout;
+  row_3_layout_ = new QHBoxLayout;
   controller_ = new QComboBox;
   planner_ = new QComboBox;
   goal_checker_ = new QComboBox;
   smoother_ = new QComboBox;
+  progress_checker_ = new QComboBox;
 
   main_layout_->setContentsMargins(10, 10, 10, 10);
 
-  left_layout_->addWidget(new QLabel("Controller"));
-  left_layout_->addWidget(controller_);
-  left_layout_->addWidget(new QLabel("Planner"));
-  left_layout_->addWidget(planner_);
-  right_layout_->addWidget(new QLabel("Goal Checker"));
-  right_layout_->addWidget(goal_checker_);
-  right_layout_->addWidget(new QLabel("Smoother"));
-  right_layout_->addWidget(smoother_);
-
-  main_layout_->addLayout(left_layout_);
-  main_layout_->addLayout(right_layout_);
+  row_1_label_layout_->addWidget(new QLabel("Controller"));
+  row_1_layout_->addWidget(controller_);
+  row_1_label_layout_->addWidget(new QLabel("Planner"));
+  row_1_layout_->addWidget(planner_);
+  row_2_label_layout_->addWidget(new QLabel("Goal Checker"));
+  row_2_layout_->addWidget(goal_checker_);
+  row_2_label_layout_->addWidget(new QLabel("Smoother"));
+  row_2_layout_->addWidget(smoother_);
+  row_3_label_layout_->addWidget(new QLabel("Progress Checker"));
+  row_3_layout_->addWidget(progress_checker_);
+
+  main_layout_->addLayout(row_1_label_layout_);
+  main_layout_->addLayout(row_1_layout_);
+  main_layout_->addLayout(row_2_label_layout_);
+  main_layout_->addLayout(row_2_layout_);
+  main_layout_->addLayout(row_3_label_layout_);
+  main_layout_->addLayout(row_3_layout_);
 
   setLayout(main_layout_);
   timer_.start(200, this);
@@ -73,6 +86,10 @@ Selector::Selector(QWidget * parent)
   connect(
     smoother_, QOverload<int>::of(&QComboBox::activated), this,
     &Selector::setSmoother);
+
+  connect(
+    progress_checker_, QOverload<int>::of(&QComboBox::activated), this,
+    &Selector::setProgressChecker);
 }
 
 Selector::~Selector()
@@ -124,6 +141,11 @@ void Selector::setSmoother()
   setSelection(smoother_, pub_smoother_);
 }
 
+void Selector::setProgressChecker()
+{
+  setSelection(progress_checker_, pub_progress_checker_);
+}
+
 // Load the available plugins into the combo box
 void Selector::pluginLoader(
   rclcpp::Node::SharedPtr node,
@@ -178,6 +200,9 @@ Selector::timerEvent(QTimerEvent * event)
       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_);
+      pluginLoader(
+        client_node_, "controller_server", "progress_checker_plugins",
+        progress_checker_);
 
       plugins_loaded_ = true;
     }