Homogenized node naming of BT nodes, functions and structure of demo

include/iri_ana_nav_module/iri_ana_nav_module_bt.h

@@ -241,11 +241,11 @@ class CIriAnaNavModuleBT
       * \return a BT:NodeStatus::SUCCESS always.
       *
       */
-    BT::NodeStatus sync_stop_nav();
+    BT::NodeStatus sync_stop();
 
-    BT::NodeStatus get_current_status_nav(BT::TreeNode& self);
+    BT::NodeStatus get_current_status(BT::TreeNode& self);
 
-    BT::NodeStatus read_3d_nav_status(BT::TreeNode& self);
+    BT::NodeStatus read_status(BT::TreeNode& self);
 
     /**
       * \brief Sets goal sent variables to false
@@ -279,7 +279,7 @@ class CIriAnaNavModuleBT
       * regardless of its success or not.
       *
       */
-    BT::NodeStatus is_finished_nav();
+    BT::NodeStatus is_finished();
     /**
       * \brief Checks if the last navigation goal has been reached successfully.
       *
@@ -291,7 +291,7 @@ class CIriAnaNavModuleBT
       * (BT:NodeStatus::FAILURE), regardless of how it has failed.
       *
       */
-    BT::NodeStatus is_succeded_nav();
+    BT::NodeStatus is_succeded();
     /**
       * \brief Checks if the module is active and operating properly.
       *
@@ -302,7 +302,7 @@ class CIriAnaNavModuleBT
       * operating properly (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
       *
       */
-    BT::NodeStatus is_running_nav();
+    BT::NodeStatus is_running();
     /**
       * \brief Checks if the goal could not be started.
       *
@@ -315,7 +315,7 @@ class CIriAnaNavModuleBT
       * (BT:NodeStatus::FAILURE).
       *
       */
-    BT::NodeStatus is_action_start_failed_nav();
+    BT::NodeStatus is_action_start_failed();
     /**
       * \brief Checks if the goal did not complete in the allowed time.
       *
@@ -328,7 +328,7 @@ class CIriAnaNavModuleBT
       * reason (BT:NodeStatus::FAILURE).
       *
       */
-    BT::NodeStatus is_timeout_nav();
+    BT::NodeStatus is_timeout();
     /**
       * \brief Checks if the feedback has not been received for a long time.
       *
@@ -341,7 +341,7 @@ class CIriAnaNavModuleBT
       * to another reason (BT:NodeStatus::FAILURE).
       *
       */
-    BT::NodeStatus is_watchdog_nav();
+    BT::NodeStatus is_watchdog();
     /**
       * \brief Checks if the navigation goal could not be reached.
       *
@@ -354,7 +354,7 @@ class CIriAnaNavModuleBT
       * reason (BT:NodeStatus::FAILURE).
       *
       */
-    BT::NodeStatus is_aborted_nav();
+    BT::NodeStatus is_aborted();
     /**
       * \brief Checks if the current navigation goal has been cancelled by
       * another goal.
@@ -368,7 +368,7 @@ class CIriAnaNavModuleBT
       * failed due to another reason (BT:NodeStatus::FAILURE).
       *
       */
-    BT::NodeStatus is_preempted_nav();
+    BT::NodeStatus is_preempted();
     /**
       * \brief Checks if the goal information is not valid.
       *
@@ -381,7 +381,7 @@ class CIriAnaNavModuleBT
       * (BT:NodeStatus::FAILURE).
       *
       */
-    BT::NodeStatus is_rejected_nav();
+    BT::NodeStatus is_rejected();
     /**
       * \brief Checks if it was impossible to set the value of a parameter.
       *
@@ -394,7 +394,7 @@ class CIriAnaNavModuleBT
       * another reason (BT:NodeStatus::FAILURE).
       *
       */
-    BT::NodeStatus is_set_param_failed_nav();
+    BT::NodeStatus is_set_param_failed();
     /**
       * \brief Checks if the parameter to be changed is not present in the
       * remote node.
@@ -408,7 +408,7 @@ class CIriAnaNavModuleBT
       * failed due to another reason (BT:NodeStatus::FAILURE).
       *
       */
