update docking

config/hybrid_local_planner_params.yaml

@@ -31,8 +31,8 @@ HybridLocalPlanner:
 
     # GoalTolerance
 
-    xy_goal_tolerance: 0.1
-    yaw_goal_tolerance: 0.07
+    xy_goal_tolerance: 0.05
+    yaw_goal_tolerance: 0.05
 
     # Optimization
 

config/maker_sources.yaml

@@ -9,14 +9,14 @@ trolley:
     footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
     delay: 1.5 # Cấm sửa không là không chạy được   
     timeout: 60.0
-    vel_x: 0.25
+    vel_x: 0.2
     vel_theta: 0.3
     yaw_goal_tolerance: 0.015  
     xy_goal_tolerance: 0.015 
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
   qrcode:
     maker_goal_frame: qr_trolley
@@ -30,7 +30,7 @@ trolley:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
 charger:
   plugins:
@@ -41,13 +41,13 @@ charger:
     footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
     delay: 1.5 # Cấm sửa không là không chạy được
     timeout: 60
-    vel_x: 0.1
+    vel_x: 0.15
     yaw_goal_tolerance: 0.015  
     xy_goal_tolerance: 0.015 
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
 dock_station:
   plugins:
@@ -102,7 +102,7 @@ undock_station:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
     
   qrcode:
     maker_goal_frame: qr_trolley
@@ -116,7 +116,7 @@ undock_station:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
 undock_station_2:
   plugins:
@@ -135,7 +135,7 @@ undock_station_2:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4
 
   qrcode:
     maker_goal_frame: qr_trolley
@@ -149,4 +149,4 @@ undock_station_2:
     min_lookahead_dist: 0.4
     max_lookahead_dist: 1.0
     lookahead_time: 1.5
-    angle_threshold: 0.16
+    angle_threshold: 0.4

config/pnkx_local_planner_params.yaml

@@ -48,7 +48,7 @@ LimitedAccelGenerator:
   min_vel_y: 0.0  # diff drive robot
 
   max_speed_xy: 2.0    # max_trans_vel: 0.8  # choose slightly less than the base's capability
-  min_speed_xy: 0.25    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+  min_speed_xy: 0.15    # min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
 
   max_vel_theta: 0.4   # max_rot_vel: 1.0  # choose slightly less than the base's capability
   min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4  # this is the min angular velocity when there is negligible translational velocity

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

@@ -477,6 +477,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   }
   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));
+
   robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
   if (transformed_plan.poses.empty())
   {
@@ -512,7 +513,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
   const double distance_allow_rotate = min_journey_squared_;
   const double path_distance_to_rotate = journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1);
   allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
-
+  allow_rotate &= std::hypot(compute_plan_.poses.front().pose.x - pose.pose.x, compute_plan_.poses.front().pose.y - pose.pose.y) <= 0.1;
   double angle_to_heading;
   if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
   {
@@ -545,10 +546,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
       // {
       //   lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
       // }
-      robot_nav_2d_msgs::Twist2D drive_target;
-      transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
-      carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
-
+      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);
       // Normal Pure Pursuit
       this->computePurePursuit(
       carrot_pose, 
@@ -585,6 +585,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
     break;
   }
 
+  if(fabs(v_max == 0.0))
+    drive_cmd.x = 0.0;
   result.velocity = drive_cmd;
   prevous_drive_cmd_ = drive_cmd;
   return result;
@@ -664,6 +666,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
       // 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
     {
@@ -675,7 +678,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
   {
     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);
@@ -702,8 +705,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(

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

@@ -176,6 +176,8 @@ namespace two_points_planner
     const double dy = goal.pose.position.y - start.pose.position.y;
     const double distance = std::sqrt(dx * dx + dy * dy);
     double theta;
+    // Lấy độ phân giải của costmap
+    double resolution = costmap_robot_->getCostmap()->getResolution();
     
     if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
     {
@@ -189,16 +191,12 @@ namespace two_points_planner
     else 
     { 
       robot_geometry_msgs::PoseStamped pose = start;
-      pose.pose.position.x += 0.01 * cos(theta);
-      pose.pose.position.y += 0.01 * sin(theta);
+      pose.pose.position.x += resolution * cos(theta);
+      pose.pose.position.y += resolution * sin(theta);
       plan.push_back(pose);
       plan.push_back(goal);
       return true;
     }
-
-    // Lấy độ phân giải của costmap
-    double resolution = costmap_robot_->getCostmap()->getResolution();
-
     // Tính số điểm cần chia
     int num_points = std::ceil(distance / resolution);
 

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

@@ -250,17 +250,12 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset()
   if(rotate_algorithm_) rotate_algorithm_->reset();
   ret_nav_ = ret_angle_ = false;
 
-  robot::log_info_at(__FILE__, __LINE__, "Debug");
-  parent_.printParams();
   std::string algorithm_nav_name;
   planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
 
   parent_.setParam(algorithm_nav_name, original_papams_);
   robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name);
-  nh_algorithm.setParam("allow_rotate", false);
-
-  robot::log_info_at(__FILE__, __LINE__, "Debug ở đây");
-  parent_.printParams();
+  // nh_algorithm.setParam("allow_rotate", false);
 }
 
 void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
@@ -455,11 +450,11 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_
     {
       robot::log_info_at(__FILE__, __LINE__, "local_pose %f %f %f", local_pose.pose.x, local_pose.pose.y, local_pose.pose.theta);
       robot::log_info_at(__FILE__, __LINE__, "local_goal %f %f %f", local_goal.pose.x, local_goal.pose.y, local_goal.pose.theta);
-      robot::log_info_at(__FILE__, __LINE__, "goal_pose_ %f %f %f", goal_pose_.pose.x, goal_pose_.pose.y, goal_pose_.pose.theta);
     }
   }
 
