Merge remote-tracking branch 'origin/3.0' into awc-devel

config/pnkx_local_planner_params.yaml

@@ -106,8 +106,8 @@ MKTAlgorithmDiffPredictiveTrajectory:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.05
-  trans_stopped_velocity: 0.06
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.03
   
   use_final_heading_alignment: true
   final_heading_xy_tolerance: 0.1
@@ -143,8 +143,8 @@ MKTAlgorithmDiffGoStraight:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.05
-  trans_stopped_velocity: 0.06
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.03
   
   use_final_heading_alignment: true
   final_heading_xy_tolerance: 0.1
@@ -180,8 +180,8 @@ MKTAlgorithmDiffRotateToGoal:
   angular_decel_zone: 0.1             
 
   # stoped
-  rot_stopped_velocity: 0.05
-  trans_stopped_velocity: 0.06
+  rot_stopped_velocity: 0.03
+  trans_stopped_velocity: 0.03
   
   use_final_heading_alignment: true
   final_heading_xy_tolerance: 0.1

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

@@ -194,11 +194,11 @@ namespace mkt_algorithm
 
       /**
        * @brief Generate Hermite trajectory
-       * @param pose
+       * @param path
        * @param sign_x
        * @return trajectory
        */
-      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);
+      robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x);
 
       /**
        * @brief Generate Hermite quadratic trajectory
@@ -206,7 +206,7 @@ namespace mkt_algorithm
        * @param sign_x
        * @return trajectory
        */
