HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update docking

Browse Source
This commit is contained in:
hoangson02
2026-03-25 09:09:58 +00:00
parent 37c7707582
commit 3944447b4d
6 changed files with 15 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
config/hybrid_local_planner_params.yaml
View File

@@ -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

2
config/pnkx_local_planner_params.yaml
View File

@@ -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

13
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
View File

@@ -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(

5
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp
View File

@@ -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));

2
src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h
View File

@@ -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

2
src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h
View File

@@ -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