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

update lan 1

Browse Source
This commit is contained in:
HiepLM
2026-04-24 11:23:35 +07:00
parent d681201698
commit 498a85a199
4 changed files with 14 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
config/move_base_common_params.yaml
View File

@@ -7,7 +7,7 @@ base_global_planner: CustomPlanner
PNKXLocalPlanner:
base_local_planner: LocalPlannerAdapter
base_global_planner: TwoPointsPlanner
base_global_planner: CustomPlanner
PNKXDockingLocalPlanner:
base_local_planner: LocalPlannerAdapter
@@ -26,7 +26,7 @@ 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
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...
max_planning_retries: -1 # ... or after 10 attempts (whichever happens first)
max_planning_retries: 0 # ... or after 10 attempts (whichever happens first)
oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s
oscillation_distance: 0.5
### recovery behaviors

2
config/pnkx_local_planner_params.yaml
View File

@@ -95,7 +95,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
min_lookahead_dist: 0.6 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 1.9 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.2 # Minimum squared journey to consider for goal (default: 0.2)
min_journey_squared: 0.35 # Minimum squared journey to consider for goal (default: 0.2)
max_journey_squared: 0.5 # Maximum squared journey to consider for goal (default: 0.2)
max_lateral_accel: 0.9 # Max lateral accel for speed reduction on curves (m/s^2)

8
src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
View File

@@ -415,7 +415,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
robot_nav_2d_msgs::Pose2DStamped sub_pose;
sub_pose = global_plan.poses[closet_index];
#ifdef SCORE_ALGORITHM_WITH_ROS
#ifdef BUILD_WITH_ROS
{
robot_geometry_msgs::PoseStamped sub_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_pose);
geometry_msgs::PoseStamped sub_pose_stamped_ros;
@@ -423,7 +423,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
sub_pose_stamped_ros.header.frame_id = sub_pose_stamped.header.frame_id;
sub_pose_stamped_ros.pose.position.x = sub_pose_stamped.pose.position.x;
sub_pose_stamped_ros.pose.position.y = sub_pose_stamped.pose.position.y;
sub_pose_stamped_ros.pose.position.z = sub_pose_stamped.pose.position.z;
sub_pose_stamped_ros.pose.position.z = 0.9;
sub_pose_stamped_ros.pose.orientation.x = sub_pose_stamped.pose.orientation.x;
sub_pose_stamped_ros.pose.orientation.y = sub_pose_stamped.pose.orientation.y;
sub_pose_stamped_ros.pose.orientation.z = sub_pose_stamped.pose.orientation.z;
@@ -434,7 +434,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
robot_nav_2d_msgs::Pose2DStamped sub_goal;
sub_goal = global_plan.poses[goal_index];
#ifdef SCORE_ALGORITHM_WITH_ROS
#ifdef BUILD_WITH_ROS
{
robot_geometry_msgs::PoseStamped sub_goal_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_goal);
geometry_msgs::PoseStamped sub_goal_stamped_ros;
@@ -442,7 +442,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
sub_goal_stamped_ros.header.frame_id = sub_goal_stamped.header.frame_id;
sub_goal_stamped_ros.pose.position.x = sub_goal_stamped.pose.position.x;
sub_goal_stamped_ros.pose.position.y = sub_goal_stamped.pose.position.y;
sub_goal_stamped_ros.pose.position.z = sub_goal_stamped.pose.position.z;
sub_goal_stamped_ros.pose.position.z = 0.9;
sub_goal_stamped_ros.pose.orientation.x = sub_goal_stamped.pose.orientation.x;
sub_goal_stamped_ros.pose.orientation.y = sub_goal_stamped.pose.orientation.y;
sub_goal_stamped_ros.pose.orientation.z = sub_goal_stamped.pose.orientation.z;

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

@@ -368,6 +368,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
return false;
}
const auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
if(fabs(carrot_pose.pose.y) > 0.2)
{
@@ -518,10 +519,12 @@ 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 |=
(path_distance_to_rotate >= distance_allow_rotate &&
fabs(transformed_plan.poses.front().pose.y * cos(transformed_plan.poses.front().pose.theta)) <= 0.1);
// allow_rotate &= std::hypot(compute_plan_.poses.front().pose.x - pose.pose.x, compute_plan_.poses.front().pose.y - pose.pose.y) <= 0.1;
// allow_rotate &= fabs(transformed_plan.poses.front().pose.y) <= 0.1;
// robot::log_info("pose.y %f", fabs(transformed_plan.poses.front().pose.y));
// robot::log_info("%f front.y %f", path_distance_to_rotate, fabs(transformed_plan.poses.front().pose.y * cos(transformed_plan.poses.front().pose.theta)));
double angle_to_heading;
if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
{
@@ -723,7 +726,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
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], -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);
robot::log_info("drive_cmd.theta: %f, drive_target.theta: %f", drive_cmd.theta, drive_target.theta);
}
void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -1266,8 +1269,7 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
// nếu đường thẳng
if (max_kappa <= straight_threshold)
{
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_lookahead_dist_ * 0.8)
{
if(fabs(path.poses.back().pose.x) < min_journey_squared_)
drive_cmd.theta = 0.01;