From bab973bb4283f2e06b85a3d741f058355b93ef71 Mon Sep 17 00:00:00 2001
From: corot <jsantossimon@gmail.com>
Date: Tue, 21 Nov 2023 13:36:42 +0900
Subject: [PATCH] Create 2 topics for current goal: controller and planner

---
 .../mbf_abstract_nav/abstract_controller_execution.h      | 5 -----
 .../include/mbf_abstract_nav/abstract_navigation_server.h | 3 ---
 .../include/mbf_abstract_nav/controller_action.h          | 2 ++
 .../include/mbf_abstract_nav/planner_action.h             | 2 +-
 mbf_abstract_nav/src/abstract_controller_execution.cpp    | 4 +---
 mbf_abstract_nav/src/abstract_navigation_server.cpp       | 8 ++------
 mbf_abstract_nav/src/controller_action.cpp                | 5 +++++
 mbf_abstract_nav/src/planner_action.cpp                   | 6 +++---
 .../mbf_costmap_nav/costmap_controller_execution.h        | 2 --
 .../src/mbf_costmap_nav/costmap_controller_execution.cpp  | 3 +--
 .../src/mbf_costmap_nav/costmap_navigation_server.cpp     | 2 +-
 11 files changed, 16 insertions(+), 26 deletions(-)

diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
index d6bd4308..c753ced2 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h
@@ -85,7 +85,6 @@ namespace mbf_abstract_nav
      * @param controller_ptr Pointer to the controller plugin
      * @param robot_info Current robot state
      * @param vel_pub Velocity publisher
