From 3944447b4d60b605a092ada31c47a3a8ced568f3 Mon Sep 17 00:00:00 2001 From: hoangson02 Date: Wed, 25 Mar 2026 09:09:58 +0000 Subject: [PATCH] update docking --- config/hybrid_local_planner_params.yaml | 4 ++-- config/pnkx_local_planner_params.yaml | 2 +- .../src/diff/diff_predictive_trajectory.cpp | 13 +++++++------ .../src/pnkx_docking_local_planner.cpp | 5 +++-- .../include/robot_nav_core/base_local_planner.h | 2 +- .../include/robot_nav_core2/local_planner.h | 2 +- 6 files changed, 15 insertions(+), 13 deletions(-) diff --git a/config/hybrid_local_planner_params.yaml b/config/hybrid_local_planner_params.yaml index 43b510e..d239c09 100644 --- a/config/hybrid_local_planner_params.yaml +++ b/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 diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 581f117..de01e63 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/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 diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 1843f90..04a0f96 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -546,9 +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, @@ -666,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 { @@ -677,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); @@ -704,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( diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp index d1a141a..8f79329 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp @@ -255,7 +255,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset() parent_.setParam(algorithm_nav_name, original_papams_); robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name); - nh_algorithm.setParam("allow_rotate", false); + // 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) @@ -453,7 +453,8 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_ } } - 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)); diff --git a/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h index 6225cdb..d302731 100755 --- a/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h +++ b/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 &path) = 0; + virtual void getGlobalPlan(std::vector &path) {return;} /** * @brief Constructs the local planner diff --git a/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h index 55802a8..559f58b 100755 --- a/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h +++ b/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