From 498a85a199de48a927485fa62c80a856b65599de Mon Sep 17 00:00:00 2001 From: HiepLM Date: Fri, 24 Apr 2026 11:23:35 +0700 Subject: [PATCH] update lan 1 --- config/move_base_common_params.yaml | 4 ++-- config/pnkx_local_planner_params.yaml | 2 +- .../Cores/score_algorithm/src/score_algorithm.cpp | 8 ++++---- .../src/diff/diff_predictive_trajectory.cpp | 12 +++++++----- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/config/move_base_common_params.yaml b/config/move_base_common_params.yaml index b183fe7..1fa1831 100755 --- a/config/move_base_common_params.yaml +++ b/config/move_base_common_params.yaml @@ -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 diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 8c44f0e..247843f 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -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) diff --git a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp index 46a85a4..beda55c 100644 --- a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp +++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp @@ -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; 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 93a409d..b67c3ac 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 @@ -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;