-     * @param goal_pub Current goal publisher
      * @param config Initial configuration for this execution
      */
     AbstractControllerExecution(
@@ -93,7 +92,6 @@ namespace mbf_abstract_nav
         const mbf_abstract_core::AbstractController::Ptr &controller_ptr,
         const mbf_utility::RobotInformation &robot_info,
         const ros::Publisher &vel_pub,
-        const ros::Publisher &goal_pub,
         const MoveBaseFlexConfig &config);
 
     /**
@@ -330,9 +328,6 @@ namespace mbf_abstract_nav
     //! publisher for the current velocity command
     ros::Publisher vel_pub_;
 
-    //! publisher for the current goal
-    ros::Publisher current_goal_pub_;
-
     //! the current controller state
     AbstractControllerExecution::ControllerState state_;
 
diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h
index 70a767e9..3ed221a6 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h
@@ -342,9 +342,6 @@ typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBase
     //! cmd_vel publisher for all controller execution objects
     ros::Publisher vel_pub_;
 
-    //! current_goal publisher for all controller execution objects
-    ros::Publisher goal_pub_;
-
     //! current robot state
     mbf_utility::RobotInformation robot_info_;
 
diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h b/mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h
index bdb6718a..0046a1f0 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h
@@ -95,6 +95,8 @@ class ControllerAction :
   geometry_msgs::PoseStamped robot_pose_; ///< Current robot pose
   geometry_msgs::PoseStamped goal_pose_;  ///< Current goal pose
 
+  //! Publish the current goal pose (the last pose of the path we are following)
+  ros::Publisher goal_pub_;
 };
 }
 
diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/planner_action.h b/mbf_abstract_nav/include/mbf_abstract_nav/planner_action.h
index d22120ef..7f6cdf9c 100644
--- a/mbf_abstract_nav/include/mbf_abstract_nav/planner_action.h
+++ b/mbf_abstract_nav/include/mbf_abstract_nav/planner_action.h
@@ -80,7 +80,7 @@ class PlannerAction : public AbstractActionBase<mbf_msgs::GetPathAction, Abstrac
  private:
 
   //! Publisher to publish the current goal pose, which is used for path planning
-  ros::Publisher current_goal_pub_;
+  ros::Publisher goal_pub_;
 
   //! Path sequence counter
   unsigned int path_seq_count_;
diff --git a/mbf_abstract_nav/src/abstract_controller_execution.cpp b/mbf_abstract_nav/src/abstract_controller_execution.cpp
index 98f29e7e..3bd0689b 100644
--- a/mbf_abstract_nav/src/abstract_controller_execution.cpp
+++ b/mbf_abstract_nav/src/abstract_controller_execution.cpp
@@ -52,11 +52,10 @@ AbstractControllerExecution::AbstractControllerExecution(
     const mbf_abstract_core::AbstractController::Ptr &controller_ptr,
     const mbf_utility::RobotInformation &robot_info,
     const ros::Publisher &vel_pub,
-    const ros::Publisher &goal_pub,
     const MoveBaseFlexConfig &config) :
   AbstractExecutionBase(name, robot_info),
     controller_(controller_ptr),
-    state_(INITIALIZED), moving_(false), max_retries_(0), patience_(0), vel_pub_(vel_pub), current_goal_pub_(goal_pub),
+    state_(INITIALIZED), moving_(false), max_retries_(0), patience_(0), vel_pub_(vel_pub),
     loop_rate_(DEFAULT_CONTROLLER_FREQUENCY)
 {
   ros::NodeHandle nh;
@@ -375,7 +374,6 @@ void AbstractControllerExecution::run()
           condition_.notify_all();
           return;
         }
-        current_goal_pub_.publish(plan.back());
       }
 
       // compute robot pose and store it in robot_pose_
diff --git a/mbf_abstract_nav/src/abstract_navigation_server.cpp b/mbf_abstract_nav/src/abstract_navigation_server.cpp
index 8e470cb7..84142c61 100644
--- a/mbf_abstract_nav/src/abstract_navigation_server.cpp
+++ b/mbf_abstract_nav/src/abstract_navigation_server.cpp
@@ -66,12 +66,8 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr)
       recovery_action_(name_action_recovery, robot_info_),
       move_base_action_(name_action_move_base, robot_info_, recovery_plugin_manager_.getLoadedNames())
 {
-  ros::NodeHandle nh;
-
-  goal_pub_ = nh.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
-
   // init cmd_vel publisher for the robot velocity
-  vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+  vel_pub_ = ros::NodeHandle().advertise<geometry_msgs::Twist>("cmd_vel", 1);
 
   action_server_get_path_ptr_ = ActionServerGetPathPtr(
     new ActionServerGetPath(
@@ -323,7 +319,7 @@ mbf_abstract_nav::AbstractControllerExecution::Ptr AbstractNavigationServer::new
     const mbf_abstract_core::AbstractController::Ptr &plugin_ptr)
 {
   return boost::make_shared<mbf_abstract_nav::AbstractControllerExecution>(plugin_name, plugin_ptr, robot_info_,
-                                                                           vel_pub_, goal_pub_, last_config_);
+                                                                           vel_pub_, last_config_);
 }
 
 mbf_abstract_nav::AbstractRecoveryExecution::Ptr AbstractNavigationServer::newRecoveryExecution(
diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp
index 2614aff2..e2c3c5c3 100644
--- a/mbf_abstract_nav/src/controller_action.cpp
+++ b/mbf_abstract_nav/src/controller_action.cpp
@@ -48,6 +48,9 @@ ControllerAction::ControllerAction(
     const mbf_utility::RobotInformation &robot_info)
     : AbstractActionBase(action_name, robot_info)
 {
+  // informative topics: current navigation goal
+  ros::NodeHandle private_nh("~");
+  goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("controller_goal", 1);
 }
 
 void ControllerAction::start(
@@ -85,6 +88,7 @@ void ControllerAction::start(
                                 goal_handle.getGoal()->angle_tolerance);
       // Update also goal pose, so the feedback remains consistent
       goal_pose_ = goal_handle.getGoal()->path.poses.back();
+      goal_pub_.publish(goal_pose_);
       mbf_msgs::ExePathResult result;
       fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Goal preempted by a new plan", result);
       concurrency_slots_[slot].goal_handle.setCanceled(result, result.message);
@@ -146,6 +150,7 @@ void ControllerAction::runImpl(GoalHandle &goal_handle, AbstractControllerExecut
   }
 
   goal_pose_ = plan.back();
+  goal_pub_.publish(goal_pose_);
   ROS_DEBUG_STREAM_NAMED(name_, "Called action \""
       << name_ << "\" with plan:" << std::endl
       << "frame: \"" << goal.path.header.frame_id << "\" " << std::endl
diff --git a/mbf_abstract_nav/src/planner_action.cpp b/mbf_abstract_nav/src/planner_action.cpp
index ad575e99..b8453514 100644
--- a/mbf_abstract_nav/src/planner_action.cpp
+++ b/mbf_abstract_nav/src/planner_action.cpp
@@ -50,9 +50,9 @@ PlannerAction::PlannerAction(
     const mbf_utility::RobotInformation &robot_info)
   : AbstractActionBase(name, robot_info), path_seq_count_(0)
 {
-  ros::NodeHandle private_nh("~");
   // informative topics: current navigation goal
-  current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
+  ros::NodeHandle private_nh("~");
+  goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("planner_goal", 1);
 }
 
 void PlannerAction::runImpl(GoalHandle &goal_handle, AbstractPlannerExecution &execution)
@@ -67,7 +67,7 @@ void PlannerAction::runImpl(GoalHandle &goal_handle, AbstractPlannerExecution &e
 
   double tolerance = goal.tolerance;
   bool use_start_pose = goal.use_start_pose;
-  current_goal_pub_.publish(goal.target_pose);
+  goal_pub_.publish(goal.target_pose);
 
   bool planner_active = true;
 
diff --git a/mbf_costmap_nav/include/mbf_costmap_nav/costmap_controller_execution.h b/mbf_costmap_nav/include/mbf_costmap_nav/costmap_controller_execution.h
index 9dcfaae0..958f7fb6 100644
--- a/mbf_costmap_nav/include/mbf_costmap_nav/costmap_controller_execution.h
+++ b/mbf_costmap_nav/include/mbf_costmap_nav/costmap_controller_execution.h
@@ -67,7 +67,6 @@ class CostmapControllerExecution : public mbf_abstract_nav::AbstractControllerEx
    * @param controller_ptr Shared pointer to the plugin to use.
    * @param robot_info Current robot state
    * @param vel_pub Velocity commands publisher.
-   * @param goal_pub Goal pose publisher (just vor visualization).
    * @param costmap_ptr Shared pointer to the local costmap.
    * @param config Current server configuration (dynamic).
    */
