update

config/costmap_common_params.yaml

@@ -1,3 +1,10 @@
+position_planner_name: PNKXLocalPlanner
+docking_planner_name: PNKXDockingLocalPlanner
+go_straight_planner_name: PNKXGoStraightLocalPlanner
+rotate_planner_name: PNKXRotateLocalPlanner
+base_local_planner: LocalPlannerAdapter
+base_global_planner: CustomPlanner
+
 robot_base_frame: base_footprint
 transform_tolerance: 1.0
 obstacle_range: 3.0

config/custom_global_params.yaml

@@ -1,4 +1,3 @@
-base_global_planner: CustomPlanner
 CustomPlanner:
   library_path: libcustom_planner
   environment_type: XYThetaLattice

config/pnkx_local_planner_params.yaml

@@ -1,10 +1,4 @@
 
-position_planner_name: PNKXLocalPlanner
-docking_planner_name: PNKXDockingLocalPlanner
-go_straight_planner_name: PNKXGoStraightLocalPlanner
-rotate_planner_name: PNKXRotateLocalPlanner
-
-base_local_planner: LocalPlannerAdapter
 LocalPlannerAdapter:
   library_path: liblocal_planner_adapter
   yaw_goal_tolerance: 0.017  
@@ -92,7 +86,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
   # only when false:
   lookahead_dist: 0.5                       # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
   # only when true:
-  min_lookahead_dist: 1.0                     # The minimum lookahead distance (m) threshold. (default: 0.3)
+  min_lookahead_dist: 0.6                     # The minimum lookahead distance (m) threshold. (default: 0.3)
   max_lookahead_dist: 2.0                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
   lookahead_time: 1.6                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
   min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 
@@ -120,6 +114,20 @@ MKTAlgorithmDiffPredictiveTrajectory:
   cost_scaling_dist: 0.2                             # (default: 0.6)
   cost_scaling_gain: 2.0                             # (default: 1.0)
 
+
+  use_mpc: true
+  mpc_horizon: 10
+  mpc_dt: 0.1
+  mpc_w_pos: 6.0
+  mpc_w_theta: 2.0
+  mpc_w_v: 0.2
+  mpc_w_w: 0.2
+  mpc_w_dv: 0.4
+  mpc_w_dw: 0.4
+  mpc_iterations: 3
+  mpc_step: 0.15
+  mpc_eps: 0.001
+
 MKTAlgorithmDiffGoStraight:
   library_path: libmkt_algorithm_diff
   avoid_obstacles: false
@@ -133,7 +141,7 @@ MKTAlgorithmDiffGoStraight:
   # only when false:
   lookahead_dist: 0.5                        # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
   # only when true:
-  min_lookahead_dist: 0.8                    # The minimum lookahead distance (m) threshold. (default: 0.3)
+  min_lookahead_dist: 1.0                    # The minimum lookahead distance (m) threshold. (default: 0.3)
   max_lookahead_dist: 1.5                     # The maximum lookahead distance (m) threshold. (default: 0.9)   
   lookahead_time: 2.5                         # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
   min_journey_squared: 0.3                    # Minimum squared journey to consider for goal (default: 0.2) 

config/two_points_global_params.yaml

@@ -1,4 +1,3 @@
-base_global_planner: TwoPointsPlanner
 TwoPointsPlanner:
   library_path: libtwo_points_planner
   lethal_obstacle: 20

examples/CSharpExample.cs