-    BT::NodeStatus is_param_not_present_nav();
+    BT::NodeStatus is_param_not_present();
     /**
       * \brief Checks if no odometry information is being received.
       *
@@ -422,7 +422,7 @@ class CIriAnaNavModuleBT
       * reason (BT:NodeStatus::FAILURE).
       *
       */
-    BT::NodeStatus is_no_odom_nav();
+    BT::NodeStatus is_no_odom();
     /**
       * \brief Checks if there exists no transform between the desired goal
       * frame and the rest of the TF tree.
@@ -438,7 +438,7 @@ class CIriAnaNavModuleBT
       * (BT:NodeStatus::FAILURE).
       *
       */
-    BT::NodeStatus is_no_transform_nav();
+    BT::NodeStatus is_no_transform();
     // Other useful functions
     /**
       * \brief Set the goal reference frame
@@ -473,7 +473,7 @@ class CIriAnaNavModuleBT
       * \return a BT:NodeStatus::SUCCESS always.
       *
       */
-    BT::NodeStatus get_current_pose_nav(BT::TreeNode& self);
+    BT::NodeStatus get_current_pose(BT::TreeNode& self);
 
     // Costmaps functions
     /**
@@ -530,7 +530,7 @@ class CIriAnaNavModuleBT
       * is enabled (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
       *
       */
-    BT::NodeStatus costmap_is_auto_clear_enabled_nav();
+    BT::NodeStatus costmap_is_auto_clear_enabled();
 
   };
 

src/iri_ana_nav_module_bt.cpp

@@ -34,42 +34,42 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
   BT::PortsList status_text_port = { BT::OutputPort<std::string>("nav_status") };
 
   // registry of conditions
