update khi lui

CATKIN_BUILD_README.md

@@ -20,7 +20,11 @@ The specified base path contains a CMakeLists.txt but "catkin_make" must be invo
 
 # Build trong workspace mới
 cd ../pnkx_nav_catkin_ws
-catkin_make
+rm -rf build devel
+catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo \
+  -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer" \
+  -DCMAKE_C_FLAGS="-fsanitize=address -fno-omit-frame-pointer" \
+  -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=address"
 source devel/setup.bash
 ```
 

config/pnkx_local_planner_params.yaml

@@ -75,7 +75,7 @@ LimitedAccelGenerator:
 MKTAlgorithmDiffPredictiveTrajectory:
   library_path: libmkt_algorithm_diff
   xy_local_goal_tolerance: 0.02
-  angle_threshold: 0.47
+  angle_threshold: 0.6
   index_samples: 60
   follow_step_path: true
 

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

@@ -131,19 +131,19 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
   auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
   robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose);
   
-  // === 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("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
-                      xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
-#endif
-  }
-  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("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
+//                       xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
+// #endif
+//   }
+//   else
+//   {
     // robot::log_info_at(__FILE__, __LINE__, "journey : %f lookahead_dist : %f", 
     //   journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1), lookahead_dist);
     if(fabs(carrot_pose.pose.y) > 0.2)
@@ -164,7 +164,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
     sign_x, 
     dt, 
     drive_cmd);
-  }
+  // }
   applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
 
   if (this->nav_stop_)

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

@@ -682,7 +682,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
       double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
       ss << "dw_heading " << dw_heading << " ";
       w_target = velocity.theta + dw_heading;
-      // w_target = std::clamp(w_target, -0.03, 0.03);
+      w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
     }
     else
     {
@@ -722,7 +722,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   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_);
+    drive_cmd.theta = std::clamp(kf_->state()[3], -fabs(drive_target.theta), fabs(drive_target.theta));
+  robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta);
 }
 
 void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -1265,8 +1266,9 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
   // nếu đường thẳng
   if (max_kappa <= straight_threshold) 
   { 
-    if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_)
+    if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_ && fabs(path.poses.back().pose.theta) < 0.1)
     {
+      drive_cmd.theta = 0.01;
       return generateParallelPath(path, sign_x);
     }
     return generateHermiteTrajectory(path, sign_x);
@@ -1277,12 +1279,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
     const double v_min = std::min(fabs(v_limited), min_speed_xy_);
     if(fabs(drive_cmd.x) < v_min) 
     {
-      robot_nav_2d_msgs::Twist2D vel_in, v_out;
-      vel_in.x = v_min;
-      vel_in.y = 0.0;
-      vel_in.theta = drive_cmd.theta;
-      moveWithAccLimits(velocity, vel_in, v_out, dt);
-      drive_cmd.x = std::copysign(v_out.x, sign_x);
+      drive_cmd.x = std::copysign(v_min, sign_x);
     }
     return generateHermiteQuadraticTrajectory(path, sign_x);
   }