-      robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);
+      robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x);
 
       /**
        * @brief Should rotate to path

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

@@ -476,7 +476,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     twist = traj_->nextTwist();
   }
   double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
-  drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
+  // drive_cmd.x = std::min(sqrt(twist.x * twist.x),  fabs(v_max));
+  drive_cmd.x = sqrt(twist.x * twist.x);
   
   robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
   if (transformed_plan.poses.empty())
@@ -529,26 +530,23 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
   else
   {
-    // === Final Heading Alignment Check ===
-    double xy_error = 0.0, heading_error = 0.0;
-    if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
-    {
-      // Use Arc Motion controller for final heading alignment
-      alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
-#ifdef BUILD_WITH_ROS
-      ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
-                        heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
-#endif
-    }
-    else
-    {
-      // if(fabs(carrot_pose.pose.y) > 0.2)
+//     // === Final Heading Alignment Check ===
+//     double xy_error = 0.0, heading_error = 0.0;
+//     if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
 //     {
-      //   lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
+//       // Use Arc Motion controller for final heading alignment
+//       alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
+// #ifdef BUILD_WITH_ROS
+//       ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
+//                         heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
+// #endif
 //     }
+//     else
+//     {
       robot_nav_2d_msgs::Twist2D drive_target = drive_cmd;
-      // transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
-      // carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
+      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
+      carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
+      
       // Normal Pure Pursuit
       this->computePurePursuit(
       carrot_pose, 
@@ -559,7 +557,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       sign_x, 
       dt, 
       drive_cmd);
-    }
+    // }
     applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
     if (this->nav_stop_)
     {
@@ -633,52 +631,55 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
     v_target = std::copysign(min_approach_linear_velocity, sign_x);
 
   // 5) Angular speed from curvature
-  double w_target = v_target * kappa + std::copysign(carrot_pose.pose.theta * dt, kappa);
+  double w_target = v_target * kappa;
   if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
   {
-    if (trajectory.poses.size() >= 2) {
-      const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
-      double heading_ref = 0.0;
-      for(int i = trajectory.poses.size() - 2; i >= 0; i--)
-      {
-        const auto& p = trajectory.poses[i].pose;
-        if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution())
-        {
-          heading_ref = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x));
-          if(sign_x < 0.0)
-          {
-            heading_ref = angles::normalize_angle(M_PI + heading_ref);
-          }
-          break;
-        }
-      }
+    // ss << w_target << " ";
+    // if (trajectory.poses.size() >= 2) {
+    //   const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
+    //   double heading_ref = 0.0;
+    //   for(int i = trajectory.poses.size() - 2; i >= 0; i--)
+    //   {
+    //     const auto& p = trajectory.poses[i].pose;
+    //     const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x;
+    //     const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y;
+    //     if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
+    //     {
+    //       if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
+    //         continue;
+    //       heading_ref = angles::normalize_angle(std::atan2(dy, dx));
+    //       break;
+    //     }
+    //   }
 
-      const double error = angles::normalize_angle(heading_ref);
-      double w_heading = 0.0;
-      pid(error, 
-        near_goal_heading_integral_, 
-        near_goal_heading_last_error_, 
-        dt, 
-        final_heading_kp_angular_, 
-        final_heading_ki_angular_, 
-        final_heading_kd_angular_, 
-        w_heading);
-      // Apply acceleration limits
-      double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
-      w_target = velocity.theta + dw_heading;
-      w_target = std::clamp(w_target, -0.05, 0.05);
-    }
-    else
-    {
+    //   const double error = angles::normalize_angle(heading_ref);
+    //   ss << "error: " << error << " ";
+    //   double w_heading = 0.0;
+    //   pid(error, 
+    //     near_goal_heading_integral_, 
+    //     near_goal_heading_last_error_, 
+    //     dt, 
+    //     final_heading_kp_angular_, 
+    //     final_heading_ki_angular_, 
+    //     final_heading_kd_angular_, 
+    //     w_heading);
+    //   // Apply acceleration limits
+    //   double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
+    //   w_target = velocity.theta + dw_heading;
+    //   ss << w_target << " ";
+    // }
+    // else
+    // {
       w_target = 0.0;
       near_goal_heading_was_active_ = false;
-    }
+    // }
   }
   else
   {
     near_goal_heading_was_active_ = false;
   }
-  // w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
+  
+  w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
 
   // 6) Apply acceleration limits (linear + angular)
   const double dv = std::clamp(v_target - velocity.x, -acc_lim_x_ * dt, acc_lim_x_ * dt);
@@ -686,8 +687,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
 
   drive_cmd.x = velocity.x + dv;
   drive_cmd.theta = velocity.theta + dw;
-
-
   Eigen::VectorXd y(2);
   y << drive_cmd.x, drive_cmd.theta;
 
@@ -705,8 +704,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   double v_min = min_approach_linear_velocity_;
   drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target));
   drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
-  // if (kf_filter_angular_)
-  //   drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
+  if (kf_filter_angular_)
+    drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -792,16 +791,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   //   (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate;
 
   bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
-// #ifdef BUILD_WITH_ROS
-  // if (result)
-  //   ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
+#ifdef BUILD_WITH_ROS
+  if (result)
+    ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
 
   // else if(fabs(velocity.x) < min_speed_xy_)
   // {
   //   ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
   //   ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
   // }
-// #endif
+#endif
   return result;
 }
 
@@ -883,13 +882,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading(
     for(int i = trajectory.poses.size() - 2; i >= 0; i--)
     {
       const auto& p = trajectory.poses[i].pose;
-      if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution())
+      const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x;
+      const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y;
+      if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
       {
-        heading_error = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x));
-        if(sign_x < 0.0)
-        {
-          heading_error = angles::normalize_angle(M_PI + heading_error);
-        }
+        heading_error = angles::normalize_angle(std::atan2(dy, dx));
         break;
       }
     }
@@ -1239,17 +1236,17 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
 
   if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng
   { 
-    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.front().pose.x) < (min_lookahead_dist_ + max_path_distance_))
+    if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.back().pose.x) < min_lookahead_dist_)
     {
       return generateParallelPath(path, sign_x);
     }
-    return generateHermiteTrajectory(path.poses.back(), sign_x);
+    return generateHermiteTrajectory(path, sign_x);
   }
   else // nếu đường cong
   {
     if(fabs(drive_cmd.x) < min_speed_xy_)
       drive_cmd.x = std::copysign(min_speed_xy_, sign_x);
-    return generateHermiteQuadraticTrajectory(path.poses.back(), sign_x);
+    return generateHermiteQuadraticTrajectory(path, sign_x);
   }
 }
 
@@ -1279,40 +1276,49 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generatePar
       dx = path.poses[i+1].pose.x - path.poses[i-1].pose.x;
       dy = path.poses[i+1].pose.y - path.poses[i-1].pose.y;
     }
-
+    if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
+      continue;
     double theta = atan2(dy, dx);
     double x_off = p.x - offset_y * sin(theta)*sign_x;
     double y_off = p.y - offset_y * cos(theta)*sign_x;
 
+    parallel_path.poses[i].header = path.poses[i].header;
     parallel_path.poses[i].pose.x = x_off;
     parallel_path.poses[i].pose.y = y_off;
-    parallel_path.poses[i].pose.theta = theta; // hoặc giữ nguyên p.theta
-    parallel_path.poses[i].header = path.poses[i].header;
+    parallel_path.poses[i].pose.theta = sign_x < 0 ? angles::normalize_angle(theta + M_PI) : theta;
   }
-
   return parallel_path;
 }
 
+
+
 robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
-  const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
+  const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
 {
   robot_nav_2d_msgs::Path2D hermite_trajectory;
   hermite_trajectory.poses.clear();
-  hermite_trajectory.header.stamp = pose.header.stamp;
-  hermite_trajectory.header.frame_id = pose.header.frame_id;
+  hermite_trajectory.header = path.header;
 
-  const double x = pose.pose.x;
-  const double y = pose.pose.y;
-  const double theta = pose.pose.theta;
+  if (path.poses.empty())
+    return hermite_trajectory;
+
+  const auto &goal = path.poses.back();
+  if (hermite_trajectory.header.frame_id.empty())
+    hermite_trajectory.header.frame_id = goal.header.frame_id;
+  if (hermite_trajectory.header.stamp.isZero())
+    hermite_trajectory.header.stamp = goal.header.stamp;
+
+  const double x = goal.pose.x;
+  const double y = goal.pose.y;
+  double theta = goal.pose.theta;
   const double L = std::hypot(x, y);
-
   if (L < 1e-6) {
     robot_nav_2d_msgs::Pose2DStamped pose_stamped;
-    pose_stamped.pose.x = 0.0;
-    pose_stamped.pose.y = 0.0;
-    pose_stamped.pose.theta = 0.0;
-    pose_stamped.header.stamp = pose.header.stamp;
-    pose_stamped.header.frame_id = pose.header.frame_id;
+    pose_stamped.pose.x = x;
+    pose_stamped.pose.y = y;
+    pose_stamped.pose.theta = theta;
+    pose_stamped.header.stamp = hermite_trajectory.header.stamp;
+    pose_stamped.header.frame_id = hermite_trajectory.header.frame_id;
     hermite_trajectory.poses.push_back(pose_stamped);
     return hermite_trajectory;
   }
@@ -1346,30 +1352,39 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
     double dx = dh10 * Lnegative + dh01 * x + dh11 * Lnegative * std::cos(theta);
     double dy = dh01 * y + dh11 * Lnegative * std::sin(theta);
 
+    if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
+      continue;
     double heading = std::atan2(dy, dx);
 
-    robot_nav_2d_msgs::Pose2DStamped pose;
-    pose.pose.x = px;
-    pose.pose.y = py;
-    pose.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
-    pose.header.stamp = hermite_trajectory.header.stamp;
-    pose.header.frame_id = hermite_trajectory.header.frame_id;
-    hermite_trajectory.poses.push_back(pose);
+    robot_nav_2d_msgs::Pose2DStamped pose_out;
+    pose_out.pose.x = px;
+    pose_out.pose.y = py;
+    pose_out.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
+    pose_out.header.stamp = hermite_trajectory.header.stamp;
+    pose_out.header.frame_id = hermite_trajectory.header.frame_id;
+    hermite_trajectory.poses.push_back(pose_out);
   }
   return hermite_trajectory;
 }
 
 robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteQuadraticTrajectory(
-  const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
+  const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
 {
   robot_nav_2d_msgs::Path2D trajectory;
   trajectory.poses.clear();
-  trajectory.header.stamp = pose.header.stamp;
-  trajectory.header.frame_id = pose.header.frame_id;
+  trajectory.header = path.header;
+  if (path.poses.empty())
+    return trajectory;
 
-  const double x = pose.pose.x;
-  const double y = pose.pose.y;
-  const double theta = sign_x < 0 ? angles::normalize_angle(pose.pose.theta + M_PI) : pose.pose.theta;
+  const auto &goal = path.poses.back();
+  if (trajectory.header.frame_id.empty())
+    trajectory.header.frame_id = goal.header.frame_id;
+  if (trajectory.header.stamp.isZero())
+    trajectory.header.stamp = goal.header.stamp;
+
+  const double x = goal.pose.x;
+  const double y = goal.pose.y;
+  const double theta = sign_x < 0 ? angles::normalize_angle(goal.pose.theta + M_PI) : goal.pose.theta;
   const double L = std::hypot(x, y);
   if (L < 1e-6)
   {
@@ -1409,6 +1424,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
 
     double dx = 2.0 * ax * t + bx;
     double dy = 2.0 * ay * t + by;
+    if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
+      continue;
     double heading = std::atan2(dy, dx);
 
     robot_nav_2d_msgs::Pose2DStamped pose_out;

src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp

@@ -70,9 +70,8 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam
       double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
       if(fabs(tolerance) <= xy_goal_tolerance_) 
       {
-        robot::log_info_at(__FILE__, __LINE__, "%x %x", fabs(tolerance) <= xy_goal_tolerance_, tolerance * old_xy_goal_tolerance_ < 0);
-        robot::log_info_at(__FILE__, __LINE__, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_);
-        robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
+        robot::log_info_at(__FILE__, __LINE__, "%.3f %.3f %.3f %.3f %.3f", fabs(cos(theta)), fabs(sin(theta)),xy_tolerance, xy_goal_tolerance_, yaw_goal_tolerance_);
+        robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %.3f %.3f %.3f %.3f %.3f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
         return true;
       }
     }

src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp

@@ -194,6 +194,10 @@ namespace two_points_planner
       pose.pose.position.x += resolution * cos(theta);
       pose.pose.position.y += resolution * sin(theta);
       plan.push_back(pose);
+      pose = start;
+      pose.pose.position.x -= resolution * cos(theta);
+      pose.pose.position.y -= resolution * sin(theta);
+      plan.push_back(pose);
       plan.push_back(goal);
       return true;
     }

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

@@ -976,6 +976,7 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
     
     robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
     as_->processGoal(action_goal);
+
     robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
   }
   catch (const std::exception &e)
@@ -1116,7 +1117,6 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
       lock.unlock();
       return false;
     }
-
     as_->processGoal(action_goal);
   }
   catch (const std::exception &e)
@@ -1278,7 +1278,6 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
     }
     
     robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
-    as_->processGoal(action_goal);
     robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
   }
   catch (const std::exception &e)
@@ -1424,7 +1423,6 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
       lock.unlock();
       return false;
     }
-
     as_->processGoal(action_goal);
   }
   catch (const std::exception &e)