@@ -53,6 +53,14 @@ namespace NavigationExample
             public double theta;
         }
 
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Twist2D
+        {
+            public double x;
+            public double y;
+            public double theta;
+        }
+
         [StructLayout(LayoutKind.Sequential)]
         public struct Quaternion
         {
@@ -93,6 +101,13 @@ namespace NavigationExample
             public Pose pose;
         }
 
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Twist2DStamped
+        {
+            public Header header;
+            public Twist2D velocity;
+        }
+
         [StructLayout(LayoutKind.Sequential)]
         public struct Vector3
         {
@@ -180,6 +195,14 @@ namespace NavigationExample
         public static extern bool navigation_set_robot_footprint(
             IntPtr handle, Point[] points, UIntPtr point_count);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_footprint(
+            IntPtr handle, out IntPtr out_points, out UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void navigation_free_points(IntPtr points);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_move_to(
@@ -231,6 +254,11 @@ namespace NavigationExample
         public static extern bool navigation_get_robot_pose_2d(
             IntPtr handle, ref Pose2D out_pose);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_twist(
+            IntPtr handle, ref Twist2DStamped out_twist);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_get_feedback(

examples/NavigationExample/Program.cs

@@ -53,6 +53,14 @@ namespace NavigationExample
             public double theta;
         }
 
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Twist2D
+        {
+            public double x;
+            public double y;
+            public double theta;
+        }
+
         [StructLayout(LayoutKind.Sequential)]
         public struct Quaternion
         {
@@ -93,6 +101,13 @@ namespace NavigationExample
             public Pose pose;
         }
 
+        [StructLayout(LayoutKind.Sequential)]
+        public struct Twist2DStamped
+        {
+            public Header header;
+            public Twist2D velocity;
+        }
+
         [StructLayout(LayoutKind.Sequential)]
         public struct Vector3
         {
@@ -180,6 +195,14 @@ namespace NavigationExample
         public static extern bool navigation_set_robot_footprint(
             IntPtr handle, Point[] points, UIntPtr point_count);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_robot_footprint(
+            IntPtr handle, out IntPtr out_points, out UIntPtr out_count);
+
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        public static extern void navigation_free_points(IntPtr points);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_move_to(
@@ -231,6 +254,11 @@ namespace NavigationExample
         public static extern bool navigation_get_robot_pose_2d(
             IntPtr handle, ref Pose2D out_pose);
 
+        [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
+        [return: MarshalAs(UnmanagedType.I1)]
+        public static extern bool navigation_get_twist(
+            IntPtr handle, ref Twist2DStamped out_twist);
+
         [DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
         [return: MarshalAs(UnmanagedType.I1)]
         public static extern bool navigation_get_feedback(

src/APIs/c_api/include/nav_c_api.h

@@ -59,6 +59,15 @@ typedef struct {
     double theta;
 } Pose2D;
 
+/**
+ * @brief Twist2D structure (x, y, theta velocities)
+ */
+typedef struct {
+    double x;
+    double y;
+    double theta;
+} Twist2D;
+
 /**
  * @brief Quaternion structure
  */
@@ -104,6 +113,14 @@ typedef struct {
     Pose pose;
 } PoseStamped;
 
+/**
+ * @brief Twist2DStamped structure
+ */
+typedef struct {
+    Header header;
+    Twist2D velocity;
+} Twist2DStamped;
+
 /**
  * @brief Vector3 structure
  */
@@ -250,6 +267,21 @@ bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
  */
 bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count);
 
+/**
+ * @brief Get the robot's footprint (outline shape)
+ * @param handle Navigation handle
+ * @param out_points Output array of points (allocated by library, free with navigation_free_points)
+ * @param out_count Output number of points in the array
+ * @return true on success, false on failure
+ */
+bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count);
+
+/**
+ * @brief Free a points array allocated by navigation_get_robot_footprint
+ * @param points Pointer to point array
+ */
+void navigation_free_points(Point* points);
+
 /**
  * @brief Send a goal for the robot to navigate to
  * @param handle Navigation handle
@@ -350,6 +382,15 @@ bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out
  */
 bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose);
 
+/**
+ * @brief Get the robot's current twist
+ * @param handle Navigation handle
+ * @param out_twist Output parameter with the robot's current twist
+ * @return true if twist was successfully retrieved
+ * @note out_twist->header.frame_id must be freed using nav_c_api_free_string
+ */
+bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist);
+
 /**
  * @brief Get navigation feedback
  * @param handle Navigation handle

src/APIs/c_api/src/nav_c_api.cpp

@@ -6,12 +6,14 @@
 #include <robot_geometry_msgs/Pose2D.h>
 #include <robot_geometry_msgs/Point.h>
 #include <robot_geometry_msgs/Vector3.h>
+#include <robot_std_msgs/Header.h>
+#include <robot_nav_2d_msgs/Twist2DStamped.h>
 #include <tf3/buffer_core.h>
 #include <string>
 #include <vector>
 #include <cstring>
 #include <cstdlib>
-#include <boost/dll/import.hpp>
+#include <stdexcept>
 #include <robot/robot.h>
 
 
@@ -65,6 +67,25 @@ namespace {
         c_pose->theta = cpp_pose.theta;
     }
 
+    void cpp_to_c_header(const robot_std_msgs::Header& cpp_header, Header* c_header) {
+        c_header->seq = cpp_header.seq;
+        c_header->sec = cpp_header.stamp.sec;
+        c_header->nsec = cpp_header.stamp.nsec;
+        if (!cpp_header.frame_id.empty()) {
+            c_header->frame_id = strdup(cpp_header.frame_id.c_str());
+        } else {
+            c_header->frame_id = nullptr;
+        }
+    }
+
+    void cpp_to_c_twist2d_stamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist,
+                                  Twist2DStamped* c_twist) {
+        cpp_to_c_header(cpp_twist.header, &c_twist->header);
+        c_twist->velocity.x = cpp_twist.velocity.x;
+        c_twist->velocity.y = cpp_twist.velocity.y;
+        c_twist->velocity.theta = cpp_twist.velocity.theta;
+    }
+
     // Convert C++ NavFeedback to C NavFeedback
     void cpp_to_c_nav_feedback(const robot::move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) {
         c_feedback->navigation_state = static_cast<NavigationState>(static_cast<int>(cpp_feedback.navigation_state));
@@ -226,22 +247,13 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle
     }
     try {
         printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__);
-        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
         auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
-        robot::PluginLoaderHelper loader;
+        if (!tf_ptr || !(*tf_ptr)) {
-        std::string lib_path = loader.findLibraryPath("MoveBase");
+            printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__);
-        auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
-            lib_path,
-            "MoveBase", 
-            boost::dll::load_mode::append_decorations);
-            
-        robot::move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
-        printf("[%s:%d]\n Navigation created\n", __FILE__, __LINE__);
-        if (nav_ptr == nullptr) {
-            printf("[%s:%d]\n Error: Failed to create navigation\n", __FILE__, __LINE__);
             return false;
         }
-        nav_ptr->initialize(*tf_ptr);
+        nav->initialize(*tf_ptr);
     
         return true;    
     } catch (const std::exception &e) {
@@ -276,6 +288,45 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
     }
 }
 
+extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count) {
+    if (!handle || !out_points || !out_count) {
+        return false;
+    }
+
+    try {
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        std::vector<robot_geometry_msgs::Point> footprint = nav->getRobotFootprint();
+        if (footprint.empty()) {
+            *out_points = nullptr;
+            *out_count = 0;
+            return true;
+        }
+
+        Point* points = static_cast<Point*>(malloc(sizeof(Point) * footprint.size()));
+        if (!points) {
+            return false;
+        }
+
+        for (size_t i = 0; i < footprint.size(); ++i) {
+            points[i].x = footprint[i].x;
+            points[i].y = footprint[i].y;
+            points[i].z = footprint[i].z;
+        }
+
+        *out_points = points;
+        *out_count = footprint.size();
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+extern "C" void navigation_free_points(Point* points) {
+    if (points) {
+        free(points);
+    }
+}
+
 extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
                                     double xy_goal_tolerance, double yaw_goal_tolerance) {
     if (!handle || !goal) {
@@ -442,6 +493,21 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* ou
     }
 }
 
+extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist) {
+    if (!handle || !out_twist) {
+        return false;
+    }
+
+    try {
+        robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
+        robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist();
+        cpp_to_c_twist2d_stamped(cpp_twist, out_twist);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
 extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback) {
     if (!handle || !out_feedback) {
         return false;

src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt

@@ -84,6 +84,7 @@ add_library(${PROJECT_NAME}_diff SHARED
   src/diff/diff_predictive_trajectory.cpp
   src/diff/diff_rotate_to_goal.cpp
   src/diff/diff_go_straight.cpp
+  # src/diff/pure_pursuit.cpp
 )
 
 if(BUILDING_WITH_CATKIN)

src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h

@@ -0,0 +1,50 @@
+#ifndef _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__
+#define _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__
+
+#include <robot/robot.h>
+#include <score_algorithm/score_algorithm.h>
+
+#include <boost/dll/alias.hpp>
+#include <robot_geometry_msgs/PoseStamped.h>
+#include <robot_geometry_msgs/PointStamped.h>
+#include <robot_nav_2d_msgs/Pose2DStamped.h>
+#include <robot_nav_core2/exceptions.h>
+#include <robot_nav_core2/costmap.h>
+#include <nav_grid/coordinate_conversion.h>
+#include <angles/angles.h>
+
+namespace mkt_algorithm
+{
+  namespace diff
+  {
+    class PurePursuit : public score_algorithm::ScoreAlgorithm
+    {
+    public:
+      PurePursuit();
+      ~PurePursuit();
+      void computePurePursuit(
+        const double &velocity_min, 
+        const double &velocity_max,
+        const double &linear_vel,
+        const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
+        const double &min_dist2,
+        const double &curvature_dist2_floor,
+        const double &dist2_override);
+        
+    private:
+      void applyConstraints(const double &dist_error, const double &lookahead_dist,
+                            const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
+                            const double &pose_cost, double &linear_vel, double &sign_x);
+
+      double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
+                                                      const double &journey_plan);
+
+      double computeDecelerationFactor(const double &effective_journey, const double &d_reduce);
+
+      bool detectWobbleByCarrotAngle(std::vector<double> &angle_history, const double &carrot_angle,
+                                     const double &amplitude_threshold, const size_t &window_size);
+    };
+  }
+}
+
+#endif  

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp

@@ -107,7 +107,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   robot_nav_2d_msgs::Twist2D twist;
   traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom
   robot::Rate r(50);
-  while (traj_->hasMoreTwists())
+  while (robot::ok() && traj_->hasMoreTwists())
   {
     twist = traj_->nextTwist();
     // ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta);
@@ -184,6 +184,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   double v_min = 
     journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
   v_min *= sign_x;
+  
   double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
   double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
   journey_plan += max_path_distance_;

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp

@@ -391,15 +391,25 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   robot_nav_2d_msgs::Twist2D drive_cmd;
   double sign_x = sign(x_direction_);
   robot_nav_2d_msgs::Twist2D twist;
+  robot::log_info("[%s:%d] velocity in predictive trajectory: %f %f %f", __FILE__, __LINE__, velocity.x, velocity.y, velocity.theta);
   traj_->startNewIteration(velocity);
-  while (traj_->hasMoreTwists())
+  while (robot::ok() && traj_->hasMoreTwists())
   {
     twist = traj_->nextTwist();
   }
   drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5);
+  robot::log_info("[%s:%d] twist: %f %f %f", __FILE__, __LINE__, twist.x, twist.y, twist.theta);
+  robot::log_info("--------------------------------");
   double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
 
   robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
+  result.poses.clear();
+  result.poses.reserve(transformed_plan.poses.size());
+  for (const auto &pose_stamped : transformed_plan.poses)
+  {
+    result.poses.push_back(pose_stamped.pose);
+  }
+  
   if (transformed_plan.poses.empty())
   {
     robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);

src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp

@@ -0,0 +1,52 @@
+#include <mkt_algorithm/diff/pure_pursuit.h>
+
+void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity_min, 
+                                                          const double &velocity_max,
+                                                          const double &linear_vel,
+                                                          const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
+                                                          const double &min_dist2,
+                                                          const double &curvature_dist2_floor,
+                                                          const double &dist2_override)
+{
+  const double vel_x_reduce = std::min(fabs(velocity_max), 0.3);
+  double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
+  carrot_dist2 = std::max(carrot_dist2, 0.05);
+  double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
+
+  if (max_lateral_accel_ > 1e-6)
+  {
+    const double curvature_abs = std::max(fabs(curvature), 1e-6);
+    const double v_lateral_limit = sqrt(max_lateral_accel_ / curvature_abs);
+    drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit);
+  }
+
+  const auto &plan_back_pose = compute_plan_.poses.back();
+  double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y);
+  post_cost = std::max(post_cost, center_cost_);
+  this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
+
+  const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
+  const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
+  double d_reduce = std::clamp(journey_plan, min_S, max_S);
+  double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
+  double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
+  double v_min =
+      journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
+  v_min *= sign_x;
+
+  double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
+  double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
+  double vel_reduce = sign_x > 0
+                          ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
+                          : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
+  drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce;
+
+  double v_theta = drive_cmd.x * curvature;
+  double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
+  if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_))
+  {
+    carrot_dist2 *= 0.6;
+    curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
+    v_theta = drive_cmd.x * curvature;
+  }
+}

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h

@@ -270,6 +270,23 @@ namespace mkt_plugins
     using Ptr = std::shared_ptr<KinematicParameters>;
 
   protected:
+    
+    /**
+     * @brief Configure the kinematic parameters
+     * @param nh NodeHandle
+     *
+     * Configures the kinematic parameters from the given NodeHandle.
+     */
+    void configure(const robot::NodeHandle &nh);
+
+    /**
+     * @brief Reconfigure the kinematic parameters
+     * @param nh NodeHandle
+     *
+     * Reconfigures the kinematic parameters from the given NodeHandle.
+     */
+    void reconfigure(const robot::NodeHandle &nh);
+  
     // For parameter descriptions, see cfg/KinematicParams.cfg
     int xytheta_direct_[3];
     double min_vel_x_, min_vel_y_;

src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h

@@ -116,9 +116,13 @@ namespace mkt_plugins
      * current velocity within acc_time seconds, respecting acceleration/deceleration limits.
      * The velocities range from min_vel_ to max_vel_ (projected from current velocity).
      */
-    OneDVelocityIterator(double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time,
+    OneDVelocityIterator(
-                         int num_samples)
+      std::string name, double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time, int num_samples)
+     : name_(name)
     {
+      if(name_ == "x_it_")
+        robot::log_warning("[%s:%d] one_d_velocity_iterator: [%s] %f %d %f %f %f %f %f %d", 
+                           __FILE__, __LINE__, name_.c_str(), current, direct, min, max, acc_limit, decel_limit, acc_time, num_samples);
       // if (current < min)
       // {
       //   current = min;
@@ -203,8 +207,6 @@ namespace mkt_plugins
      */
     bool isFinished() const
     {
-      // if(name_ == std::string("th_it_"))
-      //   ROS_INFO("%s %f %f", name_.c_str(), current_, max_vel_ + EPSILON);
       double limit_vel = direct_ > 0? max_vel_ : min_vel_;
       return fabs(current_) > fabs(limit_vel) + EPSILON;
     }

src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp

@@ -54,10 +54,48 @@ namespace mkt_plugins
     setDecelerationAsNeeded(nh, "y");
     setDecelerationAsNeeded(nh, "theta");
 
-
+    configure(nh);
     xytheta_direct_[0] = xytheta_direct_[1] = xytheta_direct_[2] = 1;
   }
 
+
+  void KinematicParameters::configure(const robot::NodeHandle &nh)
+  {
+    nh.param("max_vel_x", original_max_vel_x_, 0.0);
+    nh.param("max_vel_y", original_max_vel_y_, 0.0);
+    nh.param("max_vel_theta", original_max_vel_theta_, 0.0);
+    nh.param("min_vel_x", original_min_vel_x_, 0.0);
+    nh.param("min_vel_y", original_min_vel_y_, 0.0);
+    nh.param("min_speed_xy", original_min_speed_xy_, 0.0);
+    nh.param("max_speed_xy", original_max_speed_xy_, 0.0);
+    nh.param("min_speed_theta", original_min_speed_theta_, 0.0);
+    nh.param("acc_lim_x", original_acc_lim_x_, 0.0);
+    nh.param("acc_lim_y", original_acc_lim_y_, 0.0);
+    nh.param("acc_lim_theta", original_acc_lim_theta_, 0.0);
+    nh.param("decel_lim_x", original_decel_lim_x_, 0.0);
+    nh.param("decel_lim_y", original_decel_lim_y_, 0.0);
+    nh.param("decel_lim_theta", original_decel_lim_theta_, 0.0);
+    reconfigure(nh);
+  }
+
+  void KinematicParameters::reconfigure(const robot::NodeHandle &nh)
+  {
+    nh.param("max_vel_x", max_vel_x_, 0.0);
+    nh.param("max_vel_y", max_vel_y_, 0.0);
+    nh.param("max_vel_theta", max_vel_theta_, 0.0);
+    nh.param("min_vel_x", min_vel_x_, 0.0);
+    nh.param("min_vel_y", min_vel_y_, 0.0);
+    nh.param("min_speed_xy", min_speed_xy_, 0.0);
+    nh.param("max_speed_xy", max_speed_xy_, 0.0);
+    nh.param("min_speed_theta", min_speed_theta_, 0.0);
+    nh.param("acc_lim_x", acc_lim_x_, 0.0);
+    nh.param("acc_lim_y", acc_lim_y_, 0.0);
+    nh.param("acc_lim_theta", acc_lim_theta_, 0.0);
+    nh.param("decel_lim_x", decel_lim_x_, 0.0);
+    nh.param("decel_lim_y", decel_lim_y_, 0.0);
+    nh.param("decel_lim_theta", decel_lim_theta_, 0.0);
+  }
+
   void KinematicParameters::setDirect(int *xytheta_direct)
   {
     xytheta_direct_[0] = xytheta_direct[0];
@@ -111,13 +149,11 @@ namespace mkt_plugins
   void KinematicParameters::setMinX(double value)
   {
     min_vel_x_ = fabs(value) < fabs(original_min_vel_x_) + EPSILON ? value : min_vel_x_;
-    // ROS_INFO_THROTTLE(10.0, "vx_min %f %f %f", value , original_min_vel_x_, min_vel_x_);
   }
 
   void KinematicParameters::setMaxX(double value)
   {
     max_vel_x_ = fabs(value) <= fabs(original_max_vel_x_) + EPSILON ? value : original_max_vel_x_;
-    // ROS_INFO_THROTTLE(10, "vx %f %f %f", value , original_max_vel_x_, max_vel_x_);
   }
 
   void KinematicParameters::setAccX(double value)

src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp

@@ -18,6 +18,7 @@ void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
   else
   {
     double controller_frequency = robot_nav_2d_utils::searchAndGetParam(nh, "controller_frequency", 20.0);
+    robot::log_warning("[%s:%d] limited_accel_generator: %f", __FILE__, __LINE__, controller_frequency);
     if (controller_frequency > 0)
     {
       acceleration_time_ = 1.0 / controller_frequency;
@@ -29,6 +30,7 @@ void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
       acceleration_time_ = 0.05;
     }
   }
+
 }
 
 void LimitedAccelGenerator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity)

src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp

@@ -14,19 +14,15 @@ void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameter
 
 void XYThetaIterator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt)
 {
-  x_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(),
+  robot::log_info("[%s:%d] xy_theta_iterator: %f %f %f %f", __FILE__, __LINE__, current_velocity.x, current_velocity.y, current_velocity.theta, dt);
+  x_it_ = std::make_shared<OneDVelocityIterator>("x_it_", current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(),
                                                  kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_);
-  x_it_->name_ = std::string("x_it_");
   
-  y_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.y, kinematics_->getDirect()[1], kinematics_->getMinY(), kinematics_->getMaxY(),
+  y_it_ = std::make_shared<OneDVelocityIterator>("y_it_", current_velocity.y, kinematics_->getDirect()[1], kinematics_->getMinY(), kinematics_->getMaxY(),
                                                  kinematics_->getAccY(), kinematics_->getDecelY(), dt, vy_samples_);
   
-  y_it_->name_ = std::string("y_it_");
+  th_it_ = std::make_shared<OneDVelocityIterator>("th_it_", current_velocity.theta, kinematics_->getDirect()[2], kinematics_->getMinTheta(), kinematics_->getMaxTheta(),
-  
-  th_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.theta, kinematics_->getDirect()[2], kinematics_->getMinTheta(), kinematics_->getMaxTheta(),
                                                   kinematics_->getAccTheta(), kinematics_->getDecelTheta(), dt, vtheta_samples_);
-
-  th_it_->name_ = std::string("th_it_");
 }
 
 bool XYThetaIterator::hasMoreTwists()

src/Algorithms/Packages/global_planners/custom_planner

@@ -1 +1 @@
-Subproject commit a2bebb5be969471a135601da78866cc9d1db84b1
+Subproject commit 49afcce5b2247793282fe5e94d0e27f062562325

src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h

@@ -46,6 +46,12 @@ namespace pnkx_local_planner
      */
     void setPlan(const robot_nav_2d_msgs::Path2D &path) override;
 
+    /**
+     * @brief robot_nav_core2 getPlan - Gets the current global plan
+     * @param path The global plan
+     */
+    void getPlan(robot_nav_2d_msgs::Path2D &path) override;
+
     /**
      * @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
      *
@@ -179,7 +185,8 @@ namespace pnkx_local_planner
     std::string name_;
     robot::NodeHandle parent_, planner_nh_;  
     robot_nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan
-    robot_nav_2d_msgs::Path2D transformed_plan_;
+    robot_nav_2d_msgs::Path2D transformed_global_plan_;
+    robot_nav_2d_msgs::Path2D local_plan_;
     robot_nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose
  
     robot_costmap_2d::Costmap2D *costmap_;

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp

@@ -180,7 +180,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_
 
   try
   {
-    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
       throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
   }
   catch(const robot_nav_core2::LocalPlannerException& e)
@@ -189,7 +189,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_
   }
 
   double x_direction, y_direction, theta_direction;
-  if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+  if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
   {
     throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
   }
@@ -206,9 +206,16 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner
   robot_nav_2d_msgs::Twist2D twist;
   mkt_msgs::Trajectory2D traj;
   robot_nav_2d_msgs::Twist2DStamped cmd_vel;
-  if (!ret_nav_)
+  if (ret_nav_)
-    traj = nav_algorithm_->calculator(pose, velocity);
+  {
+    local_plan_.poses.clear();
+    return cmd_vel;
+  }
 
+  traj = nav_algorithm_->calculator(pose, velocity);
+  local_plan_.header.stamp = robot::Time::now();
+  robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now());
+  local_plan_ = robot_nav_2d_utils::pathToPath(path);
   cmd_vel.velocity = traj.velocity;
   return cmd_vel;
 }
@@ -222,7 +229,7 @@ bool pnkx_local_planner::PNKXGoStraightLocalPlanner::isGoalReached(const robot_n
   }
   // Update time stamp of goal pose
   // goal_pose_.header.stamp = pose.header.stamp;
-  ret_nav_ = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), transformed_plan_, velocity);
+  ret_nav_ = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), transformed_global_plan_, velocity);
 
   if (ret_nav_)
     robot::log_info_at(__FILE__, __LINE__, "Goal reached!");

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp

@@ -208,6 +208,15 @@ void pnkx_local_planner::PNKXLocalPlanner::setPlan(const robot_nav_2d_msgs::Path
   }
 }
 
+void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &path)
+{
+  if (local_plan_.poses.empty())
+  {
+    return;
+  }
+  path = local_plan_;
+}
+
 void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {
   this->getParams(planner_nh_);
@@ -231,7 +240,8 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
                              local_goal_pose = this->transformPoseToLocal(goal_pose_);
   // robot::log_info("local_start_pose: %f %f %f", local_start_pose.pose.x, local_start_pose.pose.y, local_start_pose.pose.theta);
   // robot::log_info("local_goal_pose: %f %f %f", local_goal_pose.pose.x, local_goal_pose.pose.y, local_goal_pose.pose.theta);
-  if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+  if (!pnkx_local_planner::transformGlobalPlan(
+    tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
   {
     robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
     throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
@@ -240,7 +250,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
   double x_direction, y_direction, theta_direction;
   if (!ret_nav_)
   {
-    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+    if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
       throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@@ -248,7 +258,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
   }
   else
   {
-    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+    if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
     {
       robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
       throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@@ -291,11 +301,24 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg
   cmd_vel.header.stamp = robot::Time::now();
 
   if (ret_nav_ && ret_angle_)
+  {
     return cmd_vel;
+    local_plan_.poses.clear();
+  }
   else if (!ret_nav_)
+  {
     traj = nav_algorithm_->calculator(pose, velocity);
+    local_plan_.header.stamp = robot::Time::now();
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now());
+    local_plan_ = robot_nav_2d_utils::pathToPath(path);
+  }
   else
+  {
     traj = rotate_algorithm_->calculator(pose, velocity);
+    local_plan_.header.stamp = robot::Time::now();
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now());
+    local_plan_ = robot_nav_2d_utils::pathToPath(path);
+  }
 
   cmd_vel.velocity = traj.velocity;
   return cmd_vel;
@@ -404,7 +427,7 @@ bool pnkx_local_planner::PNKXLocalPlanner::isGoalReached(const robot_nav_2d_msgs
   // goal_pose_.header.stamp = pose.header.stamp;
   robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
   robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
-  robot_nav_2d_msgs::Path2D plan = transformed_plan_;
+  robot_nav_2d_msgs::Path2D plan = transformed_global_plan_;
   if (!ret_nav_)
   {
     ret_nav_ = goal_checker_->isGoalReached(local_pose, local_goal, plan, velocity);

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp

@@ -155,7 +155,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
 
   try
   {
-    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
+    if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
       throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
   }
   catch(const robot_nav_core2::LocalPlannerException& e)
@@ -164,7 +164,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
   }
 
   double x_direction, y_direction, theta_direction;
-  if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
+  if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
   {
     throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
   }
@@ -199,10 +199,17 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::Sc
   robot_nav_2d_msgs::Twist2D twist;
   mkt_msgs::Trajectory2D traj;
   robot_nav_2d_msgs::Twist2DStamped cmd_vel;
+  cmd_vel.header.stamp = robot::Time::now();
   if (ret_nav_)
+  {
+    local_plan_.poses.clear();
     return cmd_vel;
+  }
 
   traj = nav_algorithm_->calculator(pose, velocity);
+  local_plan_.header.stamp = robot::Time::now();
+  robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now());
+  local_plan_ = robot_nav_2d_utils::pathToPath(path);
   cmd_vel.velocity = traj.velocity;
   return cmd_vel;
 }

src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h

@@ -159,20 +159,20 @@ protected:
    */
 		TF3SIMD_FORCE_INLINE void	setMax(const QuadWord& other)
 		{
-			// tf3SetMax(m_floats[0], other.m_floats[0]);
+			tf3SetMax(m_floats[0], other.m_floats[0]);
-			// tf3SetMax(m_floats[1], other.m_floats[1]);
+			tf3SetMax(m_floats[1], other.m_floats[1]);
-			// tf3SetMax(m_floats[2], other.m_floats[2]);
+			tf3SetMax(m_floats[2], other.m_floats[2]);
-			// tf3SetMax(m_floats[3], other.m_floats[3]);
+			tf3SetMax(m_floats[3], other.m_floats[3]);
 		}
   /**@brief Set each element to the min of the current values and the values of another QuadWord
    * @param other The other QuadWord to compare with 
    */
 		TF3SIMD_FORCE_INLINE void	setMin(const QuadWord& other)
 		{
-			// tf3SetMin(m_floats[0], other.m_floats[0]);
+			tf3SetMin(m_floats[0], other.m_floats[0]);
-			// tf3SetMin(m_floats[1], other.m_floats[1]);
+			tf3SetMin(m_floats[1], other.m_floats[1]);
-			// tf3SetMin(m_floats[2], other.m_floats[2]);
+			tf3SetMin(m_floats[2], other.m_floats[2]);
-			// tf3SetMin(m_floats[3], other.m_floats[3]);
+			tf3SetMin(m_floats[3], other.m_floats[3]);
 		}
 
 

src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h

@@ -27,7 +27,6 @@
 #include <robot/time.h>
 #include <move_base_core/common.h>
 
-
 namespace robot
 {
 	namespace move_base_core

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

@@ -133,6 +133,13 @@ namespace robot_nav_core
      */
     virtual bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &plan) = 0;
 
+    /**
+     * @brief Gets the global plan for this local planner.
+     *
+     * @param path The global plan
+     */ 
+    virtual void getPlan(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

@@ -92,6 +92,13 @@ namespace robot_nav_core2
      */
     virtual void setPlan(const robot_nav_2d_msgs::Path2D &path) = 0;
 
+    /**
+     * @brief Gets the global plan for this local planner.
+     *
+     * @param path The global plan
+     */ 
+    virtual void getPlan(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

@@ -159,6 +159,14 @@ namespace robot_nav_core_adapter
      */
     bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &plan) override;
 
+
+    /**
+     * @brief Gets the global plan for this local planner.
+     *
+     * @param path The global plan
+     */ 
+    virtual void getPlan(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

@@ -366,6 +366,17 @@ namespace robot_nav_core_adapter
     }
   }
 
+  void LocalPlannerAdapter::getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path)
+  {
+    if (!planner_)
+    {
+      return;
+    }
+    robot_nav_2d_msgs::Path2D path2d;
+    planner_->getPlan(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

@@ -607,7 +607,6 @@ void move_base::MoveBase::updateLocalCostmap(const T& value, robot_costmap_2d::L
 
 void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry)
 {
-  robot::log_info("[%s:%d] addOdometry called for: %s", __FILE__, __LINE__, odometry_name.c_str());
   odometry_ = odometry;
 }
 
@@ -2206,6 +2205,20 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
       {
         if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel))
         {
+          robot_nav_msgs::Path path;
+          tc_->getPlan(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 = controller_costmap_robot_->getGlobalFrameID();
+          }
+          local_data_.plan = robot_nav_2d_utils::pathToPath(path);
+
           last_valid_control_ = robot::Time::now();
           // make sure that we send the velocity command to the base
 
@@ -2283,6 +2296,9 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
             lock.unlock();
           }
         }
+        twist_.velocity = robot_nav_2d_utils::twist3Dto2D(cmd_vel);
+        twist_.header.stamp = robot::Time::now();
+        twist_.header.frame_id = controller_costmap_robot_->getGlobalFrameID();
       }
       else
       {