diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 97e4593..c6fcdd7 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -85,9 +85,9 @@ MKTAlgorithmDiffPredictiveTrajectory: # only when false: lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) # only when true: - min_lookahead_dist: 0.6 # The minimum lookahead distance (m) threshold. (default: 0.3) + min_lookahead_dist: 0.4 # 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) + lookahead_time: 2.0 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2) max_lateral_accel: 2.0 # Max lateral accel for speed reduction on curves (m/s^2) 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 1c7a25d..f3d8c5d 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 @@ -280,11 +280,12 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs: double lookahead_dist = this->getLookAheadDistance(velocity); transform_plan_.poses.clear(); - if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_)) + if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, sqrt(lookahead_dist * lookahead_dist + 0.25), transform_plan_)) { robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__); return false; } + x_direction = x_direction_; y_direction = y_direction_ = 0; theta_direction = theta_direction_; @@ -393,7 +394,13 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( double lookahead_dist = getLookAheadDistance(velocity); auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); - + + if(fabs(carrot_pose.pose.y) > 0.2) + { + lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist); + carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_); + } + bool allow_rotate = false; nh_priv_.param("allow_rotate", allow_rotate, false); @@ -487,6 +494,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( result.poses.reserve(transformed_plan.poses.size()); for (const auto &pose_stamped : transformed_plan.poses) { + if(fabs(pose_stamped.pose.x - carrot_pose.pose.x) < 1e-6 && + fabs(pose_stamped.pose.y - carrot_pose.pose.y) < 1e-6 && + fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6) + break; result.poses.push_back(pose_stamped.pose); } result.velocity = drive_cmd; @@ -587,7 +598,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajecto if (traveled >= preview_dist) break; } - robot::log_info("max_kappa: %f", max_kappa); double v_limit = std::fabs(v_target); if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6) { @@ -634,6 +644,18 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( // 3) Adjust speed using Hermite trajectory curvature + remaining distance double v_target = adjustSpeedWithHermiteTrajectory(velocity, transform_plan_, v_max, sign_x); + const double y_abs = std::fabs(carrot_pose.pose.y); + const double y_soft = 0.1; + if (y_abs > y_soft) + { + double scale = y_soft / y_abs; // y càng lớn => scale càng nhỏ + scale = std::clamp(scale, 0.2, 1.0); // không giảm quá sâu + v_target *= scale; + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = v_target; + this->moveWithAccLimits(velocity, cmd, result); + v_target = result.x; + } robot::log_info("v_max: %f, v_target: %f", v_max , v_target); // 4) Maintain minimum approach speed @@ -881,48 +903,48 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_ auto goal_pose_it = std::prev(global_plan.poses.end()); return goal_pose_it; } + unsigned int goal_index = (unsigned)global_plan.poses.size() - 1; + // double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; + // double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; + // double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x; + // double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y; - double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; - double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; - double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x; - double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y; + // // make sure that atan2 is defined + // double start_angle = atan2(start_direction_y, start_direction_x); + // double end_angle = atan2(end_direction_y, end_direction_x); + // double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2); - // make sure that atan2 is defined - double start_angle = atan2(start_direction_y, start_direction_x); - double end_angle = atan2(end_direction_y, end_direction_x); - double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2); + // for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++) + // { + // if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) + // { + // const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; + // const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; - for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++) - { - if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) - { - const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; - const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; + // if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) + // { + // double current_angle = atan2(current_direction_y, current_direction_x); + // goal_index = i; + // if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_)) + // continue; - if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) - { - double current_angle = atan2(current_direction_y, current_direction_x); - goal_index = i; - if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_)) - continue; - - if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold) - { - goal_index = i; - break; - } - } - } - } + // if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold) + // { + // goal_index = i; + // break; + // } + // } + // } + // } // Find the first pose which is at a distance greater than the lookahead distance auto goal_pose_it = std::find_if( - global_plan.poses.begin(), global_plan.poses.begin() + goal_index, - [&](const auto &ps) - { - return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist; - }); + global_plan.poses.begin(), global_plan.poses.begin() + goal_index, + [&](const auto &ps) + { + return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist; + }); // If the number of poses is not far enough, take the last pose if (goal_pose_it == global_plan.poses.end())