-  if (ret_nav_ && !ret_angle_ && !dock_ok)
+  // if (ret_nav_ && !ret_angle_ && !dock_ok)
+  if (ret_nav_ && !ret_angle_)
   {
     double delta_orient =
         fabs(angles::normalize_angle(local_goal.pose.theta - local_pose.pose.theta));
@@ -801,7 +796,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath(
   robot_geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose);
   robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
   std::vector<robot_geometry_msgs::PoseStamped> docking_plan;
-
+  robot::log_info_at(__FILE__, __LINE__, "start %s %f %f", start.header.frame_id.c_str(), start.pose.position.x, start.pose.position.y);
+  robot::log_info_at(__FILE__, __LINE__, "goal %s %f %f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
   if (!docking_planner_->makePlan(start, goal, docking_plan))
   {
     throw robot_nav_core2::LocalPlannerException("Making plan from goal maker is failed");

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

@@ -146,7 +146,7 @@ namespace robot_nav_core
      * @brief Gets the current global plan
      * @param path The global plan
      */
-    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
+    virtual void getGlobalPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) {return;}
 
     /**
      * @brief  Constructs the local planner

src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h

@@ -103,7 +103,7 @@ namespace robot_nav_core2
      * @brief Gets the current global plan
      * @param path The global plan
      */
-    virtual void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) = 0;
+    virtual void getGlobalPlan(robot_nav_2d_msgs::Path2D &path) {return;}
 
     /**
      * @brief Compute the best command given the current pose, velocity and goal

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

@@ -3144,15 +3144,6 @@ robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const ro
   }
 
   std::string global_frame = planner_costmap_robot_->getGlobalFrameID();
-  // robot_geometry_msgs::PoseStamped goal_pose, global_pose;
-  // goal_pose = goal_pose_msg;
-
-  // goal_pose.header.stamp = robot::Time(); // latest available
-  // try
-  // {
-  //   tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time());
-  //   tf3::doTransform(goal_pose, global_pose, transform);
-  // }
   robot_geometry_msgs::PoseStamped global_pose;
   tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
   robot_geometry_msgs::PoseStamped goal_pose;
@@ -3218,17 +3209,16 @@ robot::move_base_core::PlannerDataOutput move_base::MoveBase::getGlobalData()
   {
     robot_nav_msgs::Path path;
     tc_->getGlobalPlan(path.poses);
-    if (!path.poses.empty())
+    robot_nav_msgs::Path global_path;
+    global_path.header.stamp = robot::Time::now();
+    global_path.header.frame_id = planner_costmap_robot_->getGlobalFrameID();
+    for(auto &p : path.poses)
     {
-      path.header.stamp = path.poses[0].header.stamp;
-      path.header.frame_id = path.poses[0].header.frame_id;
+      robot_geometry_msgs::PoseStamped pose = goalToGlobalFrame(p);
+      pose.header.stamp = robot::Time::now();
+      global_path.poses.push_back(goalToGlobalFrame(pose));
     }
-    else
-    {
-      path.header.stamp = robot::Time::now();
-      path.header.frame_id = planner_costmap_robot_->getGlobalFrameID();
-    }
-    global_data_.plan = robot_nav_2d_utils::pathToPath(path);
+    global_data_.plan = robot_nav_2d_utils::pathToPath(global_path);
   }
   ConvertData convert_data(planner_costmap_robot_, planner_costmap_robot_->getGlobalFrameID(), true);
   convert_data.updateCostmap(global_data_.costmap, global_data_.costmap_update, global_data_.is_costmap_updated);