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

update

Browse Source
This commit is contained in:
HiepLM
2026-04-23 20:10:57 +07:00
parent 5812542eaf
commit d681201698
3 changed files with 9 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
config/move_base_common_params.yaml
View File

@@ -7,7 +7,7 @@ base_global_planner: CustomPlanner
PNKXLocalPlanner: PNKXLocalPlanner:
base_local_planner: LocalPlannerAdapter base_local_planner: LocalPlannerAdapter
base_global_planner: CustomPlanner base_global_planner: TwoPointsPlanner
PNKXDockingLocalPlanner: PNKXDockingLocalPlanner:
base_local_planner: LocalPlannerAdapter base_local_planner: LocalPlannerAdapter
@@ -26,11 +26,11 @@ controller_frequency: 30.0 # run controller at 15.0 Hz
controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
planner_frequency: 0.0 # don't continually replan (only when controller failed) planner_frequency: 0.0 # don't continually replan (only when controller failed)
planner_patience: 2.0 # if the first planning attempt failed, abort planning retries after 5.0 s... planner_patience: 2.0 # if the first planning attempt failed, abort planning retries after 5.0 s...
max_planning_retries: 0 # ... or after 10 attempts (whichever happens first) max_planning_retries: -1 # ... or after 10 attempts (whichever happens first)
oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s
oscillation_distance: 0.5 oscillation_distance: 0.5
### recovery behaviors ### recovery behaviors
recovery_behavior_enabled: true recovery_behavior_enabled: false
recovery_behaviors: [ recovery_behaviors: [
{name: aggressive_reset, type: ClearCostmapRecovery}, {name: aggressive_reset, type: ClearCostmapRecovery},
{name: conservative_reset, type: ClearCostmapRecovery}, {name: conservative_reset, type: ClearCostmapRecovery},

2
config/pnkx_local_planner_params.yaml
View File

@@ -1,5 +1,5 @@
yaw_goal_tolerance: 0.03 yaw_goal_tolerance: 0.03
xy_goal_tolerance: 0.02 xy_goal_tolerance: 0.05
min_approach_linear_velocity: 0.05 min_approach_linear_velocity: 0.05
LocalPlannerAdapter: LocalPlannerAdapter:

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

@@ -443,8 +443,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
} }
catch (std::exception &e) catch (std::exception &e)
{ {
robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what()); robot::log_warning_throttle(0.2, "[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
return false; x_direction = x_direction_;
} }
} }
@@ -1262,11 +1262,12 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05)); const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x); drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
drive_cmd.theta = max_vel_theta_; drive_cmd.theta = max_vel_theta_;
// nếu đường thẳng // nếu đường thẳng
if (max_kappa <= straight_threshold) if (max_kappa <= straight_threshold)
{ {
if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_)
if(hypot(path.poses.back().pose.x, path.poses.back().pose.y) * 0.9 < min_lookahead_dist_)
{ {
if(fabs(path.poses.back().pose.x) < min_journey_squared_) if(fabs(path.poses.back().pose.x) < min_journey_squared_)
drive_cmd.theta = 0.01; drive_cmd.theta = 0.01;
@@ -1326,8 +1327,6 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generatePar
return parallel_path; return parallel_path;
} }
robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory( robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
const robot_nav_2d_msgs::Path2D &path, const double &sign_x) const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
{ {