@@ -76,7 +75,6 @@ class CostmapControllerExecution : public mbf_abstract_nav::AbstractControllerEx
       const mbf_costmap_core::CostmapController::Ptr &controller_ptr,
       const mbf_utility::RobotInformation &robot_info,
       const ros::Publisher &vel_pub,
-      const ros::Publisher &goal_pub,
       const CostmapWrapper::Ptr &costmap_ptr,
       const MoveBaseFlexConfig &config);
 
diff --git a/mbf_costmap_nav/src/mbf_costmap_nav/costmap_controller_execution.cpp b/mbf_costmap_nav/src/mbf_costmap_nav/costmap_controller_execution.cpp
index 6d1cf36f..fc945c98 100644
--- a/mbf_costmap_nav/src/mbf_costmap_nav/costmap_controller_execution.cpp
+++ b/mbf_costmap_nav/src/mbf_costmap_nav/costmap_controller_execution.cpp
@@ -47,10 +47,9 @@ CostmapControllerExecution::CostmapControllerExecution(
     const mbf_costmap_core::CostmapController::Ptr &controller_ptr,
     const mbf_utility::RobotInformation &robot_info,
     const ros::Publisher &vel_pub,
-    const ros::Publisher &goal_pub,
     const CostmapWrapper::Ptr &costmap_ptr,
     const MoveBaseFlexConfig &config)
-      : AbstractControllerExecution(controller_name, controller_ptr, robot_info, vel_pub, goal_pub, toAbstract(config)),
+      : AbstractControllerExecution(controller_name, controller_ptr, robot_info, vel_pub, toAbstract(config)),
         costmap_ptr_(costmap_ptr)
 {
   ros::NodeHandle private_nh("~");
diff --git a/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp b/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp
index 2d4bed06..8af46bf2 100644
--- a/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp
+++ b/mbf_costmap_nav/src/mbf_costmap_nav/costmap_navigation_server.cpp
@@ -236,7 +236,7 @@ mbf_abstract_nav::AbstractControllerExecution::Ptr CostmapNavigationServer::newC
       findWithDefault(controller_name_to_costmap_ptr_, plugin_name, local_costmap_ptr_);
   return boost::make_shared<mbf_costmap_nav::CostmapControllerExecution>(
       plugin_name, boost::static_pointer_cast<mbf_costmap_core::CostmapController>(plugin_ptr), robot_info_, vel_pub_,
-      goal_pub_, costmap_ptr, last_config_);
+      costmap_ptr, last_config_);
 }
 
 mbf_abstract_nav::AbstractRecoveryExecution::Ptr CostmapNavigationServer::newRecoveryExecution(