update

src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h

@@ -153,11 +153,6 @@ namespace score_algorithm
     double old_xy_goal_tolerance_;
     double angle_threshold_;
 
-    bool enable_publish_;
-    // robot::Publisher reached_intermediate_goals_pub_;
-    // robot::Publisher current_goal_pub_;
-    // robot::Publisher closet_robot_goal_pub_;
-    // robot::Publisher transformed_plan_pub_, compute_plan_pub_;
 
     std::vector<robot_nav_2d_msgs::Pose2DStamped> reached_intermediate_goals_;
     std::vector<unsigned int> start_index_saved_vt_;

src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h

@@ -98,6 +98,12 @@ namespace score_algorithm
      */
     virtual robot_nav_2d_msgs::Twist2D nextTwist() = 0;
 
+    /**
+     * @brief Get the current twist
+     * @return The current twist
+     */
+    virtual robot_nav_2d_msgs::Twist2D getTwist() = 0;
+
     /**
      * @brief Get all the twists for an iteration.
      *

src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt

@@ -81,7 +81,7 @@ endif()
 # Libraries
 # ========================================================
 add_library(${PROJECT_NAME}_diff SHARED
-  src/diff/diff_predictive_trajectory.cpp
+  src/diff/diff_predictive_trajectory_copy.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

@@ -210,8 +210,6 @@ 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
@@ -231,13 +229,8 @@ 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_, avoid_obstacles_;
+      bool nav_stop_;
       robot::NodeHandle nh_, nh_priv_;
       std::string frame_id_path_;
       std::string robot_base_frame_;
@@ -285,9 +278,6 @@ 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/include/mkt_algorithm/diff/pure_pursuit.h

@@ -23,26 +23,32 @@ namespace mkt_algorithm
       PurePursuit();
       ~PurePursuit();
       void computePurePursuit(
-        const double &velocity_min, 
+        const score_algorithm::TrajectoryGenerator::Ptr &traj,
-        const double &velocity_max,
-        const double &linear_vel,
         const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
-        const double &min_dist2,
+        const robot_nav_2d_msgs::Twist2D &velocity,
-        const double &curvature_dist2_floor,
+        const double &min_approach_linear_velocity,
-        const double &dist2_override);
+        const double &journey_plan,
-        
+        const double &sign_x,
+        const double &lookahead_dist_min,
+        const double &lookahead_dist_max,
+        const double &lookahead_dist,
+        const double &lookahead_time,
+        robot_nav_2d_msgs::Twist2D &drive_cmd
+      );
+
+
     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);
+                            const double &pose_cost, double &linear_vel, const 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,
+      // properties
-                                     const double &amplitude_threshold, const size_t &window_size);
+      double max_lateral_accel_;
     };
   }
 }

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

@@ -15,7 +15,6 @@ void mkt_algorithm::diff::GoStraight::initialize(
     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);
 
     std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();

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

@@ -1,9 +1,11 @@
 #include <mkt_algorithm/diff/diff_predictive_trajectory.h>
+#include <mkt_algorithm/diff/pure_pursuit.h>
 #include <boost/dll/import.hpp>
 #include <robot/robot.h>
 
 #define LIMIT_VEL_THETA 0.33
 
+
 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)
 {
@@ -100,7 +102,6 @@ void  mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter()
 void mkt_algorithm::diff::PredictiveTrajectory::getParams()
 {
   robot_base_frame_ = nh_priv_.param<std::string>("robot_base_frame", std::string("base_link"));
-  nh_priv_.param<bool>("avoid_obstacles", avoid_obstacles_, false);
   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);
@@ -140,7 +141,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   }
   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_)
   {
@@ -297,10 +297,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
     robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
     return false;
   }
-  // else
-  // {
-  //   robot::log_info("[%s:%d]\n Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_);
-  // }
 
   x_direction = x_direction_;
   y_direction = y_direction_ = 0;
@@ -350,11 +346,7 @@ 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;
     }
@@ -391,15 +383,12 @@ 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 (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_;
@@ -433,12 +422,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   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);
   const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
-  if (avoid_obstacles_)
+
-    allow_rotate = journey_plan >= distance_allow_rotate &&
+  allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
-                   fabs(front.y) <= 0.2 &&
-                   (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution());
-  else
-    allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
 
   double angle_to_heading;
   if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
@@ -455,88 +440,24 @@ 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);
 
-    const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
+    // std::shared_ptr<PurePursuit> pure_pursuit = std::make_shared<PurePursuit>();
-    const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
+    // pure_pursuit->computePurePursuit(traj_, carrot_pose, velocity, min_approach_linear_velocity_, journey_plan, sign_x, lookahead_dist, lookahead_time_, drive_cmd);
-    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);
+    if (this->nav_stop_)
-    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;
+      if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
-      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();
+        if (!stopWithAccLimits(pose, velocity, drive_cmd))
-        robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
+          return result;
-        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;
+        drive_cmd = {};
+      result.velocity = drive_cmd;
+      return result;
     }
-    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_)
-      {
-        if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
-        {
-          if (!stopWithAccLimits(pose, velocity, drive_cmd))
-            return result;
-        }
-        else
-          drive_cmd = {};
-        result.velocity = drive_cmd;
-        return result;
-      }
   
     Eigen::VectorXd y(2);
     y << drive_cmd.x, drive_cmd.theta;
@@ -553,19 +474,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     }
     kf_->update(y, dt, A);
 
-    // Check if Kalman filter's estimated velocity exceeds v_max
+    drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
-    if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
-                             cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
-                             cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
-                             cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE))
-    {
-      drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0];
-    }
-    else
-    {
-      drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
-    }
-
     drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x);
     drive_cmd.theta = std::clamp(kf_->state()[3], -LIMIT_VEL_THETA, LIMIT_VEL_THETA);
   }
@@ -1107,49 +1016,6 @@ 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;
@@ -1227,89 +1093,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v
   return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0;
 }
 
-void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose)
-{
-  const auto &plan_back_pose = compute_plan_.poses.back();
-  // const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0;
-  // const double offset_min = this->min_path_distance_;
-
-  auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
-  for (const auto &point : points_rb)
-  {
-    double cost_goal = costAtPose(point.x, point.y);
-    double dx = point.x - pose.pose.x;
-    double dy = point.y - pose.pose.y;
-    double cos_yaw = cos(-pose.pose.theta);
-    double sin_yaw = sin(-pose.pose.theta);
-    double y_rel = dx * sin_yaw + dy * cos_yaw;
-    const double epsilon = min_path_distance_;
-    if (y_rel > epsilon)
-    {
-      cost_left_side_ = std::max(cost_left_side_, cost_goal);
-    }
-    else if (y_rel < -epsilon)
-    {
-      cost_right_side_ = std::max(cost_right_side_, cost_goal);
-    }
-  }
-
-  unsigned int step_footprint = 10;
-  if ((unsigned int)(compute_plan_.poses.size() - 1) < 10)
-  {
-    auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
-    for (const auto &point : points)
-    {
-      double cost_goal = costAtPose(point.x, point.y);
-      double dx = point.x - plan_back_pose.pose.x;
-      double dy = point.y - plan_back_pose.pose.y;
-      double cos_yaw = cos(-plan_back_pose.pose.theta);
-      double sin_yaw = sin(-plan_back_pose.pose.theta);
-      double y_rel = dx * sin_yaw + dy * cos_yaw;
-      const double epsilon = min_path_distance_;
-      if (y_rel > epsilon)
-      {
-        cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
-      }
-      else if (y_rel < -epsilon)
-      {
-        cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
-      }
-      else
-        center_cost_ = std::max(center_cost_, cost_goal);
-    }
-  }
-  else
-  {
-    for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint)
-    {
-      auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
-      for (const auto &point : points)
-      {
-        double cost_goal = costAtPose(point.x, point.y);
-        double dx = point.x - compute_plan_.poses[i].pose.x;
-        double dy = point.y - compute_plan_.poses[i].pose.y;
-        double cos_yaw = cos(-compute_plan_.poses[i].pose.theta);
-        double sin_yaw = sin(-compute_plan_.poses[i].pose.theta);
-        double y_rel = dx * sin_yaw + dy * cos_yaw;
-        const double epsilon = min_path_distance_;
-        if (y_rel > epsilon)
-        {
-          cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
-        }
-        else if (y_rel < -epsilon)
-        {
-          cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
-        }
-        else
-          center_cost_ = std::max(center_cost_, cost_goal);
-      }
-      double dx = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.x - compute_plan_.poses[i].pose.x;
-      double dy = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.y - compute_plan_.poses[i].pose.y;
-      if (hypot(dx, dy) > 1.0)
-        break;
-    }
-  }
-}
 
 score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create()
 {

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

@@ -16,7 +16,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
     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();
@@ -42,6 +41,7 @@ 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();
     x_direction_ = y_direction_ = theta_direction_ = 0;
     this->initialized_ = true;
@@ -100,7 +100,6 @@ void  mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter()
 void mkt_algorithm::diff::PredictiveTrajectory::getParams()
 {
   robot_base_frame_ = nh_priv_.param<std::string>("robot_base_frame", std::string("base_link"));
-  nh_priv_.param<bool>("avoid_obstacles", avoid_obstacles_, false);
   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);
@@ -140,7 +139,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
   }
   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_)
   {
@@ -433,13 +431,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   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);
   const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
-  if (avoid_obstacles_)
-    allow_rotate = journey_plan >= distance_allow_rotate &&
-                   fabs(front.y) <= 0.2 &&
-                   (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution());
-  else
-    allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
 
+  allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
   double angle_to_heading;
   if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
   {
@@ -461,8 +454,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     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);
+    double post_cost = 0.0;
-    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;
@@ -483,12 +475,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
 
     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;
@@ -552,19 +539,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       A(i + 1, i + 2) = dt;
     }
     kf_->update(y, dt, A);
-
+    drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
-    // Check if Kalman filter's estimated velocity exceeds v_max
+    
-    if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
-                             cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
-                             cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
-                             cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE))
-    {
-      drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0];
-    }
-    else
-    {
-      drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
-    }
 
     drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x);
     drive_cmd.theta = std::clamp(kf_->state()[3], -LIMIT_VEL_THETA, LIMIT_VEL_THETA);
@@ -1107,49 +1083,6 @@ 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;
@@ -1227,90 +1160,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v
   return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0;
 }
 
-void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose)
-{
-  const auto &plan_back_pose = compute_plan_.poses.back();
-  // const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0;
-  // const double offset_min = this->min_path_distance_;
-
-  auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
-  for (const auto &point : points_rb)
-  {
-    double cost_goal = costAtPose(point.x, point.y);
-    double dx = point.x - pose.pose.x;
-    double dy = point.y - pose.pose.y;
-    double cos_yaw = cos(-pose.pose.theta);
-    double sin_yaw = sin(-pose.pose.theta);
-    double y_rel = dx * sin_yaw + dy * cos_yaw;
-    const double epsilon = min_path_distance_;
-    if (y_rel > epsilon)
-    {
-      cost_left_side_ = std::max(cost_left_side_, cost_goal);
-    }
-    else if (y_rel < -epsilon)
-    {
-      cost_right_side_ = std::max(cost_right_side_, cost_goal);
-    }
-  }
-
-  unsigned int step_footprint = 10;
-  if ((unsigned int)(compute_plan_.poses.size() - 1) < 10)
-  {
-    auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
-    for (const auto &point : points)
-    {
-      double cost_goal = costAtPose(point.x, point.y);
-      double dx = point.x - plan_back_pose.pose.x;
-      double dy = point.y - plan_back_pose.pose.y;
-      double cos_yaw = cos(-plan_back_pose.pose.theta);
-      double sin_yaw = sin(-plan_back_pose.pose.theta);
-      double y_rel = dx * sin_yaw + dy * cos_yaw;
-      const double epsilon = min_path_distance_;
-      if (y_rel > epsilon)
-      {
-        cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
-      }
-      else if (y_rel < -epsilon)
-      {
-        cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
-      }
-      else
-        center_cost_ = std::max(center_cost_, cost_goal);
-    }
-  }
-  else
-  {
-    for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint)
-    {
-      auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
-      for (const auto &point : points)
-      {
-        double cost_goal = costAtPose(point.x, point.y);
-        double dx = point.x - compute_plan_.poses[i].pose.x;
-        double dy = point.y - compute_plan_.poses[i].pose.y;
-        double cos_yaw = cos(-compute_plan_.poses[i].pose.theta);
-        double sin_yaw = sin(-compute_plan_.poses[i].pose.theta);
-        double y_rel = dx * sin_yaw + dy * cos_yaw;
-        const double epsilon = min_path_distance_;
-        if (y_rel > epsilon)
-        {
-          cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
-        }
-        else if (y_rel < -epsilon)
-        {
-          cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
-        }
-        else
-          center_cost_ = std::max(center_cost_, cost_goal);
-      }
-      double dx = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.x - compute_plan_.poses[i].pose.x;
-      double dy = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.y - compute_plan_.poses[i].pose.y;
-      if (hypot(dx, dy) > 1.0)
-        break;
-    }
-  }
-}
-
 score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create()
 {
   return std::make_shared<mkt_algorithm::diff::PredictiveTrajectory>();

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

@@ -25,7 +25,6 @@ void mkt_algorithm::diff::RotateToGoal::initialize(
 void mkt_algorithm::diff::RotateToGoal::getParams()
 {
   robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
-  nh_priv_.param<bool>("avoid_obstacles", avoid_obstacles_, false);
   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);

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

@@ -1,13 +1,23 @@
 #include <mkt_algorithm/diff/pure_pursuit.h>
+#define LIMIT_VEL_THETA 0.33
 
-void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity_min, 
+
-                                                          const double &velocity_max,
+void mkt_algorithm::diff::PurePursuit::computePurePursuit(
-                                                          const double &linear_vel,
+                                                          const score_algorithm::TrajectoryGenerator::Ptr &traj,
                                                           const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
-                                                          const double &min_dist2,
+                                                          const robot_nav_2d_msgs::Twist2D &velocity,
-                                                          const double &curvature_dist2_floor,
+                                                          const double &min_approach_linear_velocity,
-                                                          const double &dist2_override)
+                                                          const double &journey_plan,
+                                                          const double &sign_x,
+                                                          const double &lookahead_dist_min,
+                                                          const double &lookahead_dist_max,
+                                                          const double &lookahead_dist,
+                                                          const double &lookahead_time,
+                                                          robot_nav_2d_msgs::Twist2D &drive_cmd)
 {
+  if (!traj)
+    return;
+  const double velocity_max = sign_x > 0 ? traj->getTwistLinear(true).x : traj->getTwistLinear(false).x;
   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);
@@ -20,18 +30,19 @@ void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity
     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;
+  double post_cost = 0.0; 
-  const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
+  double distance_error = 0.0;
-  double d_reduce = std::clamp(journey_plan, min_S, max_S);
+  robot_nav_2d_msgs::Twist2D twist = traj->getTwist();
+  this->applyConstraints(distance_error, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
+
+
+  double d_reduce = std::clamp(journey_plan, lookahead_dist_min, lookahead_dist_max);
+
   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_;
+      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);
@@ -39,14 +50,49 @@ void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity
   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;
+  drive_cmd.x = (journey_plan) >= 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)
   {
-    carrot_dist2 *= 0.6;
+    robot_nav_2d_msgs::Twist2D cmd_vel, cmd_result;
-    curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
+    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);
+  
 }

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

@@ -81,6 +81,12 @@ namespace mkt_plugins
      * @return Next valid velocity combination
      */
     robot_nav_2d_msgs::Twist2D nextTwist() override;
