update

src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt

@@ -81,7 +81,7 @@ endif()
 # Libraries
 # ========================================================
 add_library(${PROJECT_NAME}_diff SHARED
-  src/diff/diff_predictive_trajectory_copy.cpp
+  src/diff/diff_predictive_trajectory.cpp
   src/diff/diff_rotate_to_goal.cpp
   src/diff/diff_go_straight.cpp
   # src/diff/pure_pursuit.cpp

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

@@ -3,8 +3,6 @@
 
 #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>
@@ -12,8 +10,6 @@
 #include <robot_nav_core2/costmap.h>
 #include <nav_grid/coordinate_conversion.h>
 #include <angles/angles.h>
-#include <tf3/exceptions.h>
-#include <tf3/utils.h>
 #include <robot_nav_msgs/Path.h>
 #include <kalman/kalman.h>
 #include <vector>
@@ -44,7 +40,7 @@ namespace mkt_algorithm
        * @param nh NodeHandle to read parameters from
        */
       virtual void initialize(
-          robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
+        robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
 
       /**
        * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
@@ -101,11 +97,6 @@ namespace mkt_algorithm
        */
       virtual void getParams();
 
-      /**
-       * @brief Initialize Kalman filter
-       */
-      virtual void initKalmanFilter();
-
       /**
        * @brief Dynamically adjust look ahead distance based on the speed
        * @param velocity
@@ -210,6 +201,8 @@ namespace mkt_algorithm
           const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed,
           const double &pose_cost, double &linear_vel, double &sign_x);
 
+      std::vector<robot_geometry_msgs::Point> interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution);
+
       /**
        * @brief Cost at pose
        * @param x
@@ -229,6 +222,11 @@ namespace mkt_algorithm
       bool detectWobbleByCarrotAngle(std::vector<double>& angle_history, double current_angle, 
                                     double amplitude_threshold = 0.3, size_t window_size = 20);
 
+      void publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose);
+
+      std::vector<double> angle_history_;
+      size_t window_size_;
+
       bool initialized_;
       bool nav_stop_;
       robot::NodeHandle nh_, nh_priv_;
@@ -278,6 +276,9 @@ namespace mkt_algorithm
       double inflation_cost_scaling_factor_;
       double cost_scaling_dist_, cost_scaling_gain_;
 
+      double cost_left_goal_, cost_right_goal_;
+      double cost_left_side_ , cost_right_side_;
+      double center_cost_;
 
       // Control frequency
       double control_duration_;

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

@@ -1,27 +1,23 @@
 #include <mkt_algorithm/diff/diff_predictive_trajectory.h>
-#include <mkt_algorithm/diff/pure_pursuit.h>
+#include <boost/dll/alias.hpp>
-#include <boost/dll/import.hpp>
-#include <robot/robot.h>
-
 #define LIMIT_VEL_THETA 0.33
 
+mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {}
 
 void mkt_algorithm::diff::PredictiveTrajectory::initialize(
     robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
 {
   if (!initialized_)
   {
-    nh_ = robot::NodeHandle("~");
+    nh_ = robot::NodeHandle("~/");
-    nh_priv_ = robot::NodeHandle(nh, name);
+    nh_priv_ = robot::NodeHandle("~/" + name);
     name_ = name;
     tf_ = tf;
     traj_ = traj;
     costmap_robot_ = costmap_robot;
     this->getParams();
-    nh_.param("publish_topic", enable_publish_, false);
     nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
 
-    footprint_spec_ = costmap_robot_->getRobotFootprint();
     std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
     if (footprint.size() > 1)
     {
@@ -44,64 +40,55 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
       this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1;
       this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1;
     }
-    this->initKalmanFilter();
+
+    // kalman
+    last_actuator_update_ = robot::Time::now();
+    n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
+    m_ = 2; // measurements: x, y, theta
+    double dt = control_duration_;
+
+    // Khởi tạo ma trận
+    A = Eigen::MatrixXd::Identity(n_, n_);
+    C = Eigen::MatrixXd::Zero(m_, n_);
+    Q = Eigen::MatrixXd::Zero(n_, n_);
+    R = Eigen::MatrixXd::Identity(m_, m_);
+    P = Eigen::MatrixXd::Identity(n_, n_);
+
+    for (int i = 0; i < n_; i += 3)
+    {
+      A(i, i + 1) = dt;
+      A(i, i + 2) = 0.5 * dt * dt;
+      A(i + 1, i + 2) = dt;
+    }
+
+    C(0, 0) = 1;
+    C(1, 3) = 1;
+    Q(2, 2) = 0.1;
+    Q(5, 5) = 0.6;
+
+    R(0, 0) = 0.1;
+    R(1, 1) = 0.2;
+
+    P(3, 3) = 0.4;
+    P(4, 4) = 0.4;
+    P(5, 5) = 0.4;
+
+    kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
+    Eigen::VectorXd x0(n_);
+    x0 << 0, 0, 0, 0, 0, 0;
+    kf_->init(robot::Time::now().toSec(), x0);
+
+
+
     x_direction_ = y_direction_ = theta_direction_ = 0;
     this->initialized_ = true;
     robot::log_info("[%s:%d]\n PredictiveTrajectory Initialized successfully", __FILE__, __LINE__);
   }
 }
 
-mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory()
-{
-  if (kf_)
-  {
-    kf_.reset();
-  }
-}
-
-void  mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter()
-{
-  // kalman
-  last_actuator_update_ = robot::Time::now();
-  n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
-  m_ = 2; // measurements: x, y, theta
-  double dt = control_duration_;
-
-  // Khởi tạo ma trận
-  A = Eigen::MatrixXd::Identity(n_, n_);
-  C = Eigen::MatrixXd::Zero(m_, n_);
-  Q = Eigen::MatrixXd::Zero(n_, n_);
-  R = Eigen::MatrixXd::Identity(m_, m_);
-  P = Eigen::MatrixXd::Identity(n_, n_);
-
-  for (int i = 0; i < n_; i += 3)
-  {
-    A(i, i + 1) = dt;
-    A(i, i + 2) = 0.5 * dt * dt;
-    A(i + 1, i + 2) = dt;
-  }
-
-  C(0, 0) = 1;
-  C(1, 3) = 1;
-  Q(2, 2) = 0.1;
-  Q(5, 5) = 0.6;
-
-  R(0, 0) = 0.1;
-  R(1, 1) = 0.2;
-
-  P(3, 3) = 0.4;
-  P(4, 4) = 0.4;
-  P(5, 5) = 0.4;
-
-  kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
-  Eigen::VectorXd x0(n_);
-  x0 << 0, 0, 0, 0, 0, 0;
-  kf_->init(robot::Time::now().toSec(), x0);
-}
-
 void mkt_algorithm::diff::PredictiveTrajectory::getParams()
 {
-  robot_base_frame_ = nh_priv_.param<std::string>("robot_base_frame", std::string("base_link"));
+  robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
   nh_priv_.param<double>("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
   nh_priv_.param<double>("angle_threshold", angle_threshold_, M_PI / 8);
   nh_priv_.param<int>("index_samples", index_samples_, 0);
@@ -135,12 +122,12 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   nh_priv_.param<double>("cost_scaling_gain", cost_scaling_gain_, 1.0);
   if (inflation_cost_scaling_factor_ <= 0.0)
   {
-    robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, "
+    robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
-             "it should be >0. Disabling cost regulated linear velocity scaling.");
     use_cost_regulated_linear_velocity_scaling_ = false;
   }
   double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
   control_duration_ = 1.0 / control_frequency;
+  window_size_ = (size_t)(control_frequency * 3.0);
 
   if (traj_)
   {
@@ -298,6 +285,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     return false;
   }
 
+
   x_direction = x_direction_;
   y_direction = y_direction_ = 0;
   theta_direction = theta_direction_;
@@ -346,6 +334,9 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
 
       robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
 
+      // teb_local_planner::PoseSE2 start_pose(front);
+      // teb_local_planner::PoseSE2 goal_pose(back);
+      // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
       const double dir_path = 0.0;
       if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
         x_direction = dir_path > 0 ? FORWARD : BACKWARD;
@@ -390,15 +381,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
   drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5);
   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__);
@@ -414,10 +397,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     return result;
   }
   auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
+
   bool allow_rotate = false;
   nh_priv_.param("allow_rotate", allow_rotate, false);
 
-  // double path_distance_to_rotate = hypot(transformed_plan.poses.front().pose.x, transformed_plan.poses.front().pose.y);
   robot_geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose;
   const double distance_allow_rotate = min_journey_squared_;
   const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y);
@@ -440,11 +423,75 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
   else
   {
+    const double vel_x_reduce = std::min(fabs(v_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;
 
+    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);
 
-    // std::shared_ptr<PurePursuit> pure_pursuit = std::make_shared<PurePursuit>();
+    const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
-    // pure_pursuit->computePurePursuit(traj_, carrot_pose, velocity, min_approach_linear_velocity_, journey_plan, sign_x, lookahead_dist, lookahead_time_, drive_cmd);
+    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;
+    }
+    if (fabs(v_theta) > LIMIT_VEL_THETA)
+    {
+      robot_nav_2d_msgs::Twist2D cmd_vel, cmd_result;
+      cmd_vel.x = sign_x > 0
+                      ? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1))
+                      : std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1));
+      cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5);
+      this->moveWithAccLimits(velocity, cmd_vel, cmd_result);
+      drive_cmd.x = std::copysign(cmd_result.x, sign_x);
+      v_theta = drive_cmd.x * curvature;
+    }
+
+    if (journey_plan < min_journey_squared_)
+    {
+      if (transform_plan_.poses.size() > 2)
+      {
+        robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
+        robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
+        double dx = end.pose.x - start.pose.x;
+        double dy = end.pose.y - start.pose.y;
+        v_theta = atan2(dy, dx);
+        if (v_theta > M_PI_2)
+          v_theta -= M_PI;
+        else if (v_theta < -M_PI_2)
+          v_theta += M_PI;
+        // v_theta *= 0.5;
+        v_theta = std::clamp(v_theta, -0.02, 0.02);
+      }
+      else
+        v_theta = 0.0;
+    }
+    double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8;
+    double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt;
+    double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt;
+    drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth);
 
     if (this->nav_stop_)
     {
@@ -709,7 +756,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
     robot_nav_2d_msgs::Pose2DStamped robot;
     if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot))
     {
-      throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame");
+      throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
     }
 
     double dist_thresh_sq = dist_behind_robot * dist_behind_robot;
@@ -737,7 +784,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
   }
   catch (const tf3::TransformException &ex)
   {
-    robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s", __FILE__, __LINE__, ex.what());
+    robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s\n", __FILE__, __LINE__, ex.what());
     return false;
   }
   return true;
@@ -774,7 +821,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
     robot_nav_2d_msgs::Pose2DStamped robot_pose;
     if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose))
     {
-      throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame");
+      throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
     }
 
     // we'll discard points on the plan that are outside the local costmap
@@ -853,19 +900,19 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
   }
   catch (tf3::LookupException &ex)
   {
-    robot::log_error("[%s:%d]\n No Transform available Error: %s", __FILE__, __LINE__, ex.what());
+    robot::log_error("[%s:%d]\n No Transform available Error: %s\n", __FILE__, __LINE__, ex.what());
     return false;
   }
   catch (tf3::ConnectivityException &ex)
   {
-    robot::log_error("[%s:%d]\n Connectivity Error: %s", __FILE__, __LINE__, ex.what());
+    robot::log_error("[%s:%d]\n Connectivity Error: %s\n", __FILE__, __LINE__, ex.what());
     return false;
   }
   catch (tf3::ExtrapolationException &ex)
   {
-    robot::log_error("[%s:%d]\n Extrapolation Error: %s", __FILE__, __LINE__, ex.what());
+    robot::log_error("[%s:%d]\n Extrapolation Error: %s\n", __FILE__, __LINE__, ex.what());
     if (global_plan.poses.size() > 0)
-      robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str());
+      robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s\n", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str());
 
     return false;
   }
@@ -912,7 +959,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_na
   double vel_yaw = velocity.theta;
   double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - fabs(decel_lim_theta_) * dt));
 
-  // we do want to check whether or not the command is valid
   cmd_vel.x = vx;
   cmd_vel.y = vy;
   cmd_vel.theta = vth;
@@ -1016,6 +1062,49 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
   // ROS_INFO_STREAM_THROTTLE(0.1, ss.str());
 }
 
+std::vector<robot_geometry_msgs::Point>
+mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution)
+{
+
+  // Dịch sang tọa độ tuyệt đối
+  std::vector<robot_geometry_msgs::Point> abs_footprint;
+  double cos_th = std::cos(pose.theta);
+  double sin_th = std::sin(pose.theta);
+  for (auto pt : footprint)
+  {
+    pt.x *= 1.2;
+    pt.y *= 1.2;
+    robot_geometry_msgs::Point abs_pt;
+    abs_pt.x = pose.x + pt.x * cos_th - pt.y * sin_th;
+    abs_pt.y = pose.y + pt.x * sin_th + pt.y * cos_th;
+    abs_footprint.push_back(abs_pt);
+  }
+
+  std::vector<robot_geometry_msgs::Point> points;
+  for (size_t i = 0; i < abs_footprint.size(); ++i)
+  {
+    const robot_geometry_msgs::Point &start = abs_footprint[i];
+    const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
+
+    double dx = end.x - start.x;
+    double dy = end.y - start.y;
+    double distance = std::hypot(dx, dy);
+    int steps = std::max(1, static_cast<int>(std::floor(distance / resolution)));
+
+    for (int j = 0; j <= steps; ++j)
+    {
+      if (j == steps && i != abs_footprint.size() - 1)
+        continue; // tránh lặp
+      double t = static_cast<double>(j) / steps;
+      robot_geometry_msgs::Point pt;
+      pt.x = start.x + t * dx;
+      pt.y = start.y + t * dy;
+      points.push_back(pt);
+    }
+  }
+  return points;
+}
+
 double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, const double &y)
 {
   unsigned int mx, my;
@@ -1093,11 +1182,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v
   return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0;
 }
 
-
 score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create()
 {
   return std::make_shared<mkt_algorithm::diff::PredictiveTrajectory>();
 }
-
-// Register this planner as a GlobalPlanner plugin
-BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory)

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

@@ -120,9 +120,6 @@ namespace mkt_plugins
       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;

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

@@ -14,7 +14,6 @@ void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameter
 
 void XYThetaIterator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt)
 {
-  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_);
   

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

@@ -214,7 +214,10 @@ void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &pa
   {
     return;
   }
+  // robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(local_plan_.poses.front());
+  // pnkx_local_planner::transformGlobalPlan(tf_, local_plan_, local_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, path);
   path = local_plan_;
+
 }
 
 void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
@@ -234,12 +237,9 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
   }
 
   // Update time stamp of goal pose
-  // goal_pose_.header.stamp = pose.header.stamp;
-  // robot::log_info("pose: %f %f %f", pose.pose.x, pose.pose.y, pose.pose.theta);
   robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(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_global_plan_))
   {
@@ -309,14 +309,14 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg
   {
     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());
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), 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());
+    robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
     local_plan_ = robot_nav_2d_utils::pathToPath(path);
   }