-  factory.registerSimpleCondition("is_finished_nav",  std::bind(&CIriAnaNavModuleBT::is_finished_nav, this));
-  factory.registerSimpleCondition("is_succeded_nav",  std::bind(&CIriAnaNavModuleBT::is_succeded_nav, this));
-  factory.registerSimpleCondition("is_running_nav",  std::bind(&CIriAnaNavModuleBT::is_running_nav, this));
-  factory.registerSimpleCondition("is_action_start_failed_nav",  std::bind(&CIriAnaNavModuleBT::is_action_start_failed_nav, this));
-  factory.registerSimpleCondition("is_timeout_nav",  std::bind(&CIriAnaNavModuleBT::is_timeout_nav, this));
-  factory.registerSimpleCondition("is_watchdog_nav",  std::bind(&CIriAnaNavModuleBT::is_watchdog_nav, this));
-  factory.registerSimpleCondition("is_aborted_nav",  std::bind(&CIriAnaNavModuleBT::is_aborted_nav, this));
-  factory.registerSimpleCondition("is_preempted_nav",  std::bind(&CIriAnaNavModuleBT::is_preempted_nav, this));
-  factory.registerSimpleCondition("is_rejected_nav",  std::bind(&CIriAnaNavModuleBT::is_rejected_nav, this));
-  factory.registerSimpleCondition("is_set_param_failed_nav",  std::bind(&CIriAnaNavModuleBT::is_set_param_failed_nav, this));
-  factory.registerSimpleCondition("is_param_not_present_nav",  std::bind(&CIriAnaNavModuleBT::is_param_not_present_nav, this));
-  factory.registerSimpleCondition("is_no_odom_nav",  std::bind(&CIriAnaNavModuleBT::is_no_odom_nav, this));
-  factory.registerSimpleCondition("is_no_transform_nav",  std::bind(&CIriAnaNavModuleBT::is_no_transform_nav, this));
-  factory.registerSimpleCondition("costmap_is_auto_clear_enabled_nav",  std::bind(&CIriAnaNavModuleBT::costmap_is_auto_clear_enabled_nav, this));
+  factory.registerSimpleCondition("is_finished_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_finished, this));
+  factory.registerSimpleCondition("is_succeded_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_succeded, this));
+  factory.registerSimpleCondition("is_running_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_running, this));
+  factory.registerSimpleCondition("is_action_start_failed_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_action_start_failed, this));
+  factory.registerSimpleCondition("is_timeout_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_timeout, this));
+  factory.registerSimpleCondition("is_watchdog_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_watchdog, this));
+  factory.registerSimpleCondition("is_aborted_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_aborted, this));
+  factory.registerSimpleCondition("is_preempted_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_preempted, this));
+  factory.registerSimpleCondition("is_rejected_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_rejected, this));
+  factory.registerSimpleCondition("is_set_param_failed_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_set_param_failed, this));
+  factory.registerSimpleCondition("is_param_not_present_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_param_not_present, this));
+  factory.registerSimpleCondition("is_no_odom_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_no_odom, this));
+  factory.registerSimpleCondition("is_no_transform_nav_3d",  std::bind(&CIriAnaNavModuleBT::is_no_transform, this));
+  factory.registerSimpleCondition("costmap_is_auto_clear_enabled_nav_3d",  std::bind(&CIriAnaNavModuleBT::costmap_is_auto_clear_enabled, this));
   // registry of synchronous actions
-  factory.registerSimpleAction("sync_go_to_orientation",  std::bind(&CIriAnaNavModuleBT::sync_go_to_orientation, this, std::placeholders::_1), sync_orientation_port);
-  factory.registerSimpleAction("sync_go_to_position",  std::bind(&CIriAnaNavModuleBT::sync_go_to_position, this, std::placeholders::_1), sync_position_port);
-  factory.registerSimpleAction("sync_go_to_pose",  std::bind(&CIriAnaNavModuleBT::sync_go_to_pose, this, std::placeholders::_1), sync_pose_port);
-  factory.registerSimpleAction("sync_stop_nav",  std::bind(&CIriAnaNavModuleBT::sync_stop_nav, this));
-  factory.registerSimpleAction("get_current_status_nav",std::bind(&CIriAnaNavModuleBT::get_current_status_nav, this,std::placeholders::_1), status_code_port);
-  factory.registerSimpleAction("set_goal_frame",  std::bind(&CIriAnaNavModuleBT::set_goal_frame, this, std::placeholders::_1), frame_port);
-  factory.registerSimpleAction("get_current_pose_nav",  std::bind(&CIriAnaNavModuleBT::get_current_pose_nav, this, std::placeholders::_1), current_pose_port);
-  factory.registerSimpleAction("costmaps_clear",  std::bind(&CIriAnaNavModuleBT::costmaps_clear, this));
-  factory.registerSimpleAction("costmaps_enable_auto_clear",  std::bind(&CIriAnaNavModuleBT::costmaps_enable_auto_clear, this, std::placeholders::_1), costmap_port);
-  factory.registerSimpleAction("costmaps_disable_auto_clear",  std::bind(&CIriAnaNavModuleBT::costmaps_disable_auto_clear, this));
-  factory.registerSimpleAction("reset_goal_variables",  std::bind(&CIriAnaNavModuleBT::reset_goal_variables, this));
+  factory.registerSimpleAction("sync_go_to_orientation_nav_3d",  std::bind(&CIriAnaNavModuleBT::sync_go_to_orientation, this, std::placeholders::_1), sync_orientation_port);
+  factory.registerSimpleAction("sync_go_to_position_nav_3d",  std::bind(&CIriAnaNavModuleBT::sync_go_to_position, this, std::placeholders::_1), sync_position_port);
+  factory.registerSimpleAction("sync_go_to_pose_nav_3d",  std::bind(&CIriAnaNavModuleBT::sync_go_to_pose, this, std::placeholders::_1), sync_pose_port);
+  factory.registerSimpleAction("sync_stop_nav_3d",  std::bind(&CIriAnaNavModuleBT::sync_stop, this));
+  factory.registerSimpleAction("get_current_status_nav_3d",std::bind(&CIriAnaNavModuleBT::get_current_status, this,std::placeholders::_1), status_code_port);
+  factory.registerSimpleAction("set_goal_frame_nav_3d",  std::bind(&CIriAnaNavModuleBT::set_goal_frame, this, std::placeholders::_1), frame_port);
+  factory.registerSimpleAction("get_current_pose_nav_3d",  std::bind(&CIriAnaNavModuleBT::get_current_pose, this, std::placeholders::_1), current_pose_port);
+  factory.registerSimpleAction("costmaps_clear_nav_3d",  std::bind(&CIriAnaNavModuleBT::costmaps_clear, this));
+  factory.registerSimpleAction("costmaps_enable_auto_clear_nav_3d",  std::bind(&CIriAnaNavModuleBT::costmaps_enable_auto_clear, this, std::placeholders::_1), costmap_port);
+  factory.registerSimpleAction("costmaps_disable_auto_clear_nav_3d",  std::bind(&CIriAnaNavModuleBT::costmaps_disable_auto_clear, this));
+  factory.registerSimpleAction("reset_goal_variables_nav_3d",  std::bind(&CIriAnaNavModuleBT::reset_goal_variables, this));
   // detect huge variation in path
-  factory.registerSimpleAction("get_current_path_distance",  std::bind(&CIriAnaNavModuleBT::get_current_path_distance, this, std::placeholders::_1), path_distance_port);
-  factory.registerSimpleAction("get_current_velocity",  std::bind(&CIriAnaNavModuleBT::get_current_velocity, this, std::placeholders::_1), velocity_port);
-  factory.registerSimpleAction("get_status_text",  std::bind(&CIriAnaNavModuleBT::get_status_text, this, std::placeholders::_1), status_text_port);
-  factory.registerSimpleAction("read_3d_nav_status",std::bind(&CIriAnaNavModuleBT::read_3d_nav_status, this,std::placeholders::_1), read_status_port);
+  factory.registerSimpleAction("get_current_path_distance_nav_3d",  std::bind(&CIriAnaNavModuleBT::get_current_path_distance, this, std::placeholders::_1), path_distance_port);
+  factory.registerSimpleAction("get_current_velocity_nav_3d",  std::bind(&CIriAnaNavModuleBT::get_current_velocity, this, std::placeholders::_1), velocity_port);
+  factory.registerSimpleAction("get_status_text_nav_3d",  std::bind(&CIriAnaNavModuleBT::get_status_text, this, std::placeholders::_1), status_text_port);
+  factory.registerSimpleAction("read_status_nav_3d",std::bind(&CIriAnaNavModuleBT::read_status, this,std::placeholders::_1), read_status_port);
 
   // registry of asynchronous actions
-  factory.registerIriAsyncAction("async_go_to_orientation",  std::bind(&CIriAnaNavModuleBT::async_go_to_orientation, this, std::placeholders::_1), async_orientation_port);
-  factory.registerIriAsyncAction("async_go_to_position",  std::bind(&CIriAnaNavModuleBT::async_go_to_position, this, std::placeholders::_1), async_position_port);
-  factory.registerIriAsyncAction("async_go_to_pose",  std::bind(&CIriAnaNavModuleBT::async_go_to_pose, this, std::placeholders::_1), async_pose_port);
+  factory.registerIriAsyncAction("async_go_to_orientation_nav_3d",  std::bind(&CIriAnaNavModuleBT::async_go_to_orientation, this, std::placeholders::_1), async_orientation_port);
+  factory.registerIriAsyncAction("async_go_to_position_nav_3d",  std::bind(&CIriAnaNavModuleBT::async_go_to_position, this, std::placeholders::_1), async_position_port);
+  factory.registerIriAsyncAction("async_go_to_pose_nav_3d",  std::bind(&CIriAnaNavModuleBT::async_go_to_pose, this, std::placeholders::_1), async_pose_port);
 }
 
 CIriAnaNavModuleBT::~CIriAnaNavModuleBT(void)
@@ -390,7 +390,7 @@ BT::NodeStatus CIriAnaNavModuleBT::async_go_to_pose(BT::TreeNode& self)
   else  return BT::NodeStatus::RUNNING;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::sync_stop_nav()
+BT::NodeStatus CIriAnaNavModuleBT::sync_stop()
 {
   ROS_INFO("CIriAnaNavModuleBT:sync_stop_nav -------STOP-------");
   nav.stop();
@@ -440,7 +440,7 @@ BT::NodeStatus CIriAnaNavModuleBT::get_status_text(BT::TreeNode& self)
   return BT::NodeStatus::SUCCESS;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::is_finished_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_finished()
 {
   if (nav.is_finished())
   {
@@ -452,7 +452,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_finished_nav()
   }
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::is_succeded_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_succeded()
 {
   iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -464,7 +464,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_succeded_nav()
   return BT::NodeStatus::FAILURE;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::is_running_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_running()
 {
   iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -476,7 +476,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_running_nav()
   return BT::NodeStatus::FAILURE;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::is_action_start_failed_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_action_start_failed()
 {
   iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -488,7 +488,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_action_start_failed_nav()
   return BT::NodeStatus::FAILURE;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::is_timeout_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_timeout()
 {
   iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -500,7 +500,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_timeout_nav()
   return BT::NodeStatus::FAILURE;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::is_watchdog_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_watchdog()
 {
   iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -512,7 +512,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_watchdog_nav()
   return BT::NodeStatus::FAILURE;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::is_aborted_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_aborted()
 {
   iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -525,7 +525,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_aborted_nav()
 }
 
 
-BT::NodeStatus CIriAnaNavModuleBT::is_preempted_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_preempted()
 {
   iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -537,7 +537,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_preempted_nav()
   return BT::NodeStatus::FAILURE;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::is_rejected_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_rejected()
 {
   iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -549,7 +549,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_rejected_nav()
   return BT::NodeStatus::FAILURE;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::is_set_param_failed_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_set_param_failed()
 {
   iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -561,7 +561,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_set_param_failed_nav()
   return BT::NodeStatus::FAILURE;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::is_param_not_present_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_param_not_present()
 {
   iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -573,7 +573,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_param_not_present_nav()
   return BT::NodeStatus::FAILURE;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::is_no_odom_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_no_odom()
 {
   iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -585,7 +585,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_no_odom_nav()
   return BT::NodeStatus::FAILURE;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::is_no_transform_nav()
+BT::NodeStatus CIriAnaNavModuleBT::is_no_transform()
 {
   iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -597,7 +597,7 @@ BT::NodeStatus CIriAnaNavModuleBT::is_no_transform_nav()
   return BT::NodeStatus::FAILURE;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::get_current_status_nav(BT::TreeNode& self)
+BT::NodeStatus CIriAnaNavModuleBT::get_current_status(BT::TreeNode& self)
 {
    iri_ana_nav_module_status_t nav_status = nav.get_status();
 
@@ -644,55 +644,55 @@ BT::NodeStatus CIriAnaNavModuleBT::get_current_status_nav(BT::TreeNode& self)
    return BT::NodeStatus::SUCCESS;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::read_3d_nav_status(BT::TreeNode& self)
+BT::NodeStatus CIriAnaNavModuleBT::read_status(BT::TreeNode& self)
 {
-  ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status");
+  ROS_INFO("CIriAnaNavModuleBT:read_status");
 
   BT::Optional<iri_ana_nav_module_status_t> status_input = self.getInput<iri_ana_nav_module_status_t>("nav_status");
 
   if (!status_input)
   {
-    ROS_ERROR("CIriAnaNavModuleBT:read_3d_nav_status Incorrect or missing required input [nav_status]");
+    ROS_ERROR("CIriAnaNavModuleBT:read_status Incorrect or missing required input [nav_status]");
     return BT::NodeStatus::FAILURE;
   }
 
   iri_ana_nav_module_status_t nav_status = status_input.value();
 
   if (nav_status == IRI_ANA_NAV_MODULE_PREEMPTED){
-    ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status ------- NAV_MODULE: PREEMPTED -------");
+    ROS_INFO("CIriAnaNavModuleBT:read_status ------- NAV_MODULE: PREEMPTED -------");
   }
   if (nav_status == IRI_ANA_NAV_MODULE_SUCCESS){
-    ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status ------- NAV_MODULE: SUCCEEDED -------");
+    ROS_INFO("CIriAnaNavModuleBT:read_status ------- NAV_MODULE: SUCCEEDED -------");
   }
   if (nav_status == IRI_ANA_NAV_MODULE_ABORTED){
-    ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status ------- NAV_MODULE: ABORTED -------");
+    ROS_INFO("CIriAnaNavModuleBT:read_status ------- NAV_MODULE: ABORTED -------");
   }
   if (nav_status == IRI_ANA_NAV_MODULE_REJECTED){
-    ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status ------- NAV_MODULE: REJECTED -------");
+    ROS_INFO("CIriAnaNavModuleBT:read_status ------- NAV_MODULE: REJECTED -------");
   }
   if (nav_status == IRI_ANA_NAV_MODULE_RUNNING){
-    ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status ------- NAV_MODULE: RUNNING -------");
+    ROS_INFO("CIriAnaNavModuleBT:read_status ------- NAV_MODULE: RUNNING -------");
   }
   if (nav_status == IRI_ANA_NAV_MODULE_ACTION_START_FAIL){
-    ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status ------- NAV_MODULE: ACTION_START_FAIL -------");
+    ROS_INFO("CIriAnaNavModuleBT:read_status ------- NAV_MODULE: ACTION_START_FAIL -------");
   }
   if (nav_status == IRI_ANA_NAV_MODULE_TIMEOUT){
-    ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status ------- NAV_MODULE: TIMEOUT -------");
+    ROS_INFO("CIriAnaNavModuleBT:read_status ------- NAV_MODULE: TIMEOUT -------");
   }
   if (nav_status == IRI_ANA_NAV_MODULE_FB_WATCHDOG){
-    ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status ------- NAV_MODULE: FB_WATCHDOG -------");
+    ROS_INFO("CIriAnaNavModuleBT:read_status ------- NAV_MODULE: FB_WATCHDOG -------");
   }
   if (nav_status == IRI_ANA_NAV_MODULE_SET_PARAM_FAIL){
-    ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status ------- NAV_MODULE: SET_PARAM_FAIL -------");
+    ROS_INFO("CIriAnaNavModuleBT:read_status ------- NAV_MODULE: SET_PARAM_FAIL -------");
   }
   if (nav_status == IRI_ANA_NAV_MODULE_PARAM_NOT_PRESENT){
-    ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status ------- NAV_MODULE: PARAM_NOT_PRESENT -------");
+    ROS_INFO("CIriAnaNavModuleBT:read_status ------- NAV_MODULE: PARAM_NOT_PRESENT -------");
   }
   if (nav_status == IRI_ANA_NAV_MODULE_NO_ODOM){
-    ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status ------- NAV_MODULE: NO_ODOM -------");
+    ROS_INFO("CIriAnaNavModuleBT:read_status ------- NAV_MODULE: NO_ODOM -------");
   }
   if (nav_status == IRI_ANA_NAV_MODULE_NO_TRANSFORM){
-    ROS_INFO("CIriAnaNavModuleBT:read_3d_nav_status ------- NAV_MODULE: NO_TRANSFORM -------");
+    ROS_INFO("CIriAnaNavModuleBT:read_status ------- NAV_MODULE: NO_TRANSFORM -------");
   }
 
   return BT::NodeStatus::SUCCESS;
@@ -717,7 +717,7 @@ BT::NodeStatus CIriAnaNavModuleBT::set_goal_frame(BT::TreeNode& self)
   return BT::NodeStatus::SUCCESS;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::get_current_pose_nav(BT::TreeNode& self)
+BT::NodeStatus CIriAnaNavModuleBT::get_current_pose(BT::TreeNode& self)
 {
   ROS_INFO("CIriAnaNavModuleBT:get_current_pose_nav");
   geometry_msgs::Pose current_pose = nav.getCurrentPose();
@@ -765,7 +765,7 @@ BT::NodeStatus CIriAnaNavModuleBT::costmaps_disable_auto_clear()
   return BT::NodeStatus::SUCCESS;
 }
 
-BT::NodeStatus CIriAnaNavModuleBT::costmap_is_auto_clear_enabled_nav()
+BT::NodeStatus CIriAnaNavModuleBT::costmap_is_auto_clear_enabled()
 {
   ROS_INFO("CIriAnaNavModuleBT:costmap_is_auto_clear_enabled_nav");
   if (!this->nav.costmap_is_auto_clear_enabled())

src/xml/nav_main_tree.xml

-<root main_tree_to_execute="NAV_MAIN">
+<root main_tree_to_execute="NAV_3D_MAIN">
 
-    <BehaviorTree ID="NAV_MAIN">
+    <BehaviorTree ID="NAV_3D_MAIN">
       <Fallback>
-        <SubTree ID="go_to_goal" x_goal="x_in" y_goal="y_in" yaw_goal="yaw_in" heading_tol_goal="heading_tol_in" x_y_pos_tol_goal="x_y_pos_tol_in" update_goal="update_goal_in" status="NAV_BT_status_out" frame_id="frame_id_in"/>
+        <SubTree ID="go_to_goal_nav_3d" x_goal="x_in" y_goal="y_in" yaw_goal="yaw_in" heading_tol_goal="heading_tol_in" x_y_pos_tol_goal="x_y_pos_tol_in" update_goal="update_goal_in" status="NAV_3D_BT_status_out" frame_id="frame_id_in"/>
         <ForceFailure>
           <Sequence>
-            <Action ID="get_current_status_nav" status_code="{NAV_BT_status_out}"/>
+            <Action ID="get_current_status_nav_3d" status_code="{NAV_3D_BT_status_out}"/>
             <!-- If navigation fails we stop the movement of the robot -->
-            <Action ID="sync_stop_nav"/>
+            <Action ID="sync_stop_nav_3d"/>
             <!-- We are not waiting until the navigation really stops -->
           </Sequence>
         </ForceFailure>
       </Fallback>
     </BehaviorTree>
 
-    <BehaviorTree ID="go_to_goal">
+    <BehaviorTree ID="go_to_goal_nav_3d">
       <Sequence>
-        <Action ID="set_goal_frame" frame_id="{frame_id}"/>
+        <Action ID="set_goal_frame_nav_3d" frame_id="{frame_id}"/>
         <Timeout msec="600000">
           <RetryUntilSuccesful num_attempts="3">
             <!-- Pending: choose type of navigation required (only position/orientation) -->
-            <Action ID="async_go_to_pose" x="{x_goal}" y="{y_goal}" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" x_y_pos_tol="{x_y_pos_tol_goal}" update_goal="{update_goal}"/>
+            <Action ID="async_go_to_pose_nav_3d" x="{x_goal}" y="{y_goal}" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" x_y_pos_tol="{x_y_pos_tol_goal}" update_goal="{update_goal}"/>
           </RetryUntilSuccesful>
         </Timeout>
-        <Condition ID="is_succeded_nav"/>
-        <Action ID="get_current_status_nav" status_code="{status}"/>
+        <Condition ID="is_succeded_nav_3d"/>
+        <Action ID="get_current_status_nav_3d" status_code="{status}"/>
       </Sequence>
     </BehaviorTree>
 

src/xml/nav_restart_tree.xml

-<root main_tree_to_execute="NAV_RESTART">
-    <BehaviorTree ID="NAV_RESTART">
+<root main_tree_to_execute="NAV_3D_RESTART">
+    <BehaviorTree ID="NAV_3D_RESTART">
       <SequenceStar>
-        <Action ID="sync_stop_nav"/>
+        <Action ID="sync_stop_nav_3d"/>
         <ReactiveFallback>
-          <Condition ID="is_finished_nav"/>
+          <Condition ID="is_finished_nav_3d"/>
           <Timeout msec="10000">
             <Action ID="NOP"/>
           </Timeout>
         </ReactiveFallback>
-        <Action ID="costmaps_clear"/>
-        <Action ID="reset_goal_variables"/>
+        <Action ID="costmaps_clear_nav_3d"/>
+        <Action ID="reset_goal_variables_nav_3d"/>
       </SequenceStar>
     </BehaviorTree>
 </root>

src/xml/nav_with_gui_main_tree.xml

 <?xml version="1.0"?>
-<root main_tree_to_execute="NAV_WITH_GUI_MAIN">
+<root main_tree_to_execute="NAV_3D_WITH_GUI_MAIN">
 
-    <BehaviorTree ID="NAV_WITH_GUI_MAIN">
+    <BehaviorTree ID="NAV_3D_WITH_GUI_MAIN">
       <Sequence>
         <Action ID="sync_start_nav_gui"/>
         <Fallback>
           <ReactiveSequence>
-            <Action ID="get_status_text" nav_status="{status_text}"/>
+            <Action ID="get_status_text_nav_3d" nav_status="{status_text}"/>
             <Action ID="set_nav_status_nav_gui" nav_status="{status_text}"/>
-            <Action ID="get_current_velocity" velocity="{velocity}"/>
+            <Action ID="get_current_velocity_nav_3d" velocity="{velocity}"/>
             <Action ID="set_current_velocity_nav_gui" velocity="{velocity}"/>
-            <Action ID="get_current_path_distance" path_distance="{path_distance}"/>
+            <Action ID="get_current_path_distance_nav_3d" path_distance="{path_distance}"/>
             <Action ID="set_distance_to_goal_nav_gui" distance_to_goal="{path_distance}"/>
-            <SubTree ID="go_to_goal" x_goal="x_in" y_goal="y_in" yaw_goal="yaw_in" heading_tol_goal="heading_tol_in" x_y_pos_tol_goal="x_y_pos_tol_in" update_goal="update_goal_in" status="NAV_BT_status_out" frame_id="frame_id_in"/>
+            <SubTree ID="go_to_goal_nav_3d" x_goal="x_in" y_goal="y_in" yaw_goal="yaw_in" heading_tol_goal="heading_tol_in" x_y_pos_tol_goal="x_y_pos_tol_in" update_goal="update_goal_in" status="NAV_3D_BT_status_out" frame_id="frame_id_in"/>
           </ReactiveSequence>
           <ForceFailure>
             <Sequence>
-              <Action ID="get_current_status_nav" status_code="{NAV_BT_status_out}"/>
+              <Action ID="get_current_status_nav_3d" status_code="{NAV_3D_BT_status_out}"/>
               <!-- If navigation fails we stop the movement of the robot -->
-              <Action ID="sync_stop_nav"/>
+              <Action ID="sync_stop_nav_3d"/>
               <!-- We are not waiting until the navigation really stops -->
             </Sequence>
           </ForceFailure>
@@ -26,17 +26,17 @@
       </Sequence>
     </BehaviorTree>
 
-    <BehaviorTree ID="go_to_goal">
+    <BehaviorTree ID="go_to_goal_nav_3d">
       <Sequence>
-        <Action ID="set_goal_frame" frame_id="{frame_id}"/>
+        <Action ID="set_goal_frame_nav_3d" frame_id="{frame_id}"/>
         <Timeout msec="600000">
           <RetryUntilSuccesful num_attempts="3">
             <!-- Pending: choose type of navigation required (only position/orientation) -->
-            <Action ID="async_go_to_pose" x="{x_goal}" y="{y_goal}" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" x_y_pos_tol="{x_y_pos_tol_goal}" update_goal="{update_goal}"/>
+            <Action ID="async_go_to_pose_nav_3d" x="{x_goal}" y="{y_goal}" yaw="{yaw_goal}" heading_tol="{heading_tol_goal}" x_y_pos_tol="{x_y_pos_tol_goal}" update_goal="{update_goal}"/>
           </RetryUntilSuccesful>
         </Timeout>
-        <Condition ID="is_succeded_nav"/>
-        <Action ID="get_current_status_nav" status_code="{status}"/>
+        <Condition ID="is_succeded_nav_3d"/>
+        <Action ID="get_current_status_nav_3d" status_code="{status}"/>
       </Sequence>
     </BehaviorTree>