+
+    /**
+     * @brief Get the current velocity
+     * @return The current velocity
+     */
+    robot_nav_2d_msgs::Twist2D getTwist() override;
     
     /**
      * @brief Generate a trajectory from start pose to goal

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

@@ -54,6 +54,12 @@ public:
    * Should only be called when hasMoreTwists() returns true.
    */
   virtual robot_nav_2d_msgs::Twist2D nextTwist() = 0;
+
+  /**
+   * @brief Get the current velocity
+   * @return The current velocity
+   */
+  virtual robot_nav_2d_msgs::Twist2D getTwist() = 0;
 };  // class VelocityIterator
 
 }  // namespace mkt_plugins

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

@@ -54,6 +54,12 @@ namespace mkt_plugins
      * Automatically iterates to the next valid velocity if current is invalid.
      */
     robot_nav_2d_msgs::Twist2D nextTwist() override;
+
+    /**
+     * @brief Get the current velocity
+     * @return The current velocity (x, y, theta)
+     */
+    robot_nav_2d_msgs::Twist2D getTwist() override;
     
   protected:
     /**

src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp

@@ -166,6 +166,11 @@ namespace mkt_plugins
     return velocity_iterator_->nextTwist();
   }
 
+  robot_nav_2d_msgs::Twist2D StandardTrajectoryGenerator::getTwist()
+  {
+    return velocity_iterator_->getTwist();
+  }
+
   std::vector<double> StandardTrajectoryGenerator::getTimeSteps(const robot_nav_2d_msgs::Twist2D &cmd_vel)
   {
     std::vector<double> steps;

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

@@ -40,6 +40,15 @@ robot_nav_2d_msgs::Twist2D XYThetaIterator::nextTwist()
   return velocity;
 }
 
+robot_nav_2d_msgs::Twist2D XYThetaIterator::getTwist()
+{
+  robot_nav_2d_msgs::Twist2D velocity;
+  velocity.x = x_it_->getVelocity();
+  velocity.y = y_it_->getVelocity();
+  velocity.theta = th_it_->getVelocity();
+  return velocity;
+}
+
 bool XYThetaIterator::isValidVelocity()
 {
   return kinematics_->isValidSpeed(x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity());

src/Navigations/Libraries/robot_actionlib_msgs/CMakeLists.txt

@@ -30,7 +30,6 @@ if (NOT BUILDING_WITH_CATKIN)
 
   set(PACKAGES_DIR  
     robot_std_msgs
-    utils
     robot_time
   )
 
@@ -41,7 +40,6 @@ else()
 # ========================================================
   find_package(catkin REQUIRED COMPONENTS         
     robot_std_msgs
-    utils
     robot_time
   )
 
@@ -50,7 +48,7 @@ else()
   catkin_package(
     INCLUDE_DIRS include
     LIBRARIES ${PROJECT_NAME} 
-    CATKIN_DEPENDS robot_std_msgs utils robot_time
+    CATKIN_DEPENDS robot_std_msgs robot_time
     DEPENDS Boost
   )
 

src/Navigations/Libraries/robot_actionlib_msgs/package.xml

@@ -22,9 +22,6 @@
   <build_depend>robot_std_msgs</build_depend>
   <run_depend>robot_std_msgs</run_depend>
 
-  <build_depend>utils</build_depend>
-  <run_depend>utils</run_depend>
-
   <build_depend>robot_time</build_depend>
   <run_depend>robot_time</run_depend>
 </package>