From 620db96de04ef68afd6196955e9827372b192934 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Wed, 28 Jan 2026 09:08:55 +0700 Subject: [PATCH] update 28/2 --- config/pnkx_local_planner_params.yaml | 10 +- .../Libraries/mkt_algorithm/CMakeLists.txt | 2 +- .../diff/diff_predictive_trajectory_.h | 314 ++++ .../include/mkt_algorithm/diff/pure_pursuit.h | 8 +- .../mkt_algorithm/src/diff/Hermite.md | 121 ++ .../src/diff/diff_predictive_trajectory.cpp | 19 +- .../src/diff/diff_predictive_trajectory_.cpp | 1307 +++++++++++++++++ .../src/diff/hermite_trajectory.svg | 40 + .../mkt_algorithm/src/diff/pure_pursuit.cpp | 155 +- .../src/pnkx_local_planner.cpp | 4 +- 10 files changed, 1928 insertions(+), 52 deletions(-) create mode 100644 src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory_.h create mode 100644 src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md create mode 100644 src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory_.cpp create mode 100644 src/Algorithms/Libraries/mkt_algorithm/src/diff/hermite_trajectory.svg diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 3667330..97e4593 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -1,4 +1,3 @@ - LocalPlannerAdapter: library_path: liblocal_planner_adapter yaw_goal_tolerance: 0.017 @@ -77,7 +76,7 @@ MKTAlgorithmDiffPredictiveTrajectory: library_path: libmkt_algorithm_diff avoid_obstacles: false xy_local_goal_tolerance: 0.01 - angle_threshold: 0.6 + angle_threshold: 0.4 index_samples: 60 follow_step_path: true @@ -88,9 +87,10 @@ MKTAlgorithmDiffPredictiveTrajectory: # only when true: 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.6 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + lookahead_time: 1.9 # 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) # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) @@ -141,9 +141,9 @@ MKTAlgorithmDiffGoStraight: # 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: 1.0 # The minimum lookahead distance (m) threshold. (default: 0.3) + min_lookahead_dist: 0.325 # The minimum lookahead distance (m) threshold. (default: 0.3) max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9) - lookahead_time: 2.5 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + lookahead_time: 1.9 # 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.6 # Minimum squared journey to consider for goal (default: 0.2) diff --git a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt index 792d5da..33fb380 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt @@ -81,7 +81,7 @@ endif() # Libraries # ======================================================== add_library(${PROJECT_NAME}_diff SHARED - src/diff/diff_predictive_trajectory.cpp + src/diff/diff_predictive_trajectory_.cpp src/diff/diff_rotate_to_goal.cpp src/diff/diff_go_straight.cpp # src/diff/pure_pursuit.cpp 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 new file mode 100644 index 0000000..f12c23b --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory_.h @@ -0,0 +1,314 @@ +#ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ +#define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace mkt_algorithm +{ + namespace diff + { + /** + * @class PredictiveTrajectory + * @brief Standard PredictiveTrajectory-like ScoreAlgorithm + */ + class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm + { + public: + PredictiveTrajectory() : initialized_(false), nav_stop_(false) {}; + + virtual ~PredictiveTrajectory(); + + // Standard ScoreAlgorithm Interface + /** + * @brief Initialize parameters as needed + * @param nh NodeHandle to read parameters from + */ + virtual void initialize( + robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; + + /** + * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories + * + * Subclasses may overwrite. Return false in case there is any error. + * + * @param pose Current pose (costmap frame) + * @param velocity Current velocity + * @param goal The final goal (costmap frame) + * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points + */ + virtual bool prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan, + double &x_direction, double &y_direction, double &theta_direction) override; + + /** + * @brief Calculating algorithm + * @param pose + * @param velocity + * @param traj + */ + virtual mkt_msgs::Trajectory2D calculator( + const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; + + /** + * @brief Reset all data + */ + virtual void reset() override; + + /** + * @brief Stoping move navigation + */ + virtual void stop() override; + + /** + * @brief resume move navigation after stopped + */ + virtual void resume() override; + + /** + * @brief Create a new PredictiveTrajectory instance + * @return A pointer to the new PredictiveTrajectory instance + */ + static score_algorithm::ScoreAlgorithm::Ptr create(); + + protected: + inline double sign(double x) + { + return x < 0.0 ? -1.0 : 1.0; + } + + /** + * @brief Initialize parameters + */ + virtual void getParams(); + + /** + * @brief Dynamically adjust look ahead distance based on the speed + * @param velocity + * @return look ahead distance + */ + double getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity); + + /** + * @brief Get lookahead point on the global plan + * @param lookahead_dist + * @param global_plan + * @return lookahead point + */ + std::vector::iterator + getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan); + + /** + * @brief Prune global plan + * @param tf + * @param pose + * @param global_plan + * @param dist_behind_robot + * @return true if plan is pruned, false otherwise + */ + bool pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, + robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot); + + /** + * @brief Transforms the global plan of the robot from the planner frame to the local frame (modified). + * + * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h + * such that the index of the current goal pose is returned as well as + * the transformation between the global plan and the planning frame. + * @param tf A reference to a tf buffer + * @param global_plan The plan to be transformed + * @param pose The pose of the robot + * @param costmap A reference to the costmap being used so the window size for transforming can be computed + * @param global_frame The frame to transform the plan to + * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!] + * @param[out] transformed_plan Populated with the transformed plan + * @return \c true if the global plan is transformed, \c false otherwise + */ + bool transformGlobalPlan( + TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose, + const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length, + robot_nav_2d_msgs::Path2D &transformed_plan); + + robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose); + + /** + * @brief Should rotate to path + * @param carrot_pose + * @param angle_to_path + * @return true if should rotate, false otherwise + */ + bool shouldRotateToPath( + const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x); + + /** + * @brief Rotate to heading + * @param angle_to_path + * @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); + + /** + * @brief the robot is moving acceleration limits + * @param velocity The velocity of the robot + * @param cmd_vel The velocity commands + * @param cmd_vel_result The velocity commands result + */ + 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); + + /** + * @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 + * @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); + + /** + * @brief Apply constraints + * @param dist_error + * @param lookahead_dist + * @param curvature + * @param curr_speed + * @param pose_cost + * @param linear_vel + * @param sign + */ + void applyConstraints( + const double &dist_error, const double &lookahead_dist, + const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed, + const double &pose_cost, double &linear_vel, const double &sign_x); + + void computePurePursuit( + const score_algorithm::TrajectoryGenerator::Ptr &traj, + const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, + const robot_nav_2d_msgs::Twist2D &velocity, + const double &min_approach_linear_velocity, + const double &journey_plan, + const double &sign_x, + const double &lookahead_dist_min, + const double &lookahead_dist_max, + const double &lookahead_dist, + const double &lookahead_time, + const double &dt, + robot_nav_2d_msgs::Twist2D &drive_cmd + ); + + double adjustSpeedWithHermiteTrajectory( + const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Path2D &trajectory, + double v_target, + const double &sign_x + ); + + std::vector interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector footprint, double resolution); + + /** + * @brief Cost at pose + * @param x + * @param y + * @return cost + */ + double costAtPose(const double &x, const double &y); + + double computeDecelerationFactor(double remaining_distance, double decel_distance); + + double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan); + + double estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan); + + + std::vector angle_history_; + size_t window_size_; + + bool initialized_; + bool nav_stop_; + robot::NodeHandle nh_, nh_priv_; + std::string frame_id_path_; + std::string robot_base_frame_; + + robot_nav_2d_msgs::Pose2DStamped goal_; + robot_nav_2d_msgs::Path2D global_plan_; + robot_nav_2d_msgs::Path2D compute_plan_; + robot_nav_2d_msgs::Path2D transform_plan_; + robot_nav_2d_msgs::Twist2D prevous_drive_cmd_; + + double x_direction_, y_direction_, theta_direction_; + double max_robot_pose_search_dist_; + double global_plan_prune_distance_{1.0}; + + // Lookahead + bool use_velocity_scaled_lookahead_dist_; + double lookahead_time_; + double lookahead_dist_; + double min_lookahead_dist_; + double max_lookahead_dist_; + double max_lateral_accel_; + + // journey + double min_journey_squared_{1e9}; + double max_journey_squared_{1e9}; + + // Rotate to heading + bool use_rotate_to_heading_; + double rotate_to_heading_min_angle_; + + double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_; + double min_path_distance_, max_path_distance_; + + // Regulated linear velocity scaling + bool use_regulated_linear_velocity_scaling_; + double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_; + double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_; + + double rot_stopped_velocity_, trans_stopped_velocity_; + + double min_approach_linear_velocity_; + double regulated_linear_scaling_min_radius_; + double regulated_linear_scaling_min_speed_; + + bool use_cost_regulated_linear_velocity_scaling_; + double inflation_cost_scaling_factor_; + double cost_scaling_dist_, cost_scaling_gain_; + + double cost_left_goal_, cost_right_goal_; + double cost_left_side_ , cost_right_side_; + double center_cost_; + + // Control frequency + double control_duration_; + std::vector footprint_spec_; + + robot::Time last_actuator_update_; + boost::shared_ptr kf_; + int m_, n_; + Eigen::MatrixXd A; + Eigen::MatrixXd C; + Eigen::MatrixXd Q; + Eigen::MatrixXd R; + Eigen::MatrixXd P; + + }; // class PredictiveTrajectory + + } // namespace diff + +} // namespace mkt_algorithm + +#endif //_NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h index 9d03e91..20d5d86 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h @@ -3,8 +3,6 @@ #include #include - -#include #include #include #include @@ -17,11 +15,9 @@ namespace mkt_algorithm { namespace diff { - class PurePursuit : public score_algorithm::ScoreAlgorithm + class PurePursuit { public: - PurePursuit(); - ~PurePursuit(); void computePurePursuit( const score_algorithm::TrajectoryGenerator::Ptr &traj, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, @@ -33,6 +29,7 @@ namespace mkt_algorithm const double &lookahead_dist_max, const double &lookahead_dist, const double &lookahead_time, + const double &dt, robot_nav_2d_msgs::Twist2D &drive_cmd ); @@ -52,5 +49,4 @@ namespace mkt_algorithm }; } } - #endif \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md b/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md new file mode 100644 index 0000000..59665b6 --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md @@ -0,0 +1,121 @@ +## Hermite (quỹ đạo 2D) từ robot đến look-ahead pose + +![Hermite trajectory](hermite_trajectory.svg) + +### Bài toán +Cho pose đích trong hệ tọa độ robot: `goal = (x, y, theta)`, với robot ở gốc +`(0, 0, 0)` (hướng theo trục +x). Cần sinh quỹ đạo từ robot đến pose đích. + +### Ý tưởng +Dùng đường cong Hermite bậc 3 để đảm bảo: +- Đi qua đúng điểm đầu và cuối. +- Đúng hướng đầu và cuối. + +Đường cong tham số `t` từ `0` đến `1`: + +``` +p(t) = (x(t), y(t)) +``` + +Ràng buộc: +- p(0) = (0, 0), hướng 0 rad. +- p(1) = (x, y), hướng theta rad. + +### Hệ số Hermite +``` +h00 = 2t^3 - 3t^2 + 1 +h10 = t^3 - 2t^2 + t +h01 = -2t^3 + 3t^2 +h11 = t^3 - t^2 +``` + +### Đạo hàm đầu/cuối +Chọn độ dài đặc trưng `L` (thường L = sqrt(x^2 + y^2)): +``` +p'(0) = (L, 0) +p'(1) = (L*cos(theta), L*sin(theta)) +``` + +### Công thức quỹ đạo +``` +x(t) = h10*L + h01*x + h11*L*cos(theta) +y(t) = h01*y + h11*L*sin(theta) +``` + +### Hướng (heading) trên đường cong +Đạo hàm: +``` +h00' = 6t^2 - 6t +h10' = 3t^2 - 4t + 1 +h01' = -6t^2 + 6t +h11' = 3t^2 - 2t + +dx = h10'*L + h01'*x + h11'*L*cos(theta) +dy = h01'*y + h11'*L*sin(theta) +``` +Hướng: +``` +heading(t) = atan2(dy, dx) +``` + +### Cách sử dụng +Lấy mẫu `t` đều (ví dụ 50-200 điểm) để tạo danh sách điểm quỹ đạo. +Nếu cần độ cong, có thể tính thêm từ đạo hàm bậc 2. + +### Ví dụ code C++ +``` +#include +#include + +struct Pose2D { + double x; + double y; + double theta; +}; + +std::vector generateHermiteTrajectory( + double x, double y, double theta, + int samples = 100) +{ + std::vector path; + path.reserve(samples + 1); + + double L = std::sqrt(x * x + y * y); + if (L < 1e-6) { + path.push_back({0.0, 0.0, 0.0}); + return path; + } + + for (int i = 0; i <= samples; ++i) { + double t = static_cast(i) / samples; + double t2 = t * t; + double t3 = t2 * t; + + double h00 = 2 * t3 - 3 * t2 + 1; + double h10 = t3 - 2 * t2 + t; + double h01 = -2 * t3 + 3 * t2; + double h11 = t3 - t2; + + double px = h10 * L + h01 * x + h11 * (L * std::cos(theta)); + double py = h01 * y + h11 * (L * std::sin(theta)); + + double dh00 = 6 * t2 - 6 * t; + double dh10 = 3 * t2 - 4 * t + 1; + double dh01 = -6 * t2 + 6 * t; + double dh11 = 3 * t2 - 2 * t; + + double dx = dh10 * L + dh01 * x + dh11 * (L * std::cos(theta)); + double dy = dh01 * y + dh11 * (L * std::sin(theta)); + + double heading = std::atan2(dy, dx); + + path.push_back({px, py, heading}); + } + + return path; +} +``` + +### Ghi chú +- Nếu (x, y) rất gần 0 thì quỹ đạo suy biến, chỉ trả về điểm gốc. +- L có thể điều chỉnh lớn hơn để quỹ đạo mềm hơn. \ No newline at end of file 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 d555b69..4dbab9f 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 @@ -10,7 +10,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize( if (!initialized_) { nh_ = robot::NodeHandle("~/"); - nh_priv_ = robot::NodeHandle("~/" + name); + nh_priv_ = robot::NodeHandle(nh, name); name_ = name; tf_ = tf; traj_ = traj; @@ -385,8 +385,20 @@ 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); + result.velocity = drive_cmd; + prevous_drive_cmd_ = drive_cmd; return result; } + else + { + result.poses.clear(); + result.poses.reserve(transformed_plan.poses.size()); + for (const auto &pose_stamped : transformed_plan.poses) + { + result.poses.push_back(pose_stamped.pose); + } + } double lookahead_dist = getLookAheadDistance(velocity); double tolerance = hypot(pose.pose.x - compute_plan_.poses.front().pose.x, pose.pose.y - compute_plan_.poses.front().pose.y); @@ -407,7 +419,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1); allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; - double angle_to_heading; if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x)) { @@ -1185,4 +1196,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create() { return std::make_shared(); -} \ No newline at end of file +} + +BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory) \ No newline at end of file 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 new file mode 100644 index 0000000..1c7a25d --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory_.cpp @@ -0,0 +1,1307 @@ +#include +#include +#include + +mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {} + +void mkt_algorithm::diff::PredictiveTrajectory::initialize( + robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) +{ + if (!initialized_) + { + nh_ = robot::NodeHandle("~/"); + nh_priv_ = robot::NodeHandle(nh, name); + name_ = name; + tf_ = tf; + traj_ = traj; + costmap_robot_ = costmap_robot; + this->getParams(); + nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); + + std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); + if (footprint.size() > 1) + { + double min_length = 1e10; + double max_length = 0.0; + for (size_t i = 0; i < footprint.size(); ++i) + { + const robot_geometry_msgs::Point &p1 = footprint[i]; + const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point + double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0; + if (len > max_length) + { + max_length = len; + } + if (len < min_length) + { + min_length = len; + } + } + this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; + this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; + } + + // kalman + last_actuator_update_ = robot::Time::now(); + n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta] + m_ = 2; // measurements: x, y, theta + double dt = control_duration_; + + // Khởi tạo ma trận + A = Eigen::MatrixXd::Identity(n_, n_); + C = Eigen::MatrixXd::Zero(m_, n_); + Q = Eigen::MatrixXd::Zero(n_, n_); + R = Eigen::MatrixXd::Identity(m_, m_); + P = Eigen::MatrixXd::Identity(n_, n_); + + 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; + } + + C(0, 0) = 1; + C(1, 3) = 1; + Q(2, 2) = 0.1; + Q(5, 5) = 0.6; + + R(0, 0) = 0.1; + R(1, 1) = 0.2; + + P(3, 3) = 0.4; + P(4, 4) = 0.4; + P(5, 5) = 0.4; + + kf_ = boost::make_shared(dt, A, C, Q, R, P); + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(robot::Time::now().toSec(), x0); + + + + x_direction_ = y_direction_ = theta_direction_ = 0; + this->initialized_ = true; + robot::log_info("[%s:%d]\n PredictiveTrajectory Initialized successfully", __FILE__, __LINE__); + } +} + +void mkt_algorithm::diff::PredictiveTrajectory::getParams() +{ + robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link")); + nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); + nh_priv_.param("angle_threshold", angle_threshold_, M_PI / 8); + nh_priv_.param("index_samples", index_samples_, 0); + nh_priv_.param("follow_step_path", follow_step_path_, false); + + nh_priv_.param("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false); + nh_priv_.param("lookahead_dist", lookahead_dist_, 0.25); + nh_priv_.param("min_lookahead_dist", min_lookahead_dist_, 0.3); + nh_priv_.param("max_lookahead_dist", max_lookahead_dist_, 0.9); + nh_priv_.param("lookahead_time", lookahead_time_, 1.5); + nh_priv_.param("min_journey_squared", min_journey_squared_, std::numeric_limits::infinity()); + nh_priv_.param("max_journey_squared", max_journey_squared_, std::numeric_limits::infinity()); + + nh_priv_.param("use_rotate_to_heading", use_rotate_to_heading_, false); + nh_priv_.param("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785); + + nh_priv_.param("rot_stopped_velocity", rot_stopped_velocity_, 0.0); + nh_priv_.param("trans_stopped_velocity", trans_stopped_velocity_, 0.0); + if (trans_stopped_velocity_ <= min_approach_linear_velocity_ || trans_stopped_velocity_ - min_approach_linear_velocity_ < 0.02) + trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.05; + + // Regulated linear velocity scaling + nh_priv_.param("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false); + nh_priv_.param("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9); + nh_priv_.param("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25); + + // Inflation cost scaling (Limit velocity by proximity to obstacles) + nh_priv_.param("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true); + nh_priv_.param("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0); + nh_priv_.param("cost_scaling_dist", cost_scaling_dist_, 0.6); + nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); + nh_priv_.param("max_lateral_accel", max_lateral_accel_, 1.0); + if (inflation_cost_scaling_factor_ <= 0.0) + { + robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__); + use_cost_regulated_linear_velocity_scaling_ = false; + } + double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); + control_duration_ = 1.0 / control_frequency; + window_size_ = (size_t)(control_frequency * 3.0); + + if (traj_) + { + traj_.get()->getNodeHandle().param("max_vel_x", max_vel_x_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_x", min_vel_x_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_x", acc_lim_x_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_x", decel_lim_x_, 0.0); + + traj_.get()->getNodeHandle().param("max_vel_y", max_vel_y_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_y", min_vel_y_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_y", acc_lim_y_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_y", decel_lim_y_, 0.0); + + traj_.get()->getNodeHandle().param("max_vel_theta", max_vel_theta_, 0.0); + traj_.get()->getNodeHandle().param("min_vel_theta", min_vel_theta_, 0.0); + traj_.get()->getNodeHandle().param("acc_lim_theta", acc_lim_theta_, 0.0); + traj_.get()->getNodeHandle().param("decel_lim_theta", decel_lim_theta_, 0.0); + } +} + +void mkt_algorithm::diff::PredictiveTrajectory::reset() +{ + if (this->initialized_) + { + robot::log_info("[%s:%d]\n PredictiveTrajectory is reset", __FILE__, __LINE__); + reached_intermediate_goals_.clear(); + start_index_saved_vt_.clear(); + this->clear(); + x_direction_ = y_direction_ = theta_direction_ = 0; + this->follow_step_path_ = false; + this->nav_stop_ = false; + last_actuator_update_ = robot::Time::now(); + prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); + + if (kf_) + { + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(robot::Time::now().toSec(), x0); + } + } +} + +void mkt_algorithm::diff::PredictiveTrajectory::stop() +{ + this->nav_stop_ = true; +} + +void mkt_algorithm::diff::PredictiveTrajectory::resume() +{ + if (this->initialized_) + { + if(!this->nav_stop_) + return; + prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); + last_actuator_update_ = robot::Time::now(); + + if (kf_) + { + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(robot::Time::now().toSec(), x0); + } + this->nav_stop_ = false; + } +} + +bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan, + double &x_direction, double &y_direction, double &theta_direction) +{ + if (!initialized_) + { + robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__); + return false; + } + + std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); + if (footprint.size() > 1) + { + double min_length = 1e10; + double max_length = 0.0; + for (size_t i = 0; i < footprint.size(); ++i) + { + const robot_geometry_msgs::Point &p1 = footprint[i]; + const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point + double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0; + if (len > max_length) + { + max_length = len; + } + if (len < min_length) + { + min_length = len; + } + } + this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; + this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; + } + + if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2) + { + 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; + } + this->getParams(); + + frame_id_path_ = global_plan.header.frame_id; + goal_ = goal; + global_plan_ = global_plan; + + // prune global plan to cut off parts of the past (spatially before the robot) + if (!this->pruneGlobalPlan(tf_, pose, global_plan_, 1.0)) + { + robot::log_error("[%s:%d]\n pruneGlobalPlan Failed", __FILE__, __LINE__); + return false; + } + + double S = std::numeric_limits::infinity(); + S = std::max(costmap_robot_->getCostmap()->getSizeInCellsX() * costmap_robot_->getCostmap()->getResolution() / 2.0, + costmap_robot_->getCostmap()->getSizeInCellsY() * costmap_robot_->getCostmap()->getResolution() / 2.0); + const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_; + S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S); + compute_plan_.poses.clear(); + + if ((unsigned int)global_plan_.poses.size() == 2) + { + double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x; + double dy = global_plan_.poses.back().pose.y - global_plan_.poses.front().pose.y; + if (hypot(dx, dy) <= 2.0 * costmap_robot_->getCostmap()->getResolution()) + { + compute_plan_ = global_plan_; + } + else + { + robot::log_error("[%s:%d]\n The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution()); + return false; + } + } + else + { + unsigned int start_index, goal_index; + if (!this->computePlanCommand(pose, velocity, S, global_plan_, start_index, goal_index, compute_plan_)) + { + robot::log_error("[%s:%d]\n computePlanCommand is Failed", __FILE__, __LINE__); + return false; + } + } + + 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_)) + { + 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_; + + if ((unsigned int)(compute_plan_.poses.size() > 1) && compute_plan_.poses.back().header.seq != global_plan_.poses.back().header.seq) + { + const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose; + int index; + for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--) + { + robot_geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose; + if (robot_nav_2d_utils::poseDistance(p2, pose) > xy_local_goal_tolerance_) + break; + } + const robot_geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose; + if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) + { + const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x); + const double dir_path = cos(fabs(path_angle - p2.theta)); + if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) + x_direction = dir_path > 0 ? FORWARD : BACKWARD; + } + } + else + { + try + { + auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_); + auto prev_carrot_pose_it = transform_plan_.poses.begin(); + double distance_it = 0; + for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it) + { + double dx = it->pose.x - carrot_pose_it->pose.x; + double dy = it->pose.y - carrot_pose_it->pose.y; + distance_it += std::hypot(dx, dy); + if (distance_it > costmap_robot_->getCostmap()->getResolution()) + { + prev_carrot_pose_it = it; + break; + } + } + + robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution() + ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose) + : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D()); + + robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose); + + // teb_local_planner::PoseSE2 start_pose(front); + // teb_local_planner::PoseSE2 goal_pose(back); + // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec()); + const double dir_path = 0.0; + if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) + x_direction = dir_path > 0 ? FORWARD : BACKWARD; + } + catch (std::exception &e) + { + robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what()); + return false; + } + } + + x_direction_ = x_direction; + y_direction_ = y_direction; + theta_direction_ = theta_direction; + return true; +} + +mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( + const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) +{ + mkt_msgs::Trajectory2D result; + result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; + if (!traj_) + return result; + if (compute_plan_.poses.size() < 2) + { + robot::log_warning("[%s:%d]\n Local compute plan is available", __FILE__, __LINE__); + return result; + } + + robot::Time current_time = robot::Time::now(); + double dt = (current_time - last_actuator_update_).toSec(); + last_actuator_update_ = current_time; + + robot_nav_2d_msgs::Twist2D drive_cmd; + double sign_x = sign(x_direction_); + + robot_nav_2d_msgs::Twist2D twist; + traj_->startNewIteration(velocity); + while (robot::ok() && traj_->hasMoreTwists()) + { + twist = traj_->nextTwist(); + } + double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; + drive_cmd.x = std::min(sqrt(twist.x * twist.x), fabs(v_max)); + robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; + 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); + result.velocity = drive_cmd; + prevous_drive_cmd_ = drive_cmd; + return result; + } + + double lookahead_dist = getLookAheadDistance(velocity); + auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); + + bool allow_rotate = false; + nh_priv_.param("allow_rotate", allow_rotate, false); + + const double distance_allow_rotate = min_journey_squared_; + const double path_distance_to_rotate = hypot(pose.pose.x - transformed_plan.poses.back().pose.x, pose.pose.y - transformed_plan.poses.back().pose.y); + const double journey_plan = transformed_plan.poses.empty() ? distance_allow_rotate : journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1); + + allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; + + double angle_to_heading; + if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x)) + { + if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_)) + { + if (!stopWithAccLimits(pose, velocity, drive_cmd)) + return result; + } + else + { + rotateToHeading(angle_to_heading, velocity, drive_cmd); + } + } + else + { + transformed_plan = this->generateHermiteTrajectory(transformed_plan.poses.back()); + lookahead_dist = getLookAheadDistance(velocity); + carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); + this->computePurePursuit( + traj_, + carrot_pose, + velocity, + min_approach_linear_velocity_, + journey_plan, sign_x, + min_lookahead_dist_, + max_lookahead_dist_, + lookahead_dist, + lookahead_time_, + dt, drive_cmd); + + // const double vel_x_reduce = std::min(fabs(v_max), 0.3); + // const double scale = fabs(velocity.x) * lookahead_time_ * 0.9; + // const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale; + // double d_reduce = std::clamp(journey_plan, min_S, max_S); + // double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0); + // double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce))); + // double v_min = + // journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_; + // v_min *= sign_x; + + // double effective_journey = getEffectiveDistance(carrot_pose, journey_plan); + // double decel_factor = computeDecelerationFactor(effective_journey, d_reduce); + // double vel_reduce = sign_x > 0 + // ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min) + // : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min); + // drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce; + + if (this->nav_stop_) + { + if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_)) + { + if (!stopWithAccLimits(pose, velocity, drive_cmd)) + return result; + } + else + drive_cmd = {}; + result.velocity = drive_cmd; + return result; + } + + // 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) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x); + // drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_); + } + + result.poses.clear(); + result.poses.reserve(transformed_plan.poses.size()); + for (const auto &pose_stamped : transformed_plan.poses) + { + result.poses.push_back(pose_stamped.pose); + } + result.velocity = drive_cmd; + prevous_drive_cmd_ = drive_cmd; + return result; +} + +// void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( +// const score_algorithm::TrajectoryGenerator::Ptr &traj, +// const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, +// const robot_nav_2d_msgs::Twist2D &velocity, +// const double &min_approach_linear_velocity, +// const double &journey_plan, +// const double &sign_x, +// const double &lookahead_dist_min, +// const double &lookahead_dist_max, +// const double &lookahead_dist, +// const double &lookahead_time, +// const double &dt, +// robot_nav_2d_msgs::Twist2D &drive_cmd) +// { + // if (!traj) + // return; + // const double velocity_max = sign_x > 0 ? traj->getTwistLinear(true).x : traj->getTwistLinear(false).x; + // const double vel_x_reduce = std::min(fabs(velocity_max), 0.3); + // double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; + // carrot_dist2 = std::max(carrot_dist2, 0.05); + // double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + + // if (max_lateral_accel_ > 1e-6) + // { + // const double curvature_abs = std::max(fabs(curvature), 1e-6); + // const double v_lateral_limit = sqrt(max_lateral_accel_ / curvature_abs); + // drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit); + // } + + + // double post_cost = 0.0; + // double distance_error = 0.0; + // robot_nav_2d_msgs::Twist2D twist = traj->getTwist(); + // this->applyConstraints(distance_error, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x); + + + // double d_reduce = std::clamp(journey_plan, lookahead_dist_min, lookahead_dist_max); + + // double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0); + // double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce))); + // double v_min = + // journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity) * cosine_factor_begin_reduce + min_approach_linear_velocity; + // v_min *= sign_x; + + // double v_theta = drive_cmd.x * curvature; + // double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); + + // carrot_dist2 *= 0.6; + // curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; + // v_theta = drive_cmd.x * curvature; + + // double limit_acc_theta = fabs(v_theta) > 0.15 ? 1.0 : 1.8; + // double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt; + // double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt; + // drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth); +// } + +double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajectory( + const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Path2D &trajectory, + double v_target, + const double &sign_x) +{ + if (trajectory.poses.size() < 3) + return v_target; + + // Use speed-scaled preview distance to evaluate curvature + double preview_dist = std::clamp(std::fabs(velocity.x) * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_); + + double traveled = 0.0; + double max_kappa = 0.0; + for (size_t i = 1; i + 1 < trajectory.poses.size(); ++i) + { + const auto &p0 = trajectory.poses[i - 1].pose; + const auto &p1 = trajectory.poses[i].pose; + const auto &p2 = trajectory.poses[i + 1].pose; + + const double a = std::hypot(p1.x - p0.x, p1.y - p0.y); + const double b = std::hypot(p2.x - p1.x, p2.y - p1.y); + const double c = std::hypot(p2.x - p0.x, p2.y - p0.y); + traveled += a; + + if (a > 1e-6 && b > 1e-6 && c > 1e-6) + { + const double cross = (p1.x - p0.x) * (p2.y - p0.y) - (p1.y - p0.y) * (p2.x - p0.x); + const double area2 = std::fabs(cross); // 2 * area + const double kappa = 2.0 * area2 / (a * b * c); + max_kappa = std::max(max_kappa, std::fabs(kappa)); + } + + 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) + { + const double v_curve = std::sqrt(max_lateral_accel_ / max_kappa); + v_limit = std::min(v_limit, v_curve); + } + + if (decel_lim_x_ > 1e-6) + { + const double remaining = journey(trajectory.poses, 0, trajectory.poses.size() - 1); + const double v_stop = std::sqrt(2.0 * decel_lim_x_ * std::max(0.0, remaining)); + v_limit = std::min(v_limit, v_stop); + } + + return std::copysign(v_limit, sign_x); +} + +void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( + const score_algorithm::TrajectoryGenerator::Ptr &traj, + const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, + const robot_nav_2d_msgs::Twist2D &velocity, + const double &min_approach_linear_velocity, + const double &journey_plan, + const double &sign_x, + const double &lookahead_dist_min, + const double &lookahead_dist_max, + const double &lookahead_dist, + const double &lookahead_time, + const double &dt, + robot_nav_2d_msgs::Twist2D &drive_cmd) +{ + if (!traj_) + return; + + // 1) Curvature from pure pursuit + const double L2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; + if (L2 < 1e-6) + return; + const double kappa = 2.0 * carrot_pose.pose.y / L2; + + // 2) Base speed from trajectory limits (forward/backward) + double v_max = sign_x > 0 ? traj->getTwistLinear(true).x : traj->getTwistLinear(false).x; + + // 3) Adjust speed using Hermite trajectory curvature + remaining distance + double v_target = adjustSpeedWithHermiteTrajectory(velocity, transform_plan_, v_max, sign_x); + + robot::log_info("v_max: %f, v_target: %f", v_max , v_target); + + // 4) Maintain minimum approach speed + if (std::fabs(v_target) < min_approach_linear_velocity) + v_target = std::copysign(min_approach_linear_velocity, sign_x); + + // 5) Angular speed from curvature + double w_target = v_target * kappa; + w_target = std::clamp(w_target, -max_vel_theta_, max_vel_theta_); + + // 6) Apply acceleration limits (linear + angular) + const double dv = std::clamp(v_target - velocity.x, -acc_lim_x_ * dt, acc_lim_x_ * dt); + const double dw = std::clamp(w_target - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt); + + drive_cmd.x = velocity.x + dv; + drive_cmd.theta = velocity.theta + dw; +} + +double mkt_algorithm::diff::PredictiveTrajectory::estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan) { + if (plan.poses.size() < 2) return 0.0; + const auto& p1 = plan.poses[plan.poses.size() - 2]; + const auto& p2 = plan.poses[plan.poses.size() - 1]; + return std::atan2(p2.pose.y - p1.pose.y, p2.pose.x - p1.pose.x); +} + +void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( + const double &dist_error, const double &lookahead_dist, + const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity, + const double &pose_cost, double &linear_vel, const double &sign_x) +{ + double curvature_vel = linear_vel; + double cost_vel = linear_vel; + double approach_vel = linear_vel; + + + if (use_regulated_linear_velocity_scaling_) + { + const double &min_rad = regulated_linear_scaling_min_radius_; + const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad; + if (radius < min_rad) + { + curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = curvature_vel; + this->moveWithAccLimits(velocity, cmd, result); + curvature_vel = result.x; + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + } + } + + if (use_cost_regulated_linear_velocity_scaling_ && + pose_cost != static_cast(robot_costmap_2d::NO_INFORMATION) && + pose_cost != static_cast(robot_costmap_2d::FREE_SPACE)) + { + const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * + std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) + + inscribed_radius; + + if (min_distance_to_obstacle < cost_scaling_dist_) + { + cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; + } + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = cost_vel; + this->moveWithAccLimits(velocity, cmd, result); + cost_vel = result.x; + linear_vel = std::min(cost_vel, curvature_vel); + } + // ss << linear_vel << " "; + // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + // if the actual lookahead distance is shorter than requested, that means we're at the + // end of the path. We'll scale linear velocity by error to slow to a smooth stop. + // This expression is eq. to + // (1) holding time to goal, t, constant using the theoretical + // lookahead distance and proposed velocity and + // (2) using t with the actual lookahead + // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). + + double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr + ? 2.0 * costmap_robot_->getCostmap()->getResolution() + : 0.1; + if (dist_error > dist_error_limit) + { + double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0; + double unbounded_vel = approach_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) + { + approach_vel = min_approach_linear_velocity_; + } + else + { + approach_vel *= velocity_scaling; + } + + // Use the lowest velocity between approach and other constraints, if all overlapping + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = approach_vel; + this->moveWithAccLimits(velocity, cmd, result); + approach_vel = result.x; + linear_vel = std::min(linear_vel, approach_vel); + } + + // Limit linear velocities to be valid + 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); + double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); + double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); + double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y); + double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y); + double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel); + linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel); + linear_vel = sign_x * linear_vel; +} + + +bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( + const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x) +{ + bool curvature = false; + double path_angle = 0; + if (!global_plan.poses.empty()) + { + if ((unsigned)global_plan.poses.size() > 2) + { + const double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; + const double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; + if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) + { + double start_angle = atan2(start_direction_y, start_direction_x); + for (unsigned int i = 1; i < (unsigned)global_plan.poses.size(); i++) + { + 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); + if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) > 1e-9) + { + curvature = true; + break; + } + path_angle = current_angle; + } + } + } + } + } + // Whether we should rotate robot to rough path heading + angle_to_path = curvature ? path_angle : std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); + angle_to_path = sign_x < 0 + ? angles::normalize_angle(M_PI + angle_to_path) + : angles::normalize_angle(angle_to_path); + + 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_; + bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_); + heading_rotate = is_stopped + ? rotate_to_heading_min_angle_ + heading_linear + : std::clamp(heading_rotate + heading_linear, 0.3, M_PI_2); + + bool result = use_rotate_to_heading_ && (is_stopped || sign(angle_to_path) * sign_x < 0) && fabs(angle_to_path) > heading_rotate; + 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) +{ + 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 angular_decel_zone = 0.3; // rad - khoảng cách bắt đầu giảm tốc + nh_priv_.param("angular_decel_zone", angular_decel_zone, 0.5); + angular_decel_zone += std::clamp( + fabs(max_vel_theta) + (M_PI_2 - fabs(atan2(min_path_distance_, max_path_distance_))), 0.4, M_PI_2); + double zone_begin = std::max(angular_decel_zone * 0.2, 0.2); + double cosine_begin_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / zone_begin))); + + double reduce_vel_theta = std::min(max_vel_theta, 0.25 + min_vel_theta_); + reduce_vel_theta = fabs(ang_diff) > zone_begin ? reduce_vel_theta : (reduce_vel_theta - min_vel_theta_) * cosine_begin_factor + min_vel_theta_; + + // === ANGULAR DECELERATION ZONE === + double target_angular_speed = max_vel_theta; + if (fabs(ang_diff) < angular_decel_zone) + { + double cosine_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / angular_decel_zone))); + double cosine_speed = max_vel_theta * cosine_factor; + + // Choose deceleration profile + target_angular_speed = cosine_speed; // Smoothest option + + // Ensure minimum speed to avoid stalling + target_angular_speed = std::max(target_angular_speed, reduce_vel_theta); + } + // 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); + + // === 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; +} + +double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity) +{ + // If using velocity-scaled look ahead distances, find and clamp the dist + // Else, use the static look ahead distance + double lookahead_dist = lookahead_dist_; + if (use_velocity_scaled_lookahead_dist_) + { + lookahead_dist = fabs(velocity.x) * lookahead_time_; + lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + } + + return lookahead_dist; +} + +std::vector::iterator +mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan) +{ + if (global_plan.poses.empty()) + { + throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty."); + } + + if ((unsigned)global_plan.poses.size() < 2) + { + 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; + + // 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; + + 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; + } + } + } + } + + // 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; + }); + + // If the number of poses is not far enough, take the last pose + if (goal_pose_it == global_plan.poses.end()) + { + goal_pose_it = std::prev(global_plan.poses.end()); + } + return goal_pose_it; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot) +{ + if (global_plan.poses.empty()) + return false; + if (tf == nullptr) + return false; + try + { + // let's get the pose of the robot in the frame of the plan + robot_nav_2d_msgs::Pose2DStamped robot; + if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot)) + { + throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); + } + + double dist_thresh_sq = dist_behind_robot * dist_behind_robot; + + // iterate plan until a pose close the robot is found + std::vector::iterator it = global_plan.poses.begin(); + std::vector::iterator erase_end = it; + while (it != global_plan.poses.end()) + { + double dx = robot.pose.x - it->pose.x; + double dy = robot.pose.y - it->pose.y; + double dist_sq = dx * dx + dy * dy; + if (dist_sq < dist_thresh_sq) + { + erase_end = it; + break; + } + ++it; + } + if (erase_end == global_plan.poses.end()) + return false; + + if (erase_end != global_plan.poses.begin()) + global_plan.poses.erase(global_plan.poses.begin(), erase_end); + } + catch (const tf3::TransformException &ex) + { + robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s\n", __FILE__, __LINE__, ex.what()); + return false; + } + return true; +} + +robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose) +{ + robot_nav_2d_msgs::Path2D hermite_trajectory; + hermite_trajectory.poses.clear(); + hermite_trajectory.header.stamp = pose.header.stamp; + hermite_trajectory.header.frame_id = pose.header.frame_id; + // Characteristic length (can be tuned) + const double x = pose.pose.x; + const double y = pose.pose.y; + const double theta = pose.pose.theta; + double L = std::sqrt(x * x + y * y); + if (L < 1e-6) { + // Degenerate: return single pose + robot_nav_2d_msgs::Pose2DStamped pose_stamped; + pose_stamped.pose.x = 0.0; + pose_stamped.pose.y = 0.0; + pose_stamped.pose.theta = 0.0; + pose_stamped.header.stamp = pose.header.stamp; + pose_stamped.header.frame_id = pose.header.frame_id; + hermite_trajectory.poses.push_back(pose_stamped); + return hermite_trajectory; + } + int samples = L/costmap_robot_->getCostmap()->getResolution(); + if (samples < 1) { + samples = 1; + } + hermite_trajectory.poses.reserve(samples + 1); + + for (int i = 0; i <= samples; ++i) { + double t = static_cast(i) / samples; + double t2 = t * t; + double t3 = t2 * t; + + double h00 = 2 * t3 - 3 * t2 + 1; + double h10 = t3 - 2 * t2 + t; + double h01 = -2 * t3 + 3 * t2; + double h11 = t3 - t2; + + // Hermite interpolation + double px = h10 * L + h01 * x + h11 * (L * std::cos(theta)); + double py = h01 * y + h11 * (L * std::sin(theta)); + + // First derivatives for heading + double dh00 = 6 * t2 - 6 * t; + double dh10 = 3 * t2 - 4 * t + 1; + double dh01 = -6 * t2 + 6 * t; + double dh11 = 3 * t2 - 2 * t; + + double dx = dh10 * L + dh01 * x + dh11 * (L * std::cos(theta)); + double dy = dh01 * y + dh11 * (L * std::sin(theta)); + + double heading = std::atan2(dy, dx); + + robot_nav_2d_msgs::Pose2DStamped pose; + pose.pose.x = px; + pose.pose.y = py; + pose.pose.theta = heading; + pose.header.stamp = hermite_trajectory.header.stamp; + pose.header.frame_id = hermite_trajectory.header.frame_id; + hermite_trajectory.poses.push_back(pose); + } + return hermite_trajectory; +} + +bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( + TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose, + const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length, + robot_nav_2d_msgs::Path2D &transformed_plan) +{ + // this method is a slightly modified version of base_local_planner/goal_functions.h + transformed_plan.poses.clear(); + transformed_plan.header.stamp = pose.header.stamp; + transformed_plan.header.frame_id = robot_base_frame; + + if (global_plan.poses.empty()) + return false; + + if (tf == nullptr) + return false; + + if (costmap == nullptr || costmap->getCostmap() == nullptr) + return false; + + try + { + if (global_plan.poses.empty()) + { + robot::log_error("[%s:%d]\n Received plan with zero length", __FILE__, __LINE__); + return false; + } + + // let's get the pose of the robot in the frame of the plan + robot_nav_2d_msgs::Pose2DStamped robot_pose; + if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) + { + throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); + } + + // we'll discard points on the plan that are outside the local costmap + double dist_threshold = std::max(costmap->getCostmap()->getSizeInCellsX() * costmap->getCostmap()->getResolution() / 2.0, + costmap->getCostmap()->getSizeInCellsY() * costmap->getCostmap()->getResolution() / 2.0); + + dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are + double sq_dist_threshold = dist_threshold * dist_threshold; + // located on the border of the local costmap + + int i = 0; + double sq_dist = std::numeric_limits::infinity(); + + // we need to loop to a point on the plan that is within a certain distance of the robot + bool robot_reached = false; + for (int j = 0; j < (int)global_plan.poses.size(); ++j) + { + double x_diff = robot_pose.pose.x - global_plan.poses[j].pose.x; + double y_diff = robot_pose.pose.y - global_plan.poses[j].pose.y; + double new_sq_dist = x_diff * x_diff + y_diff * y_diff; + if (robot_reached && new_sq_dist > sq_dist_threshold) + { + break; + } // force stop if we have reached the costmap border + + if (new_sq_dist < sq_dist) // find closest distance + { + sq_dist = new_sq_dist; + i = j; + if (sq_dist < costmap->getCostmap()->getResolution()) // 5 cm to the robot; take the immediate local minima; if it's not the global + robot_reached = true; // minima, probably means that there's a loop in the path, and so we prefer this + } + } + + double plan_length = 0; // check cumulative Euclidean distance along the plan + double x_direction_saved = 0.0; + // now we'll transform until points are outside of our distance threshold + while (i < (int)global_plan.poses.size() && sq_dist <= sq_dist_threshold && + (max_plan_length <= 0 || plan_length <= max_plan_length)) + { + robot_nav_2d_msgs::Pose2DStamped stamped_pose; + stamped_pose.pose = global_plan.poses[i].pose; + stamped_pose.header.frame_id = global_plan.header.frame_id; + + robot_nav_2d_msgs::Pose2DStamped newer_pose; + if (!robot_nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, newer_pose)) + { + ++i; + continue; + } + transformed_plan.poses.push_back(newer_pose); + + double x_diff = robot_pose.pose.x - global_plan.poses[i].pose.x; + double y_diff = robot_pose.pose.y - global_plan.poses[i].pose.y; + sq_dist = x_diff * x_diff + y_diff * y_diff; + + // Calculate distance to previous pose + if (i > 0 && max_plan_length > 0) + { + robot_geometry_msgs::Pose2D p1 = global_plan.poses[i].pose; + robot_geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].pose; + plan_length += std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2)); + if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) + { + const double egde_angle = atan2(p1.y - p2.y, p1.x - p2.x); + const double target_direction = cos(fabs(egde_angle - p1.theta)); + if (target_direction * x_direction_saved < 0.0) + { + plan_length = 0; + } + x_direction_saved = target_direction; + } + } + ++i; + } + } + catch (tf3::LookupException &ex) + { + robot::log_error("[%s:%d]\n No Transform available Error: %s\n", __FILE__, __LINE__, ex.what()); + return false; + } + catch (tf3::ConnectivityException &ex) + { + robot::log_error("[%s:%d]\n Connectivity Error: %s\n", __FILE__, __LINE__, ex.what()); + return false; + } + catch (tf3::ExtrapolationException &ex) + { + robot::log_error("[%s:%d]\n Extrapolation Error: %s\n", __FILE__, __LINE__, ex.what()); + if (global_plan.poses.size() > 0) + robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s\n", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str()); + + return false; + } + return true; +} + +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 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); + + double vx = cmd_vel.x > 0.0 ? std::min(max_vel_x, std::max(min_vel_x, cmd_vel.x)) : std::max(-max_vel_x, std::min(-min_vel_x, cmd_vel.x)); + + double max_acc_vx = velocity.x + fabs(acc_lim_x_) * dt; + double min_acc_vx = velocity.x - fabs(decel_lim_x_) * dt; + vx = std::clamp(vx, min_acc_vx, max_acc_vx); + + // --- Y axis --- + double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); + double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); + double vy = cmd_vel.y > 0.0 ? std::min(max_vel_y, std::max(min_vel_y, cmd_vel.y)) : std::max(-max_vel_y, std::min(-min_vel_y, cmd_vel.y)); + + double max_acc_vy = velocity.y + fabs(acc_lim_y_) * dt; + double min_acc_vy = velocity.y - fabs(decel_lim_y_) * dt; + vy = std::clamp(vy, min_acc_vy, max_acc_vy); + // --- Assign result --- + cmd_vel_result.x = vx; + cmd_vel_result.y = vy; + // 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) +{ + // 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)); + + double vel_yaw = velocity.theta; + double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - fabs(decel_lim_theta_) * dt)); + + cmd_vel.x = vx; + cmd_vel.y = vy; + cmd_vel.theta = vth; + return true; +} + +std::vector +mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector footprint, double resolution) +{ + + // Dịch sang tọa độ tuyệt đối + std::vector abs_footprint; + double cos_th = std::cos(pose.theta); + double sin_th = std::sin(pose.theta); + for (auto pt : footprint) + { + pt.x *= 1.2; + pt.y *= 1.2; + robot_geometry_msgs::Point abs_pt; + abs_pt.x = pose.x + pt.x * cos_th - pt.y * sin_th; + abs_pt.y = pose.y + pt.x * sin_th + pt.y * cos_th; + abs_footprint.push_back(abs_pt); + } + + std::vector points; + for (size_t i = 0; i < abs_footprint.size(); ++i) + { + const robot_geometry_msgs::Point &start = abs_footprint[i]; + const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()]; + + double dx = end.x - start.x; + double dy = end.y - start.y; + double distance = std::hypot(dx, dy); + int steps = std::max(1, static_cast(std::floor(distance / resolution))); + + for (int j = 0; j <= steps; ++j) + { + if (j == steps && i != abs_footprint.size() - 1) + continue; // tránh lặp + double t = static_cast(j) / steps; + robot_geometry_msgs::Point pt; + pt.x = start.x + t * dx; + pt.y = start.y + t * dy; + points.push_back(pt); + } + } + return points; +} + +double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, const double &y) +{ + unsigned int mx, my; + unsigned char cost; + if (costmap_robot_->getCostmap()->worldToMap(x, y, mx, my)) + { + cost = costmap_robot_->getCostmap()->getCost(mx, my); + } + return static_cast(cost); +} + + +double mkt_algorithm::diff::PredictiveTrajectory::computeDecelerationFactor(double remaining_distance, double decel_distance) +{ + if (remaining_distance >= decel_distance) + { + return 1.0; // Full speed + } + + // Smooth transition using cosine function + double ratio = fabs(remaining_distance / decel_distance); + return 0.5 * (1.0 + cos(M_PI * (1.0 - ratio))); // Smooth from 1 to 0 +} + +double mkt_algorithm::diff::PredictiveTrajectory::getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, + double journey_plan) +{ + double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x + + carrot_pose.pose.y * carrot_pose.pose.y); + + // Avoid division by zero and handle backward motion + if (carrot_distance < 1e-3) + return journey_plan; + + // Project remaining path onto carrot direction + double alignment = fabs(carrot_pose.pose.x / carrot_distance); // cos(angle) + return journey_plan * std::max(0.0, alignment); // Only positive projection +} + +score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create() +{ + return std::make_shared(); +} + +BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory) \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/hermite_trajectory.svg b/src/Algorithms/Libraries/mkt_algorithm/src/diff/hermite_trajectory.svg new file mode 100644 index 0000000..fe475ae --- /dev/null +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/hermite_trajectory.svg @@ -0,0 +1,40 @@ + + + + + + + + + + + x + y + O(0,0) + + + + + + + + + + + + + + + + + + + + + + + heading 0 + goal (x,y,theta) + Hermite trajectory + + diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp index 993879b..00f48ac 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp @@ -1,6 +1,4 @@ #include -#define LIMIT_VEL_THETA 0.33 - void mkt_algorithm::diff::PurePursuit::computePurePursuit( const score_algorithm::TrajectoryGenerator::Ptr &traj, @@ -13,6 +11,7 @@ void mkt_algorithm::diff::PurePursuit::computePurePursuit( const double &lookahead_dist_max, const double &lookahead_dist, const double &lookahead_time, + const double &dt, robot_nav_2d_msgs::Twist2D &drive_cmd) { if (!traj) @@ -59,40 +58,126 @@ void mkt_algorithm::diff::PurePursuit::computePurePursuit( curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; v_theta = drive_cmd.x * curvature; - if (fabs(v_theta) > LIMIT_VEL_THETA) - { - robot_nav_2d_msgs::Twist2D cmd_vel, cmd_result; - cmd_vel.x = sign_x > 0 - ? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1)) - : std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1)); - cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5); - // this->moveWithAccLimits(velocity, cmd_vel, cmd_result); - drive_cmd.x = std::copysign(cmd_result.x, sign_x); - v_theta = drive_cmd.x * curvature; - } - - if (journey_plan < min_journey_squared_) - { - if (transform_plan_.poses.size() > 2) - { - robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back(); - robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2]; - double dx = end.pose.x - start.pose.x; - double dy = end.pose.y - start.pose.y; - v_theta = atan2(dy, dx); - if (v_theta > M_PI_2) - v_theta -= M_PI; - else if (v_theta < -M_PI_2) - v_theta += M_PI; - // v_theta *= 0.5; - v_theta = std::clamp(v_theta, -0.02, 0.02); - } - else - v_theta = 0.0; - } - double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8; + double limit_acc_theta = fabs(v_theta) > 0.15 ? 1.0 : 1.8; double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt; double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt; drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth); - } + +void mkt_algorithm::diff::PurePursuit::applyConstraints( + const double &dist_error, const double &lookahead_dist, + const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity, + const double &pose_cost, double &linear_vel, const double &sign_x) +{ + double curvature_vel = linear_vel; + double cost_vel = linear_vel; + double approach_vel = linear_vel; + + + if (use_regulated_linear_velocity_scaling_) + { + const double &min_rad = regulated_linear_scaling_min_radius_; + const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad; + if (radius < min_rad) + { + curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = curvature_vel; + this->moveWithAccLimits(velocity, cmd, result); + curvature_vel = result.x; + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + } + } + + if (use_cost_regulated_linear_velocity_scaling_ && + pose_cost != static_cast(robot_costmap_2d::NO_INFORMATION) && + pose_cost != static_cast(robot_costmap_2d::FREE_SPACE)) + { + const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * + std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) + + inscribed_radius; + + if (min_distance_to_obstacle < cost_scaling_dist_) + { + cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; + } + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = cost_vel; + this->moveWithAccLimits(velocity, cmd, result); + cost_vel = result.x; + linear_vel = std::min(cost_vel, curvature_vel); + } + // ss << linear_vel << " "; + // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + // if the actual lookahead distance is shorter than requested, that means we're at the + // end of the path. We'll scale linear velocity by error to slow to a smooth stop. + // This expression is eq. to + // (1) holding time to goal, t, constant using the theoretical + // lookahead distance and proposed velocity and + // (2) using t with the actual lookahead + // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). + + double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr + ? 2.0 * costmap_robot_->getCostmap()->getResolution() + : 0.1; + if (dist_error > dist_error_limit) + { + double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0; + double unbounded_vel = approach_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) + { + approach_vel = min_approach_linear_velocity_; + } + else + { + approach_vel *= velocity_scaling; + } + + // Use the lowest velocity between approach and other constraints, if all overlapping + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = approach_vel; + this->moveWithAccLimits(velocity, cmd, result); + approach_vel = result.x; + linear_vel = std::min(linear_vel, approach_vel); + } + + // Limit linear velocities to be valid + 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); + double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); + double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); + double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y); + double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y); + double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel); + linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel); + linear_vel = sign_x * linear_vel; +} + +double mkt_algorithm::diff::PurePursuit::getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, + double journey_plan) +{ +double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x + +carrot_pose.pose.y * carrot_pose.pose.y); + +// Avoid division by zero and handle backward motion +if (carrot_distance < 1e-3) +return journey_plan; + +// Project remaining path onto carrot direction +double alignment = fabs(carrot_pose.pose.x / carrot_distance); // cos(angle) +return journey_plan * std::max(0.0, alignment); // Only positive projection +} + + +double mkt_algorithm::diff::PurePursuit::computeDecelerationFactor(double remaining_distance, double decel_distance) +{ + if (remaining_distance >= decel_distance) + { + return 1.0; // Full speed + } + + // Smooth transition using cosine function + double ratio = fabs(remaining_distance / decel_distance); + return 0.5 * (1.0 + cos(M_PI * (1.0 - ratio))); // Smooth from 1 to 0 +} \ No newline at end of file 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 23940e0..b23b0b5 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 @@ -105,7 +105,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, catch (const std::exception &ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s algorithm , are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_nav_name.c_str(), ex.what()); - exit(1); + // exit(1); } std::string algorithm_rotate_name; @@ -120,7 +120,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, if (!rotate_algorithm_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str()); - exit(1); + // exit(1); } rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_); robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());