From 951549a835ed3e4efadbdadfb59e0a637a9ade3d Mon Sep 17 00:00:00 2001
From: PRP <padhupradheep@gmail.com>
Date: Fri, 2 Feb 2024 20:02:42 +0100
Subject: [PATCH] setting default in combox and removing after selected

Signed-off-by: PRP <padhupradheep@gmail.com>
---
 ...hrough_poses_w_replanning_and_recovery.xml |  4 ++--
 ...gate_to_pose_w_replanning_and_recovery.xml |  4 ++--
 .../include/nav2_rviz_plugins/selector.hpp    |  8 +++++--
 nav2_rviz_plugins/src/selector.cpp            | 22 ++++++++++++++-----
 4 files changed, 26 insertions(+), 12 deletions(-)

diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
index 170d18ba461..7a235ff0407 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml
@@ -13,7 +13,7 @@
           <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
             <ReactiveSequence>
               <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
-              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased" error_code_id="{compute_path_error_code}"/>
+              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
             </ReactiveSequence>
             <Sequence>
               <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
@@ -22,7 +22,7 @@
           </RecoveryNode>
         </RateController>
         <RecoveryNode number_of_retries="1" name="FollowPath">
-          <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
+          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
           <Sequence>
             <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
             <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
index ede9e7bb5b4..8cb0eb749ca 100644
--- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
+++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
@@ -8,11 +8,11 @@
   <BehaviorTree ID="MainTree">
     <RecoveryNode number_of_retries="6" name="NavigateRecovery">
       <PipelineSequence name="NavigateWithReplanning">
-        <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath2" topic_name="controller_selector"/>
+        <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
         <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
         <RateController hz="1.0">
           <RecoveryNode number_of_retries="1" name="ComputePathToPose">
-            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased" error_code_id="{compute_path_error_code}"/>
+            <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
             <Sequence>
               <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
               <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp
index d5821cc78db..42105e16683 100644
--- a/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp
+++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/selector.hpp
@@ -47,7 +47,6 @@ class Selector : public rviz_common::Panel
   void timerEvent(QTimerEvent * event) override;
 
   rclcpp::Node::SharedPtr client_node_;
-  rclcpp::Node::SharedPtr node_;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_controller_;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_planner_;
   rclcpp::TimerBase::SharedPtr rclcpp_timer_;
@@ -59,7 +58,6 @@ class Selector : public rviz_common::Panel
   QComboBox * controller_;
   QComboBox * planner_;
 
-  void start_ros_timer();
   void setController();
   void setPlanner();
 
@@ -76,6 +74,12 @@ class Selector : public rviz_common::Panel
     const std::string & plugin_type,
     QComboBox * combo_box);
 
+  /*
+    * @brief Remove the item from the combo box
+    * @param combo_box The combo box to remove the item from
+  */
+  void removeItem(QComboBox * combo_box);
+
 protected:
   QVBoxLayout * layout1 = new QVBoxLayout;
 };
diff --git a/nav2_rviz_plugins/src/selector.cpp b/nav2_rviz_plugins/src/selector.cpp
index eb6950ea4a1..667a91c7180 100644
--- a/nav2_rviz_plugins/src/selector.cpp
+++ b/nav2_rviz_plugins/src/selector.cpp
@@ -57,9 +57,20 @@ Selector::~Selector()
 {
 }
 
+// Function to remove the option "default" from the combo box
+void Selector::removeItem(QComboBox * combo_box)
+{
+  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_);
+
   std_msgs::msg::String msg;
   msg.data = controller_->currentText().toStdString();
 
@@ -70,6 +81,9 @@ void Selector::setController()
 // Publish the selected planner
 void Selector::setPlanner()
 {
+  // If "default" option is selected, it get's removed and the next item is selected
+  removeItem(planner_);
+
   std_msgs::msg::String msg;
   msg.data = planner_->currentText().toStdString();
 
@@ -97,11 +111,13 @@ void Selector::pluginLoader(
 
   // Loading the plugins into the combo box
   if (!plugins_loaded_) {
+    combo_box->addItem("Default");
     auto parameters = parameter_client->get_parameters({plugin_type});
     auto str_arr = parameters[0].as_string_array();
     for (auto str : str_arr) {
       combo_box->addItem(QString::fromStdString(str));
     }
+    combo_box->setCurrentText("Default");
   }
 }
 
@@ -118,12 +134,6 @@ Selector::timerEvent(QTimerEvent * event)
   }
 }
 
-void
-Selector::start_ros_timer()
-{
-  rclcpp::spin(node_);
-}
-
 }  // namespace nav2_rviz_plugins
 #include "pluginlib/class_list_macros.hpp"
 PLUGINLIB_EXPORT_CLASS(nav2_rviz_plugins::Selector, rviz_common::Panel)