diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index dbc8ca5..238e214 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -51,7 +51,7 @@ LimitedAccelGenerator: min_speed_xy: 0.25 # min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity max_vel_theta: 0.7 # max_rot_vel: 1.0 # choose slightly less than the base's capability - min_vel_theta: 0.07 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity + 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 acc_lim_x: 1.0 acc_lim_y: 0.0 # diff drive robot @@ -102,17 +102,11 @@ MKTAlgorithmDiffPredictiveTrajectory: rot_stopped_velocity: 0.05 trans_stopped_velocity: 0.06 - # Regulated linear velocity scaling - use_regulated_linear_velocity_scaling: false # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true) - # only when true: - regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9) - regulated_linear_scaling_min_speed: 0.15 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25) - - # Inflation cost scaling (Limit velocity by proximity to obstacles) - use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true) - inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0 - cost_scaling_dist: 0.2 # (default: 0.6) - cost_scaling_gain: 2.0 # (default: 1.0) + use_final_heading_alignment: true + final_heading_xy_tolerance: 0.1 + final_heading_angle_tolerance: 0.05 + final_heading_min_velocity: 0.05 + final_heading_kp_angular: 2.0 MKTAlgorithmDiffGoStraight: library_path: libmkt_algorithm_diff diff --git a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt index 7f0ca0e..7b0869a 100644 --- a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt +++ b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt @@ -75,6 +75,10 @@ add_library(${PROJECT_NAME} SHARED if(BUILDING_WITH_CATKIN) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + target_compile_definitions(${PROJECT_NAME} + PUBLIC BUILD_WITH_ROS + ) + target_include_directories(${PROJECT_NAME} PUBLIC $ diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h index 1e6ebd6..e5c72fd 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h @@ -1,8 +1,10 @@ #ifndef _SCORE_ALGORITHM_H_INCLUDED__ #define _SCORE_ALGORITHM_H_INCLUDED__ +#ifdef BUILD_WITH_ROS #include #include +#endif #include #include #include @@ -155,8 +157,10 @@ namespace score_algorithm double old_xy_goal_tolerance_; double angle_threshold_; +#ifdef BUILD_WITH_ROS ros::Publisher current_goal_pub_; ros::Publisher closet_robot_goal_pub_; +#endif std::vector reached_intermediate_goals_; std::vector start_index_saved_vt_; diff --git a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp index 3e5362b..2f48520 100644 --- a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp +++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp @@ -415,6 +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 { robot_geometry_msgs::PoseStamped sub_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_pose); geometry_msgs::PoseStamped sub_pose_stamped_ros; @@ -429,9 +430,11 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs sub_pose_stamped_ros.pose.orientation.w = sub_pose_stamped.pose.orientation.w; closet_robot_goal_pub_.publish(sub_pose_stamped_ros); } +#endif robot_nav_2d_msgs::Pose2DStamped sub_goal; sub_goal = global_plan.poses[goal_index]; +#ifdef SCORE_ALGORITHM_WITH_ROS { robot_geometry_msgs::PoseStamped sub_goal_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_goal); geometry_msgs::PoseStamped sub_goal_stamped_ros; @@ -446,6 +449,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs sub_goal_stamped_ros.pose.orientation.w = sub_goal_stamped.pose.orientation.w; current_goal_pub_.publish(sub_goal_stamped_ros); } +#endif result.header = global_plan.header; for (int i = closet_index; i <= goal_index; i++) diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index 66857c8..b223179 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -210,7 +210,7 @@ namespace mkt_algorithm * @param velocity The velocity of the robot * @param cmd_vel The velocity commands to be filled */ - void rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel); + void rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel, const double &dt); /** * @brief Check if robot should align to final heading (near goal position) @@ -226,7 +226,8 @@ namespace mkt_algorithm const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Twist2D &velocity, double &xy_error, - double &heading_error); + double &heading_error, + const double &sign_x); /** * @brief Arc Motion controller for final heading alignment @@ -251,18 +252,20 @@ namespace mkt_algorithm * @param velocity The velocity of the robot * @param cmd_vel The velocity commands * @param cmd_vel_result The velocity commands result + * @param dt The time step */ void moveWithAccLimits( - const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result); + const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result, const double &dt); /** * @brief Stop the robot taking into account acceleration limits * @param pose The pose of the robot in the global frame * @param velocity The velocity of the robot * @param cmd_vel The velocity commands to be filled + * @param dt The time step * @return True if a valid trajectory was found, false otherwise */ - bool stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel); + bool stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel, const double &dt); /** * @brief Compute pure pursuit @@ -285,6 +288,21 @@ namespace mkt_algorithm const double &dt, robot_nav_2d_msgs::Twist2D &drive_cmd); + /** + * @brief Apply distance-based speed scaling and acceleration limits + * @param plan The plan to be scaled + * @param velocity Current velocity + * @param drive_cmd Input/output velocity command + * @param sign_x Direction of motion + * @param dt Time step + */ + void applyDistanceSpeedScaling( + const robot_nav_2d_msgs::Path2D &plan, + const robot_nav_2d_msgs::Twist2D &velocity, + robot_nav_2d_msgs::Twist2D &drive_cmd, + const double &sign_x, + const double &dt); + /** * @brief Interpolate footprint * @param pose @@ -365,8 +383,9 @@ namespace mkt_algorithm Eigen::MatrixXd Q; Eigen::MatrixXd R; Eigen::MatrixXd P; - +#ifdef BUILD_WITH_ROS ros::Publisher lookahead_point_pub_; +#endif }; // class PredictiveTrajectory diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp index bdd3d18..31322b3 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp @@ -96,7 +96,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( } robot::Time current_time = robot::Time::now(); - double dt = (current_time - last_actuator_update_).toSec(); + const double dt = (current_time - last_actuator_update_).toSec(); last_actuator_update_ = current_time; robot_nav_2d_msgs::Twist2D drive_cmd; 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 96046fc..3af32ea 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 @@ -77,10 +77,12 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize( x0 << 0, 0, 0, 0, 0, 0; kf_->init(robot::Time::now().toSec(), x0); +#ifdef BUILD_WITH_ROS ros::NodeHandle nh_ros; current_goal_pub_ = nh_ros.advertise("current_goal", 10); closet_robot_goal_pub_ = nh_ros.advertise("closet_robot_goal", 10); lookahead_point_pub_ = nh_ros.advertise("lookahead_point", 10); +#endif x_direction_ = y_direction_ = theta_direction_ = 0; this->initialized_ = true; @@ -217,6 +219,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs: robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__); return false; } + if(robot::Time::now() - last_actuator_update_ > robot::Duration(0.5)) + { + last_actuator_update_ = robot::Time::now(); + } std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); if (footprint.size() > 1) @@ -392,7 +398,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( } robot::Time current_time = robot::Time::now(); - double dt = (current_time - last_actuator_update_).toSec(); + const double dt = (current_time - last_actuator_update_).toSec(); last_actuator_update_ = current_time; robot_nav_2d_msgs::Twist2D drive_cmd; @@ -410,7 +416,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( if (transformed_plan.poses.empty()) { robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__); - stopWithAccLimits(pose, velocity, drive_cmd); + stopWithAccLimits(pose, velocity, drive_cmd, dt); result.velocity = drive_cmd; prevous_drive_cmd_ = drive_cmd; return result; @@ -418,6 +424,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( double lookahead_dist = getLookAheadDistance(velocity); auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose); + +#ifdef BUILD_WITH_ROS { geometry_msgs::PoseStamped lookahead_point; lookahead_point.header.stamp = ros::Time::now(); @@ -431,6 +439,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( lookahead_point.pose.orientation.w = carrot_pose_stamped.pose.orientation.w; lookahead_point_pub_.publish(lookahead_point); } +#endif bool allow_rotate = false; nh_priv_.param("allow_rotate", allow_rotate, false); @@ -444,35 +453,37 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( { if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_)) { - if (!stopWithAccLimits(pose, velocity, drive_cmd)) + if (!stopWithAccLimits(pose, velocity, drive_cmd, dt)) return result; } else { - rotateToHeading(angle_to_heading, velocity, drive_cmd); + rotateToHeading(angle_to_heading, velocity, drive_cmd, dt); } } else { - if(fabs(carrot_pose.pose.y) > 0.2) - { - 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); - // === Final Heading Alignment Check === double xy_error = 0.0, heading_error = 0.0; - if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error)) + if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x)) { // Use Arc Motion controller for final heading alignment alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd); - ROS_INFO_THROTTLE(0.5, "[FinalHeadingAlign] xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w=%.3f", - xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, drive_cmd.theta); +#ifdef BUILD_WITH_ROS + ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f", + xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta); +#endif } else { + if(fabs(carrot_pose.pose.y) > 0.2) + { + 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); + // Normal Pure Pursuit this->computePurePursuit( carrot_pose, @@ -484,48 +495,13 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( dt, drive_cmd); } - double s = compute_plan_.poses.empty() - ? min_journey_squared_ - : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1); - double S_th = 0.1 + max_path_distance_ + fabs(velocity.x) * lookahead_time_; - double S_final = std::min(S_th * 0.2, 0.1 + max_path_distance_); - - double max_speed = fabs(drive_cmd.x); - double target_speed = max_speed; - if (s < S_th) - { - double r = std::clamp(s / S_th, 0.0, 1.0); - double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r))); - target_speed = max_speed * cosine_factor; - } - - double reduce_speed = std::min(max_speed, min_speed_xy_); - if (s < S_final) - { - double r = std::clamp(s / S_final, 0.0, 1.0); - double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r))); - reduce_speed = (reduce_speed - min_approach_linear_velocity_) * cosine_factor + min_approach_linear_velocity_; - reduce_speed = std::max(reduce_speed, min_approach_linear_velocity_); - } - target_speed = std::max(target_speed, reduce_speed); - - double v_desired = std::copysign(target_speed, sign_x); - double max_acc_vel = velocity.x + fabs(acc_lim_x_) * dt; - double min_acc_vel = velocity.x - fabs(acc_lim_x_) * dt; - double v_samp = std::clamp(v_desired, min_acc_vel, max_acc_vel); - - double max_speed_to_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, s)); - v_samp = std::copysign(std::min(max_speed_to_stop, fabs(v_samp)), sign_x); - if (sign_x > 0.0) - drive_cmd.x = std::clamp(v_samp, min_approach_linear_velocity_, max_speed); - else - drive_cmd.x = std::clamp(v_samp, -max_speed, -min_approach_linear_velocity_); + applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt); if (this->nav_stop_) { if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_)) { - if (!stopWithAccLimits(pose, velocity, drive_cmd)) + if (!stopWithAccLimits(pose, velocity, drive_cmd, dt)) return result; } else @@ -534,24 +510,24 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( return result; } - // Eigen::VectorXd y(2); - // y << drive_cmd.x, drive_cmd.theta; + Eigen::VectorXd y(2); + y << drive_cmd.x, drive_cmd.theta; - // // Cập nhật lại A nếu dt thay đổi - // for (int i = 0; i < n_; ++i) - // for (int j = 0; j < n_; ++j) - // A(i, j) = (i == j ? 1.0 : 0.0); - // for (int i = 0; i < n_; i += 3) - // { - // A(i, i + 1) = dt; - // A(i, i + 2) = 0.5 * dt * dt; - // A(i + 1, i + 2) = dt; - // } - // kf_->update(y, dt, A); - - // drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); - // drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x); - // drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_); + // Cập nhật lại A nếu dt thay đổi + for (int i = 0; i < n_; ++i) + for (int j = 0; j < n_; ++j) + A(i, j) = (i == j ? 1.0 : 0.0); + for (int i = 0; i < n_; i += 3) + { + A(i, i + 1) = dt; + A(i, i + 2) = 0.5 * dt * dt; + A(i + 1, i + 2) = dt; + } + kf_->update(y, dt, A); + double v_min = min_approach_linear_velocity_; + drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); + drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x); + drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_); } result.poses.clear(); @@ -564,7 +540,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6) break; } - + result.velocity = drive_cmd; prevous_drive_cmd_ = drive_cmd; return result; @@ -603,7 +579,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( v_target *= scale; robot_nav_2d_msgs::Twist2D cmd, result; cmd.x = v_target; - this->moveWithAccLimits(velocity, cmd, result); + this->moveWithAccLimits(velocity, cmd, result, dt); v_target = result.x; } @@ -617,9 +593,21 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( { const double k_heading = 0.3; if (trajectory.poses.size() >= 2) { - const auto& p1 = trajectory.poses[trajectory.poses.size() - 2].pose; - const auto& p2 = trajectory.poses[trajectory.poses.size() - 1].pose; - double heading_ref = std::atan2(p2.y - p1.y, p2.x - p1.x); + const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose; + double heading_ref = 0.0; + for(int i = trajectory.poses.size() - 2; i >= 0; i--) + { + const auto& p = trajectory.poses[i].pose; + if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution()) + { + heading_ref = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x)); + if(sign_x < 0.0) + { + heading_ref = angles::normalize_angle(M_PI + heading_ref); + } + break; + } + } // robot heading in base_link = 0, so error = heading_ref double w_heading = k_heading * angles::normalize_angle(heading_ref); @@ -639,24 +627,80 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( drive_cmd.x = velocity.x + dv; drive_cmd.theta = velocity.theta + dw; + robot::log_info("v_target: %f, w_target: %f, dv: %f, dw: %f, drive_cmd.x: %f, drive_cmd.theta: %f", v_target, w_target, dv, dw, drive_cmd.x, drive_cmd.theta); +} + +void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( + const robot_nav_2d_msgs::Path2D &plan, + const robot_nav_2d_msgs::Twist2D &velocity, + robot_nav_2d_msgs::Twist2D &drive_cmd, + const double &sign_x, + const double &dt) +{ + double s = plan.poses.empty() + ? min_journey_squared_ + : journey(plan.poses, 0, plan.poses.size() - 1); + double S_th = 0.1 + max_path_distance_ + fabs(velocity.x) * lookahead_time_; + double S_final = std::min(S_th * 0.2, 0.1 + max_path_distance_); + + double max_speed = fabs(drive_cmd.x); + double target_speed = max_speed; + if (s < S_th) + { + double r = std::clamp(s / S_th, 0.0, 1.0); + double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r))); + target_speed = max_speed * cosine_factor; + } + + double reduce_speed = std::min(max_speed, min_speed_xy_); + if (s < S_final) + { + double r = std::clamp(s / S_final, 0.0, 1.0); + double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r))); + reduce_speed = (reduce_speed - min_approach_linear_velocity_) * cosine_factor + min_approach_linear_velocity_; + reduce_speed = std::max(reduce_speed, min_approach_linear_velocity_); + } + target_speed = std::max(target_speed, reduce_speed); + + double v_desired = std::copysign(target_speed, sign_x); + double max_acc_vel = velocity.x + fabs(acc_lim_x_) * dt; + double min_acc_vel = velocity.x - fabs(acc_lim_x_) * dt; + double v_samp = std::clamp(v_desired, min_acc_vel, max_acc_vel); + + double max_speed_to_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, s)); + v_samp = std::copysign(std::min(max_speed_to_stop, fabs(v_samp)), sign_x); + if (sign_x > 0.0) + drive_cmd.x = std::clamp(v_samp, min_approach_linear_velocity_, max_speed); + else + drive_cmd.x = std::clamp(v_samp, -max_speed, -min_approach_linear_velocity_); } bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, robot_nav_2d_msgs::Twist2D velocity, double &angle_to_path, const double &sign_x) { - bool curvature = false; - bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_); - - double max_kappa = calculateMaxKappa(global_plan); - const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05)); - curvature = max_kappa > straight_threshold; + + const bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_ + 0.01, trans_stopped_velocity_ + 0.01); + // const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05)); + // const double max_kappa = calculateMaxKappa(global_plan); + // const bool curvature = max_kappa > straight_threshold; double path_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); + if(is_stopped && global_plan.poses.size() >= 2) + { + const auto& p1 = global_plan.poses[1]; + for(int i = 2; i < global_plan.poses.size(); i++) + { + const auto& p = global_plan.poses[i]; + if(std::hypot(p.pose.x, p.pose.y) > costmap_robot_->getCostmap()->getResolution()) + { + path_angle = std::atan2(p.pose.y - p1.pose.y, p.pose.x - p1.pose.x); + break; + } + } + } + // Whether we should rotate robot to rough path heading - double blend = 0.85; // Tỉ lệ blend - angle_to_path = curvature ? blend * path_angle : path_angle; - - angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + angle_to_path) : angle_to_path; + angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : path_angle; double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y); // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) double heading_rotate = rotate_to_heading_min_angle_; @@ -665,10 +709,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( ? rotate_to_heading_min_angle_ + heading_linear : std::clamp(heading_rotate + heading_linear, angle_threshold_ + rotate_to_heading_min_angle_, M_PI_2); - - bool result = use_rotate_to_heading_ && - (is_stopped || sign(angle_to_path) * sign_x < 0 ) && - fabs(angle_to_path) > heading_rotate; + // bool result = use_rotate_to_heading_ && + // (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate; + + bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate; // if (result) // ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); @@ -680,11 +724,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( return result; } -void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel) +void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading( + const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel, const double &dt) { - const double dt = control_duration_; const double theta_direction = angle_to_path > 0.0 ? 1.0 : -1.0; - double vel_yaw = theta_direction * fabs(velocity.theta); double ang_diff = angle_to_path; double max_vel_theta = traj_ ? fabs(traj_->getTwistAngular(true).z) : 0.3; double min_vel_theta = min_vel_theta_; @@ -715,24 +758,21 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an // Apply direction double v_theta_desired = std::copysign(target_angular_speed, ang_diff); - // === ACCELERATION LIMITS === - double max_acc_vel = vel_yaw + fabs(acc_lim_theta_) * dt; - double min_acc_vel = vel_yaw - fabs(acc_lim_theta_) * dt; - double v_theta_samp = std::clamp(v_theta_desired, min_acc_vel, max_acc_vel); - // === STOPPING CONSTRAINT === double max_speed_to_stop = sqrt(2.0 * fabs(decel_lim_theta_) * fabs(ang_diff)); - v_theta_samp = std::copysign( - std::min(max_speed_to_stop, fabs(v_theta_samp)), ang_diff); + + double v_theta_samp = std::copysign( + std::min(max_speed_to_stop, fabs(v_theta_desired)), ang_diff); + + // === ACCELERATION LIMITS === + double max_acc_vel = fabs(acc_lim_theta_) * dt; + double domega = std::clamp(v_theta_samp - velocity.theta, -max_acc_vel, max_acc_vel); // === FINAL LIMITS === - v_theta_samp = theta_direction > 0 - ? std::clamp(v_theta_samp, min_vel_theta, max_vel_theta) - : std::clamp(v_theta_samp, -max_vel_theta, -min_vel_theta); - cmd_vel.x = 0.0; cmd_vel.y = 0.0; - cmd_vel.theta = v_theta_samp; + cmd_vel.theta = velocity.theta + domega; + // robot::log_info("cmd_vel.theta: %f, velocity.theta: %f, domega: %f, max_acc_vel: %f, v_theta_samp: %f", cmd_vel.theta, velocity.theta, domega, max_acc_vel, v_theta_samp); } bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading( @@ -740,7 +780,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading( const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Twist2D &velocity, double &xy_error, - double &heading_error) + double &heading_error, + const double &sign_x) { if (!use_final_heading_alignment_) return false; @@ -754,15 +795,23 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading( // Transform goal heading to robot frame // In robot frame, robot heading is 0, so heading_error = goal_heading_in_robot_frame - robot_nav_2d_msgs::Pose2DStamped goal_robot_frame; - if (tf_ && robot_nav_2d_utils::transformPose(tf_, robot_base_frame_, goal, goal_robot_frame)) + heading_error = 0.0; + if(trajectory.poses.size() >= 2) { - heading_error = angles::normalize_angle(goal_robot_frame.pose.theta); - } - else - { - // Fallback: use last pose heading in trajectory - heading_error = angles::normalize_angle(last_pose.theta); + const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose; + for(int i = trajectory.poses.size() - 2; i >= 0; i--) + { + const auto& p = trajectory.poses[i].pose; + if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution()) + { + heading_error = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x)); + if(sign_x < 0.0) + { + heading_error = angles::normalize_angle(M_PI + heading_error); + } + break; + } + } } // Check if we're close enough to goal position to start final heading alignment @@ -836,7 +885,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading( { // Ideal omega to complete rotation while traveling remaining distance // omega = v * heading_error / xy_error - omega_arc = std::fabs(v_target) * heading_error / xy_error; + omega_arc = std::fabs(v_target - velocity.x) * heading_error / xy_error; } // Blend proportional and arc-based control @@ -844,13 +893,22 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading( double blend_factor = std::clamp(xy_error / final_heading_xy_tolerance_, 0.0, 1.0); omega_target = blend_factor * omega_arc + (1.0 - blend_factor) * omega_p; + // Reduce angular speed as heading error approaches zero + double heading_scale = std::clamp( + abs_heading_error / std::max(final_heading_angle_tolerance_, 1e-3), + 0.0, 1.0); + heading_scale = heading_scale * heading_scale; // smoother near zero + omega_target *= heading_scale; + // Ensure minimum angular velocity for responsiveness double omega_min = min_vel_theta_; - if (std::fabs(omega_target) < omega_min && abs_heading_error > final_heading_angle_tolerance_) + if (std::fabs(omega_target) < omega_min && + abs_heading_error > final_heading_angle_tolerance_ * 3.0) { omega_target = heading_sign * omega_min; } } + // --- Apply acceleration limits --- // Linear @@ -861,7 +919,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading( // Angular double omega_current = velocity.theta; - double domega_max = max_vel_theta_; + double domega_max = acc_lim_theta_ * dt; double domega = std::clamp(omega_target - omega_current, -domega_max, domega_max); cmd_vel.theta = omega_current + domega; @@ -878,8 +936,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading( cmd_vel.y = 0.0; +#ifdef BUILD_WITH_ROS ROS_DEBUG_THROTTLE(0.2, "[FinalHeading] xy=%.3f, heading=%.3f, v=%.3f, omega=%.3f", xy_error, heading_error, cmd_vel.x, cmd_vel.theta); +#endif } double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity) @@ -1400,10 +1460,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( } void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits( - const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result) + const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result, const double &dt) { - const double dt = control_duration_; - // --- X axis --- double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x); double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x); @@ -1428,11 +1486,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits( // cmd_vel_result.theta = vth; } -bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel) +bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel, const double &dt) { // slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible // but we'll use a tenth of a second to be consistent with the implementation of the local planner. - const double dt = control_duration_; double vx = sign(velocity.x) * std::max(0.0, (fabs(velocity.x) - fabs(decel_lim_x_) * dt)); double vy = sign(velocity.y) * std::max(0.0, (fabs(velocity.y) - fabs(decel_lim_y_) * dt)); diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp index ae7241c..c621c02 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp @@ -17,6 +17,7 @@ void mkt_algorithm::diff::RotateToGoal::initialize( nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0); this->getParams(); + last_actuator_update_ = robot::Time::now(); x_direction_ = y_direction_ = theta_direction_ = 0; this->initialized_ = true; robot::log_info("[%s:%d]\n RotateToGoal Initialized successfully", __FILE__, __LINE__); @@ -25,10 +26,12 @@ void mkt_algorithm::diff::RotateToGoal::initialize( void mkt_algorithm::diff::RotateToGoal::reset() { + robot::log_info("[%s:%d]\n RotateToGoal is reset", __FILE__, __LINE__); if (initialized_) { - this->clear(); + this->clear(); x_direction_ = y_direction_ = theta_direction_ = 0; + last_actuator_update_ = robot::Time::now(); } } @@ -47,6 +50,10 @@ bool mkt_algorithm::diff::RotateToGoal::prepare(const robot_nav_2d_msgs::Pose2DS robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size()); return false; } + if(robot::Time::now() - last_actuator_update_ > robot::Duration(0.5)) + { + last_actuator_update_ = robot::Time::now(); + } this->getParams(); frame_id_path_ = global_plan.header.frame_id; goal_ = goal; @@ -65,10 +72,13 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::RotateToGoal::calculator( if (!traj_) return result; - const double dt = control_duration_; + robot::Time current_time = robot::Time::now(); + const double dt = (current_time - last_actuator_update_).toSec(); + last_actuator_update_ = current_time; double vel_yaw = velocity.theta; double ang_diff = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta); - rotateToHeading(ang_diff, velocity, drive_cmd); + rotateToHeading(ang_diff, velocity, drive_cmd, dt); + drive_cmd.x = 0.0; drive_cmd.y = 0.0; result.velocity = drive_cmd; diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp index 0e10f6c..ce00ff3 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -181,7 +181,6 @@ void pnkx_local_planner::PNKXLocalPlanner::getParams(robot::NodeHandle &nh) void pnkx_local_planner::PNKXLocalPlanner::reset() { - robot::log_info_at(__FILE__, __LINE__, "New Goal Received."); this->unlock(); if(traj_generator_) traj_generator_->reset(); if(goal_checker_) goal_checker_->reset();