update plan when docking

src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h

@@ -141,6 +141,13 @@ namespace robot_nav_core
      */ 
     virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
 
+
+    /**
+     * @brief Gets the current global plan
+     * @param path The global plan
+     */
+    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
+
     /**
      * @brief  Constructs the local planner
      * @param name The name to give this instance of the local planner

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h

@@ -99,6 +99,12 @@ namespace robot_nav_core2
      */ 
     virtual void getPlan(robot_nav_2d_msgs::Path2D &path) = 0;
 
+    /**
+     * @brief Gets the current global plan
+     * @param path The global plan
+     */
+    virtual void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) = 0;
+
     /**
      * @brief Compute the best command given the current pose, velocity and goal
      *

src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h

@@ -167,6 +167,12 @@ namespace robot_nav_core_adapter
      */ 
     virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) override;
 
+    /**
+     * @brief Gets the current global plan
+     * @param path The global plan
+     */
+    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) override;
+
     /**
      * @brief Create a new LocalPlannerAdapter
      * @return A shared pointer to the new LocalPlannerAdapter

src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp

@@ -407,6 +407,17 @@ namespace robot_nav_core_adapter
     path = robot_nav_2d_utils::pathToPath(path2d).poses;
   }
 
+  void LocalPlannerAdapter::getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path)
+  {
+    if (!planner_)
+    {
+      return;
+    }
+    robot_nav_2d_msgs::Path2D path2d;
+    planner_->getGlobalPlan(path2d);
+    path = robot_nav_2d_utils::pathToPath(path2d).poses;
+  }
+
   bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal)
   {
     if (last_goal_.header.frame_id != new_goal.header.frame_id ||

src/Navigations/Packages/move_base/src/move_base.cpp

@@ -3214,6 +3214,22 @@ robot::move_base_core::NavFeedback *move_base::MoveBase::getFeedback()
 
 robot::move_base_core::PlannerDataOutput move_base::MoveBase::getGlobalData()
 {
+  if (tc_)
+  {
+    robot_nav_msgs::Path path;
+    tc_->getGlobalPlan(path.poses);
+    if (!path.poses.empty())
+    {
+      path.header.stamp = path.poses[0].header.stamp;
+      path.header.frame_id = path.poses[0].header.frame_id;
+    }
+    else
+    {
+      path.header.stamp = robot::Time::now();
+      path.header.frame_id = planner_costmap_robot_->getGlobalFrameID();
+    }
+    global_data_.plan = robot_nav_2d_utils::pathToPath(path);
+  }
   ConvertData convert_data(planner_costmap_robot_, planner_costmap_robot_->getGlobalFrameID(), true);
   convert_data.updateCostmap(global_data_.costmap, global_data_.costmap_update, global_data_.is_costmap_updated);
   convert_data.updateFootprint(global_data_.footprint);