From 15f842d7030f35fe7f2366ef1f3ac3de45155cd2 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Mon, 2 Feb 2026 14:02:25 +0700 Subject: [PATCH] update 2/2 --- config/move_base_common_params.yaml | 6 +- config/pnkx_local_planner_params.yaml | 38 +- .../Cores/score_algorithm/README.md | 44 + .../include/score_algorithm/score_algorithm.h | 10 + .../score_algorithm/src/score_algorithm.cpp | 68 +- .../diff/diff_predictive_trajectory.h | 147 ++- .../mkt_algorithm/src/diff/Hermite.md | 19 +- .../mkt_algorithm/src/diff/Hermite.pdf | Bin 0 -> 35440 bytes .../src/diff/diff_go_straight.cpp | 101 -- .../src/diff/diff_predictive_trajectory.cpp | 868 +++++++++++------- .../mkt_plugins/src/kinematic_parameters.cpp | 1 - .../Packages/global_planners/custom_planner | 2 +- .../Packages/move_base/src/move_base.cpp | 4 +- 13 files changed, 788 insertions(+), 520 deletions(-) create mode 100644 src/Algorithms/Cores/score_algorithm/README.md create mode 100644 src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.pdf diff --git a/config/move_base_common_params.yaml b/config/move_base_common_params.yaml index b9a0b0a..02f44d8 100755 --- a/config/move_base_common_params.yaml +++ b/config/move_base_common_params.yaml @@ -1,12 +1,12 @@ ### replanning -controller_frequency: 20.0 # run controller at 15.0 Hz +controller_frequency: 30.0 # run controller at 15.0 Hz controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan planner_frequency: 0.0 # don't continually replan (only when controller failed) -planner_patience: 5.0 # if the first planning attempt failed, abort planning retries after 5.0 s... +planner_patience: 2.0 # if the first planning attempt failed, abort planning retries after 5.0 s... max_planning_retries: 5 # ... or after 10 attempts (whichever happens first) oscillation_timeout: -1 # abort controller and trigger recovery behaviors after 30.0 s -oscillation_distance: 0.4 +oscillation_distance: 0.5 ### recovery behaviors recovery_behavior_enabled: true recovery_behaviors: [ diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 3a861f4..dbc8ca5 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -1,6 +1,6 @@ yaw_goal_tolerance: 0.017 -xy_goal_tolerance: 0.03 -min_approach_linear_velocity: 0.06 +xy_goal_tolerance: 0.02 +min_approach_linear_velocity: 0.05 LocalPlannerAdapter: library_path: liblocal_planner_adapter @@ -48,14 +48,14 @@ LimitedAccelGenerator: min_vel_y: 0.0 # diff drive robot max_speed_xy: 2.0 # max_trans_vel: 0.8 # choose slightly less than the base's capability - min_speed_xy: 0.1 # min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity + 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.9 # max_rot_vel: 1.0 # choose slightly less than the base's capability - min_vel_theta: 0.04 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational 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 acc_lim_x: 1.0 acc_lim_y: 0.0 # diff drive robot - acc_lim_theta: 0.9 + acc_lim_theta: 1.5 decel_lim_x: -1.0 decel_lim_y: -0.0 decel_lim_theta: -1.5 @@ -75,8 +75,8 @@ LimitedAccelGenerator: MKTAlgorithmDiffPredictiveTrajectory: library_path: libmkt_algorithm_diff avoid_obstacles: false - xy_local_goal_tolerance: 0.01 - angle_threshold: 0.6 + xy_local_goal_tolerance: 0.02 + angle_threshold: 0.47 index_samples: 60 follow_step_path: true @@ -85,12 +85,12 @@ MKTAlgorithmDiffPredictiveTrajectory: # only when false: lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) # only when true: - min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3) + min_lookahead_dist: 0.6 # The minimum lookahead distance (m) threshold. (default: 0.3) max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9) lookahead_time: 1.9 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) - min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) + min_journey_squared: 0.2 # 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) + max_lateral_accel: 0.9 # 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) @@ -100,7 +100,7 @@ MKTAlgorithmDiffPredictiveTrajectory: # stoped rot_stopped_velocity: 0.05 - trans_stopped_velocity: 0.03 + 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) @@ -114,20 +114,6 @@ MKTAlgorithmDiffPredictiveTrajectory: cost_scaling_dist: 0.2 # (default: 0.6) cost_scaling_gain: 2.0 # (default: 1.0) - - use_mpc: true - mpc_horizon: 10 - mpc_dt: 0.1 - mpc_w_pos: 6.0 - mpc_w_theta: 2.0 - mpc_w_v: 0.2 - mpc_w_w: 0.2 - mpc_w_dv: 0.4 - mpc_w_dw: 0.4 - mpc_iterations: 3 - mpc_step: 0.15 - mpc_eps: 0.001 - MKTAlgorithmDiffGoStraight: library_path: libmkt_algorithm_diff avoid_obstacles: false diff --git a/src/Algorithms/Cores/score_algorithm/README.md b/src/Algorithms/Cores/score_algorithm/README.md new file mode 100644 index 0000000..b86fc66 --- /dev/null +++ b/src/Algorithms/Cores/score_algorithm/README.md @@ -0,0 +1,44 @@ +# score_algorithm + +## Giới thiệu +`score_algorithm` là lớp nền (base class) cho các thuật toán đánh giá và sinh lệnh điều khiển (local planning) trong `pnkx_nav_core`. Module này không tự sinh quỹ đạo cụ thể, mà cung cấp bộ khung và các hàm dùng chung để các thuật toán con triển khai. + +## Vai trò chính +- Chuẩn hóa giao diện (interface) cho các thuật toán tính toán quỹ đạo/lệnh. +- Cắt đoạn `global_plan` thành đoạn con phù hợp với `local costmap`. +- Chọn `sub-goal` và `start` để theo đuổi theo từng bước đường đi. +- Cung cấp các hàm tiện ích: kiểm tra dừng/đi, tính quãng đường, tìm mục tiêu theo góc và khoảng cách. + +## API chính +Được khai báo trong `include/score_algorithm/score_algorithm.h`: +- `initialize(...)`: khởi tạo tham số, TF, costmap, bộ sinh quỹ đạo. +- `prepare(...)`: chuẩn bị dữ liệu dùng chung trước khi đánh giá. +- `calculator(...)`: tính toán ra `mkt_msgs::Trajectory2D`. +- `computePlanCommand(...)`: cắt `global_plan` thành đoạn kết quả và chọn `start/goal`. +- `findSubGoalIndex(...)`, `getGoalIndex(...)`, `journey(...)`, `stopped(...)`: hàm phụ trợ. + +## Nguyên lý `computePlanCommand(...)` +Hàm này nhận `robot_pose`, `velocity`, độ dài xem trước `S` và `global_plan`, sau đó: +1) Tìm các mốc `sub-goal` (đổi hướng / điểm gãy) bằng `findSubGoalIndex`. +2) Khôi phục `sub_goal_index` và `sub_start_index` đã lưu (nếu có), cập nhật nếu robot đang dừng và đã vượt mốc. +3) Tìm `start_index` gần nhất với robot trong đoạn con hợp lệ. +4) Tìm `goal_index` dựa trên quy tắc "đi đủ xa `S` hoặc đổi hướng vượt ngưỡng". +5) Xuất `result` là đoạn từ `closet_index` đến `goal_index`. + +Kết quả được sử dụng bởi các local planner để sinh lệnh điều khiển tiếp theo. + +## Sử dụng trong hệ thống +`score_algorithm` được nạp qua pluginlib trong các local planner, ví dụ: +- `pnkx_local_planner` +- `pnkx_go_straight_local_planner` +- `pnkx_rotate_local_planner` +- `pnkx_docking_local_planner` + +Mỗi thuật toán cụ thể sẽ kế thừa `ScoreAlgorithm` và triển khai `initialize`, `prepare`, `calculator`. + +## Thư mục liên quan +- `include/score_algorithm/score_algorithm.h`: giao diện chính. +- `src/score_algorithm.cpp`: các hàm dùng chung (getGoalIndex, computePlanCommand, ...). + +## Ghi chú +Module này phụ thuộc vào `robot_nav_2d_msgs`, `robot_costmap_2d`, `mkt_msgs` và hệ thống TF/ROS. 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 e70c7ad..1e6ebd6 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,6 +1,8 @@ #ifndef _SCORE_ALGORITHM_H_INCLUDED__ #define _SCORE_ALGORITHM_H_INCLUDED__ +#include +#include #include #include #include @@ -153,11 +155,19 @@ namespace score_algorithm double old_xy_goal_tolerance_; double angle_threshold_; + ros::Publisher current_goal_pub_; + ros::Publisher closet_robot_goal_pub_; std::vector reached_intermediate_goals_; std::vector start_index_saved_vt_; unsigned int sub_goal_index_saved_, sub_goal_seq_saved_; unsigned int sub_start_index_saved_, sub_start_seq_saved_; + + 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 max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_; + double min_speed_xy_, max_speed_xy_; + double rot_stopped_velocity_, trans_stopped_velocity_; }; } // namespace score_algorithm diff --git a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp index 62e6851..3e5362b 100644 --- a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp +++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -137,7 +138,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs result.poses.clear(); if (global_plan.poses.empty()) { - std::cerr << "ScoreAlgorithm: The global plan was empty." << std::endl; + robot::log_error("ScoreAlgorithm: The global plan was empty."); return false; } @@ -145,7 +146,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs std::vector mutes; if (!this->findSubGoalIndex(global_plan.poses, seq_s, mutes)) { - std::cerr << "ScoreAlgorithm: Cannot find SubGoal Index" << std::endl; + robot::log_error("ScoreAlgorithm: Cannot find SubGoal Index"); return false; } @@ -187,21 +188,19 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs } } - // Handle empty seq_s if (index_s.empty()) { sub_goal_index = (unsigned int)global_plan.poses.size(); sub_goal_seq_saved_ = global_plan.poses.back().header.seq; - std::cout << "ScoreAlgorithm: seq_s is empty, setting sub_goal_index to " << sub_goal_index << std::endl; } else { - // Update sub_goal_index if index_s.front() is greater and valid - if (index_s.front() > sub_goal_index_saved_ && index_s.front() < (unsigned int)global_plan.poses.size()) + bool is_stopped = stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_); + if (is_stopped && index_s.front() > sub_goal_index_saved_ && index_s.front() < (unsigned int)global_plan.poses.size()) { - sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front(); + // sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front(); + sub_goal_index = index_s.front(); sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq; - // std::cout << "ScoreAlgorithm: Updated sub_goal_index to " << sub_goal_index << " based on index_s.front()" << std::endl; } // Process index_s with multiple elements @@ -211,7 +210,6 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs { if (index_s[i - 1] >= (unsigned int)global_plan.poses.size() || index_s[i] >= (unsigned int)global_plan.poses.size()) { - std::cout << "ScoreAlgorithm: Invalid index index_s[" << i - 1 << "]=" << index_s[i - 1] << " or index_s[" << i << "]=" << index_s[i] << ", plan size=" << (unsigned int)global_plan.poses.size() << std::endl; continue; } @@ -226,12 +224,8 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; if (fabs(tolerance) <= xy_local_goal_tolerance_) { - // ROS_INFO_THROTTLE(0.1, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), robot_pose.pose.theta, global_plan.poses[index_s[i - 1]].pose.theta); - // ROS_INFO_THROTTLE(0.1, "tolerance 1 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta); if (index_s[i] > sub_goal_index_saved_) { - // ROS_INFO("%d %d %d %d %d", index_s[i], sub_goal_index, sub_goal_index_saved_, sub_goal_seq_saved_, (unsigned int)global_plan.poses.size()); - // ROS_INFO_NAMED("ScoreAlgorithm", "Following from %u to %d", sub_goal_index, i < (unsigned int)(index_s.size() - 1) ? index_s[i] + 1 : (unsigned int)global_plan.poses.size()); sub_goal_index = (i < index_s.size() - 1) ? index_s[i] : (unsigned int)global_plan.poses.size() - 1; sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq; old_xy_goal_tolerance_ = 0.0; @@ -255,7 +249,6 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs { sub_goal_index = (unsigned int)global_plan.poses.size() - 1; sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq; - // std::cout << "ScoreAlgorithm: Invalid sub_goal_index, setting to " << sub_goal_index << std::endl; } else { @@ -269,12 +262,10 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y; if (fabs(tolerance) <= xy_local_goal_tolerance_) { - // ROS_INFO_THROTTLE(0.1, "%f %f %f %f ", fabs(cos(theta)), fabs(sin(theta)), robot_pose.pose.theta, global_plan.poses[sub_goal_index].pose.theta); - // ROS_INFO_THROTTLE(0.1, "tolerance 2 %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta); sub_goal_index = (unsigned int)global_plan.poses.size() - 1; sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq; old_xy_goal_tolerance_ = 0.0; - std::cout << "ScoreAlgorithm: Single sub-goal reached, setting sub_goal_index to " << sub_goal_index << std::endl; + robot::log_info("ScoreAlgorithm: Single sub-goal reached, setting sub_goal_index to %d", sub_goal_index); } } else @@ -284,7 +275,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs } if (sub_start_index >= sub_goal_index) { - std::cerr << "ScoreAlgorithm: Sub path is empty" << std::endl; + robot::log_error("ScoreAlgorithm: Sub path is empty"); return false; } @@ -310,7 +301,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs // Nếu khồng tìm được điểm gần với robot nhất thì trả về false if (!started_path) { - std::cerr << "ScoreAlgorithm: None of the points of the global plan were in the local costmap." << std::endl; + robot::log_error("ScoreAlgorithm: None of the points of the global plan were in the local costmap."); return false; } std::stringstream ss; @@ -369,17 +360,9 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs // check if goal already reached bool goal_already_reached = false; - // robot_geometry_msgs::PoseArray poseArrayMsg; - // poseArrayMsg.header.stamp = robot::Time::now(); - // poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID(); - // int c = 0; for (auto &reached_intermediate_goal : reached_intermediate_goals_) { double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[goal_index].pose)); - - // robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal.pose); - // poseArrayMsg.poses.push_back(pose); - if (distance < xy_local_goal_tolerance_) { goal_already_reached = true; @@ -403,7 +386,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs { // the robot has currently reached the goal reached_intermediate_goals_.push_back(global_plan.poses[goal_index]); - std::cout << "ScoreAlgorithm: Number of reached_intermediate goals: " << reached_intermediate_goals_.size() << std::endl; + robot::log_info("ScoreAlgorithm: Number of reached_intermediate goals: %d", reached_intermediate_goals_.size()); } else { @@ -432,8 +415,37 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs robot_nav_2d_msgs::Pose2DStamped sub_pose; sub_pose = global_plan.poses[closet_index]; + { + robot_geometry_msgs::PoseStamped sub_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_pose); + geometry_msgs::PoseStamped sub_pose_stamped_ros; + sub_pose_stamped_ros.header.stamp = ros::Time::now(); + sub_pose_stamped_ros.header.frame_id = sub_pose_stamped.header.frame_id; + sub_pose_stamped_ros.pose.position.x = sub_pose_stamped.pose.position.x; + sub_pose_stamped_ros.pose.position.y = sub_pose_stamped.pose.position.y; + sub_pose_stamped_ros.pose.position.z = sub_pose_stamped.pose.position.z; + sub_pose_stamped_ros.pose.orientation.x = sub_pose_stamped.pose.orientation.x; + sub_pose_stamped_ros.pose.orientation.y = sub_pose_stamped.pose.orientation.y; + sub_pose_stamped_ros.pose.orientation.z = sub_pose_stamped.pose.orientation.z; + sub_pose_stamped_ros.pose.orientation.w = sub_pose_stamped.pose.orientation.w; + closet_robot_goal_pub_.publish(sub_pose_stamped_ros); + } + robot_nav_2d_msgs::Pose2DStamped sub_goal; sub_goal = global_plan.poses[goal_index]; + { + robot_geometry_msgs::PoseStamped sub_goal_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_goal); + geometry_msgs::PoseStamped sub_goal_stamped_ros; + sub_goal_stamped_ros.header.stamp = ros::Time::now(); + sub_goal_stamped_ros.header.frame_id = sub_goal_stamped.header.frame_id; + sub_goal_stamped_ros.pose.position.x = sub_goal_stamped.pose.position.x; + sub_goal_stamped_ros.pose.position.y = sub_goal_stamped.pose.position.y; + sub_goal_stamped_ros.pose.position.z = sub_goal_stamped.pose.position.z; + sub_goal_stamped_ros.pose.orientation.x = sub_goal_stamped.pose.orientation.x; + sub_goal_stamped_ros.pose.orientation.y = sub_goal_stamped.pose.orientation.y; + sub_goal_stamped_ros.pose.orientation.z = sub_goal_stamped.pose.orientation.z; + sub_goal_stamped_ros.pose.orientation.w = sub_goal_stamped.pose.orientation.w; + current_goal_pub_.publish(sub_goal_stamped_ros); + } 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 f4f8ac8..66857c8 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 @@ -144,16 +144,65 @@ namespace mkt_algorithm 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 Calculate max kappa + * @param path + * @param preview_distance + * @return max kappa + */ + double calculateMaxKappa(const robot_nav_2d_msgs::Path2D &path, const double preview_distance = std::numeric_limits::max()); + + /** + * @brief Adjust speed with Hermite trajectory + * @param velocity + * @param trajectory + * @param v_target + * @param sign_x + * @return speed + */ + double adjustSpeedWithHermiteTrajectory( + const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Path2D &trajectory, double v_target, const double &sign_x + ); + + /** + * @brief Generate Hermite trajectory + * @param path + * @param drive_target + * @param velocity + * @param sign_x + * @param drive_cmd + * @return trajectory + */ + robot_nav_2d_msgs::Path2D generateTrajectory( + const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &drive_target, + const robot_nav_2d_msgs::Twist2D &velocity, const double &sign_x, robot_nav_2d_msgs::Twist2D &drive_cmd); + + /** + * @brief Generate Hermite trajectory + * @param pose + * @param sign_x + * @return trajectory + */ + robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x); + + /** + * @brief Generate Hermite quadratic trajectory + * @param pose + * @param sign_x + * @return trajectory + */ + robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x); /** * @brief Should rotate to path * @param carrot_pose + * @param velocity + * @param sign_x * @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); + 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); /** * @brief Rotate to heading @@ -163,6 +212,40 @@ namespace mkt_algorithm */ void rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel); + /** + * @brief Check if robot should align to final heading (near goal position) + * @param trajectory The transformed trajectory in robot frame + * @param goal The goal pose with desired final heading + * @param velocity Current velocity + * @param xy_error Output: distance to goal + * @param heading_error Output: angle difference to goal heading + * @return true if should enter final heading alignment phase + */ + bool shouldAlignToFinalHeading( + const robot_nav_2d_msgs::Path2D &trajectory, + const robot_nav_2d_msgs::Pose2DStamped &goal, + const robot_nav_2d_msgs::Twist2D &velocity, + double &xy_error, + double &heading_error); + + /** + * @brief Arc Motion controller for final heading alignment + * Smoothly reduces linear velocity while adjusting angular velocity to reach final heading + * @param xy_error Distance to goal position + * @param heading_error Angle difference to goal heading + * @param velocity Current velocity + * @param sign_x Direction of motion (forward/backward) + * @param dt Time step + * @param cmd_vel Output velocity command + */ + void alignToFinalHeading( + const double &xy_error, + const double &heading_error, + const robot_nav_2d_msgs::Twist2D &velocity, + const double &sign_x, + const double &dt, + robot_nav_2d_msgs::Twist2D &cmd_vel); + /** * @brief the robot is moving acceleration limits * @param velocity The velocity of the robot @@ -182,20 +265,16 @@ namespace mkt_algorithm 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 + * @brief Compute pure pursuit + * @param carrot_pose + * @param drive_target + * @param velocity + * @param trajectory + * @param min_approach_linear_velocity + * @param sign_x + * @param dt + * @param drive_cmd */ - 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 robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &drive_target, @@ -206,14 +285,15 @@ namespace mkt_algorithm 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 Interpolate footprint + * @param pose + * @param footprint + * @param resolution + * @return interpolated footprint + */ + std::vector interpolateFootprint( + const robot_geometry_msgs::Pose2D &pose, const std::vector &footprint, const double &resolution); /** * @brief Cost at pose @@ -223,11 +303,6 @@ namespace mkt_algorithm */ 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); bool initialized_; bool nav_stop_; @@ -258,17 +333,17 @@ namespace mkt_algorithm // 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_; + // Final heading alignment (Arc Motion) + bool use_final_heading_alignment_; + double final_heading_xy_tolerance_; // Distance threshold to trigger alignment + double final_heading_angle_tolerance_; // Angle threshold to consider heading reached + double final_heading_min_velocity_; // Minimum linear velocity during alignment + double final_heading_kp_angular_; // Proportional gain for angular control + // 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 min_speed_xy_, max_speed_xy_; - - double rot_stopped_velocity_, trans_stopped_velocity_; double min_approach_linear_velocity_; double regulated_linear_scaling_min_radius_; @@ -291,6 +366,8 @@ namespace mkt_algorithm Eigen::MatrixXd R; Eigen::MatrixXd P; + ros::Publisher lookahead_point_pub_; + }; // class PredictiveTrajectory } // namespace diff diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md b/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md index 59665b6..9d58754 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.md @@ -30,16 +30,21 @@ 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)): +Chọn độ dài đặc trưng `L` (thường L = sqrt(x^2 + y^2)) và hướng `dir`: ``` -p'(0) = (L, 0) -p'(1) = (L*cos(theta), L*sin(theta)) +dir = +1 (đi xuôi) +dir = -1 (đi ngược 180° ở cả đầu và cuối) +``` +Khi đó: +``` +p'(0) = (dir * L, 0) +p'(1) = (dir * L*cos(theta), dir * 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) +x(t) = h10*(dir*L) + h01*x + h11*(dir*L*cos(theta)) +y(t) = h01*y + h11*(dir*L*sin(theta)) ``` ### Hướng (heading) trên đường cong @@ -50,8 +55,8 @@ 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) +dx = h10'*(dir*L) + h01'*x + h11'*(dir*L*cos(theta)) +dy = h01'*y + h11'*(dir*L*sin(theta)) ``` Hướng: ``` diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.pdf b/src/Algorithms/Libraries/mkt_algorithm/src/diff/Hermite.pdf new file mode 100644 index 0000000000000000000000000000000000000000..aabf3d7ed25a13ec1b8a642617ea354c1992f5ad GIT binary patch literal 35440 zcmb5WbC4)cmo3`1ZQI?aZQHhOyHDH3Y1_7K+qP}vo$tQ6_x>j4n-}vcqB62FBeVY4 zD_8B@YiE(l3yab+(y>619^PKPLa`Gt5ZD=7LUD7`OPko5Ihzx(aIkUEi&|Jan>f;o zS{pc<2%8w$8Jqk^HzNVZFIIM5UMMGLM-u}ZDEG`GPAhjMm1eIjG1-LY!=+*#=HN_w zNe>Wn3C&!HLkJrhRnj6_G7*B{08o74b%S2Dz+6LwQaGl-n0cRSEd z;F;T5q;>u&6FmW-_&{-y)OmKqN5x_TC$+j_(nmJj*m}kO!AHL)eLO#R?FAQX15VF!Gs_10O;3pKZnL^f5YoeDdF-6ySMS?l54p| zIL=E3PYXrD6UJj+-eb+H&jOb`g1$x6En0RcWf+)C(lsMX%6jeOt)WGa28TYT&%EiT z)KR6=Zkz9N?E8FsSU_k>q@fH`IZ)=)wVVH>0~QCnwbWZj{bBh1Nve+BbHl%qUpRJCXUviX{~i z8v=3U3uu#&KN27sXcAFB1b`F>1wtR$4`~+~P!{U88Gs&gma<34Tu3K6D?QG&ke*~86B+E`vfS-3v;LuXj9)?t)JOi;lpD z+!rTa_F6sVZ2-nI%bqckGe@R?PkOIAZ7k3#k7hKng- zIh;4l=HNV-5Ja?))Jk*Sur~(Z-;S);0^vaQl%iO={C_mVCI(j7q2oi4D}vp5f_R## zzwhf_4r;i;sQ2!T7@pL6!hN0Ir9X4x9L}Nj%T{=!+^*Gj$7A*3HlKw**JAww>+W9m z^@e+5dH@d^xfihD6=Q2d9c%|opv9P?n2iRVO`0)qnF)D!2oiHeXqqK-aZQ?59g76V z*WVW(ymB^P4xzpDP7CY%Ktp#f!(*d;k$#wYzh6T4AlMaN`1gN_3U2*kD2PsIuxXdh zC+_LaE#i$NEy8AfTFaTQJH`})x-U;^wdMhO*FC|-u!{b z5h5^nL+V(vTySEw71u=zu*u9i1}0pePXyoc$UW?{xJnxh+~yVs-gqEL0JqQ^fe>$N ztC~z%R>|0NzV&pxR2|b~!Ykb4Y&vSn2i0Fs(%UZnJTGJ1YhS*~%EFnoN}=C=QT0V^ zayyDZI`aSKS{=CCDA_U?Wa-9QQP-~~Vr2^YRDU+paM?rt_hAjgs3cTqsSU*{c7$oMm)& zjG<3Hi=8sXku*!Bab3(3%OUP-cNyOX>t0;w^KRi}`U{uWdwJ9KrTvQY^?hxJt`sMQ zfC1*706FBThhi|>-F?TE)>I`cU4c9tC2{Ir+u_kXrXgeUz+&Bc#)um^1ZpWSkAtYr zv7kZ-$BdL0IaP*QS{clSNuvtWA7R)W&=!ni3(x$8KLb1;FU+0b(%%m}f@SwJ(uG7< zMr5;gJR-r<(!oJjK}E&Y-N0f@7(Xc)rTjg5kfIM&`6vdCkLvpgtdlcmRPYOQJ`B$ymn>g{=WLr;xARn~ z<(f<-yWoHm&iw=6CGEj_8pn$!w-r8ZWW&<^;dA{|^XIgT*QjeD{3*hLF>Q~2c|)c& zCl^y4f8&M>h_T!!Pez1!tyAvD<;-TFFciNA03^^^Er2c1xXHf2Wi%Ds%XCYUbpA+h zW%?B^MVAR}3+^N`HO?|_opX=JV`BRLV=Y%tigKlv(Bt=?*7xhHx>Y&0%C!w;_%#A~ zR3zl^{Yk%1Bz z_-g=VF>*HmA^=@62rT`=05ib|z}R^pmTdi4fhU*cfh#Y_< zFKK8vshoQb!aAmngw9Id-GC9Zop6msLG zc(d2^BD>FH!=LYc_6|GLL;1whn;%C9>9LZZO^JHk$2*ZJ0&cgSWXa=db~$$i7EyY9 z)T>_mp=f$(S0VktKid&r>0}7O)nesh>N&9YjX#HWr#m+)4-+3w#C0s7UeUK_dZeiM z<2pYi@^-9k)J*9mOps*5*al3f1h)r7$OX&^#)PpS7b!p!QZST@B9yUAHiW>g0JAm3 z(1EcYNvH`UGNexzvM@AT!14%u()GAq{hk1xkFO-$l zusJ2l*rszPd$~!}M>1Od`XxULe0B`dSpBhI3y_3YT01>v#p2_9p|e*6Xjm^sI^l`{WL0&i;R>VqS*$u6-U(!Ef*`B(8ZFy`@(S4gY9IlLI98 zaixASLwC_+>gQ#E1_$HP2y2AZ?i%+^_MZuqx}vSCr%nk98lBDUTI@s1=>-ClpyD7$ zEColn)Q8R_n?E>^$ts=NkYp&2f_l5UhOAlZr{Z*kFBx$gr^3cbU>kR()q@Hu%QAy2 zij{T}r;2r1ipq#(iEV5%3MFe6Dc+FOY+0owSR`U0r~ho*Tu7<)e7CQt$ycLy7jEmi z9Z$A@w^w)<7wX`NDTTSWOb}2XX~z^}@{n$0+32|%m|(rT zgu*G9!7F6EXRI}Sd6lj=RE=@i%HQ;zs%Z!((6-ZX(;J&^OmP+jGa`2iym{;15QN^9 zGv*kr2=cB}dqlZ3*tTRlh$W*%QKVvNh{)+-USQQZl-(&DL}q=3f)CG1un(KJ>c~PA z+&x>_hqHDGFt?5z8bzIGfl$|NU`E+X6|b2PJd3g^xAmQXwP0u3GIZ=Iv1Qs1IN7@Q z!(wmQM`EkC?jgZu?1dbK{Zlj|k53j~b)eK?fcYMuS?@Uk+C2JgkRc;wxI}WMkB<28) z*pfd4+m|&ZsiMJgM*gbC9^IW7chivv1KRu**Dx^+-tH+=(gGT;Q!@BT(PX+g|A!Hm@{)e^%Pm%LD=~t^Hlh9d+zfy z*CR2d5#F+|R`M(f{ieAcxB>#!;$1OieSBLvz>$D`tbmPE--m{cfL(doAP~EJw?<$P z=)=02=c2YspbxwTHpGfxW8V)J0lRK9YZ|zQQsH{AaB%rgm_4L7!wJN@Jka@dlW+{yWpnrE?^ zwCwfX(}qzc~yw`+3DLA^WYjKLUeQ07|InpVA>@vpTt zw8+xP)BJWkothX@&PcjSmkz{UO!KU1^TC07#*wqx1g-bD(+9LEH$C?qaohlKJL}Bm zEP>?^uYRj3xec$E&$b)q8>5kj9L6f`W>QI5-`?1@Km9rcB9Ce7lid^;DfwEt|gjU zW3G-bFY~nI7xL4xI6vNy#Llp9m@%6ozk{!T;dHW}yHH*_+z-1&2E^{cI`U;JR1cg8 zc=HBPexRMhwB@7=*%rpkS&iYkZ&;9X>0_=1JQ~y^AIF12$RvXrLuxq0&tg~*$0lS* zJaUKWWV~kH?%?&N?F2eD$D16IX-hn`3?P|_Lq4eWkL(bJnv+la(0fQ4X>j#UsLW54 zl~9)dkgvAIYDYE!ZjX4_=F@)xT7B0<+R`ov*Qc?m;7fk;Zbvxr#ZW)eXdaY$uXVo5 z*H$;1%DU)3tElNJxV%-4Pkf>t{W8<0`Y4vm@El93A4?=lmZhM>;Sq@_9c7qKGNogT!It8T!d#u~O=gmeaPBQV z%VjB5Kvm+T$Uj*!P*{iv5haz!t)y^k)y{J;GNU9b(-w+POBWYmQ`%#W-)C|;H!fIZ zZ02&cO9`fd>iC6SG#p|T0JY`U$=FVW)}d9`{%20Hjp_Y35y-Mz7?b*s8ilQj^>dEM=H&(2-O=E2?ENkW&C%6Z@z6h596Y__sRy^6Z~5 zx&+}!l#9{HC=sgJO%eoQM4fOmHcphEF2W%s+W^YEr1NhM6-m{n&RoIh+ejH$h`t&* zM;X`FN{jT~mpsUH(C9!k*EZ!})|UxmM3)qE=lb>Z2_i9H!gP+IC{&e(?cfa0oT$jS zy@hK7NYyqfG7QBG#HK8gsOERku#(P*o-2+n1|*S&@8f$3hO8Cpswng3BAH8-!zTIt zX!-GRRt&)J(c?%B=C})LV=I*4aPsC&;U-8C3P;M~sC$cUDv5qT!pzP^TxSqX0wQ!H zA)9fUbrCW9M2F@O2LZIyD`-lT2}F|3%*0*o=Z#d!OCT)CC>aSnN`!kulypUHKQn(pa;$)QIfEPN8mt| zx0fByRc11Um_96OF6qu<8`Cf&I}m#27GNpeb>Id@S7;YMEo_ZP&NhI}8cNfvEDb8} zDM}LbrOhaYo%i1xR|rjjTJlS!dWk@XG@{Mc!D*-lwpm0_38yb%Bw7Y2u~<%$w{Y01 zj?~9fm6m5Q+R_v^W8xxNmtMyi2+8=|VscN0V_OD(Q0AJ3m<4e{A)tGUk_a#7CPAtA zTZ$=jl^1fX&RIRim>CP^VFo8P~`Ed8!wfE_5HC1EezN6U-h$?gSk5m)`B0^VfD_wnrD{f~hB# zKoo)E;23~)ISH+!MYhzhNfeu~NC+28negy?B zJZqJncg_r>Q(}~qGNr(rhe5qo+Uq9KF6rlRRu}~|j@@4eN0H~z*PRU}q&)803)7G- zqeks7L~8gn$#f;o3$fnW%;s{)^Egi#OyeHuqVs*oeg8JPdi6QMyZ8xr;Bbp74ds?R zke}HP%E0T1m}GX%_PV^B8a|?YeIwds*@p-`@v5w>j$)SYkf2_W22#d6>vtths~liv^*Qmn;j&bqWZbU0kg9e}DK-4;v(K3So*XX+BC)gM~Gk7NUTHdB|| zwd>Q%?J9U`SI$) z=-0QSW9q|*Pl-vEQ$H_pjO*YA_LZR9SWuEx!atkat(=xN_fNp`uuopaub&&F$H z__B|Wop05boIi!vH6(kV_Uqy8$owll%+b{!ezNRSzV_e#^LY1mprJpfQn!LH$|sTj zf+r6>ct_IQ!2Z?ZYK@VqKl*W}^&X$of!%=(@YU?Ap7tYsKHwaC7GalmJ2jUjQy+O( zl=9g?4@$Xg__w&HT>N5xSc4UlY>UM5BA?%$6|Ur{n>)i2kDMa;8EFu;rk+N?9H*QAK+bhYgtw-w$C@$+g&D>2UVM2dEa=c zJn@B_g9 z!_-O-I-RrNRN#qi374%tkux98sk5G5r|(zK)tTWB@-C`N2otj<^#t_fb|f2omLS{e z(i1AA$UlzG5eq)WfEgy}IjI%yP1IlB_JVq5kS2`X}EImd%RQ5K?6b(t&B51+>I$qBnGwhDdLN20@Q0YXvYB z3V4ssJw;js2wk2Y0&oE`gxqY`g-v0q6uDUhNV%DniodT%Ko?ZuOK}m<9~_P z|N6!7Z-VvzjL+*N%-9ApzzpAbK;d@?fr4PELK4bs1rL0iTf@hZ6$@bzLOu3w(8pBB zuKORNpM2mTlXJPuWMQj~ZVy;{Y8zl=!M0AW=p#(KV`fp1tj<-os9;SMc8%t@?Ifd? zOv5&qV~z6eAJ>P#y2`oo9s~mTUDB$<6m;}nUo1M6_IUmTBDbSD&Nb^hgq`U&`Ys_0 zCQp7$4}UpqJu&zwkdsyWz1SSo1;=3`k-YbZ@eSe4HEO2kObYb|RTPF03W9W&{yU%8&Xm(IL& zr&>YOk)AOY>dK>whQ7S)o5@u_egVC67LNZLbNu?h=1}&qH=$RTGqf}@a;8^uF?9ad zv#6b;&HpO?)l0xgFDPi|uEp?o5E~N%0W%YW4!w+lv!jK(*1zn^K=8jx9ePC*Cp#BM zBNL~;nv96MvzU_eUlHbS-GJe5AtV6BNN;37z{vae`a8F+^WR2OS_WDM0@lA%DE`0A z|DAgCSKMLx2R7D=ls7Cl84!F{)FumHnhH8PYoLor3-R+CD*!vl*le*(pTu&X;#V$m{rn$b_EWCVbGL}JY(w9ZQVrYPF zGL8*eRc^q`m)BC!)Qw9orpTVV3Z6VS7ab~a@IdU2Cial+`_Q_q!q_#%aC_NA3ki0t zwwKkA^|{{FX8-|Ljt+BE(4R%7WreddxlY>D;z~wo8dh>4Ji^QJbc?;xbt%VKMV?(a zZS|^*N|26o_s?h$6Uxd88|jjd5bda>t)Cjq@m0$;uY3vqYo_BcL1YrjE7!Va?67%Z;GnR#DV#Hrz#Je5F(OChZ9=~cFTtatnE7quGH@Qf!Ht&`aZ z^$vO7f8>6%-fQZ{?_}}TsyDNfbs2W<-FYos;72`Q0xO_NjE}@Lo};PDi#bMVJhq;^ z=KQJ_GxkrD%+bhAA>)caM}~bs%Ek&#UefNXayft}aQ?J|NVp-cx8rz>x>U$v2UnTy z+z()!N!#l8p;1cIRT|eWPmYuNa`IO21vHBk4EnDtRjFqFD=w95&+0MS5) zVMM?%;*E-MTOS?pE(7W!h}Y75RJlOP__tY7Vv&W17sttKM@@&r;Xr(EW3ztNIl~rZ z%ih76n8lu@zKObP$O@)|jT1M7nFVlB$@F9<700#BqKbkz7OR6yBDF}|X72(0A5Pkh zPVXmm98GdB*o-&LN9-h~(~gMTD_PNXcHZ%H0>!I%?Q)uHnqR0a3JWE9(BAyK=7(bw zfv+VRpTg5nN&)PvsQX^*+FXlj-qNnv`S|C|D>`GQ7&GEP0;gwFrD!92f5}!D+t9VV zbC~EKYjFrO;5(5qVI09c!Pl(EZf>1&c%hY|ol$vc`44dU|8Gq$4P zrvlARx0C2|tvf2iSlOJl97c|ht`H24WnW)i=-%Fsi{6&&#j|;MMKN9<$!JGmcpF*SfiKs!qbaiMp!E#2Ab)0LagQ}Hq&QGizT!`A>};Q_@W zbIE6+ay$_Ez}p2rO@5o6x`phQz0lnp32rYV6d}H*2aYdH{-FuCLSGJxR9~U9Mp}*Z zK5M7DzVT!&)TJ`9y(oIa0EZWaKd8-M|GaR(v0DRo=pt%ijo0tzj2){)V?h_Sh6=I|3-sD2`~`(9?WjQ7XE4lFhu&0*>|OeQ4o+QG4aP zPuYNsQQph*%``Q0%`p8N2UJ{dcPA~X+PX>a4=%M1P|;rLBTU{#NvCr$S#o-R>vHYk z6+78;#bx}fAYb%$VXthkAo=jIENPT4eEM5k%A@%xiC}mC*`qRvP7ci&4k4cNpc+?e zrV~^T)6=Su_3VNf)kU5PT{++<085+K``z9_h|xc(&67T!b-zWSbsdo?AcqwH8cU99 zy}HfP_NV*%9iN)idFtO#(?34m{~y%E@QeAM)KvACnl=~^t{za|0j~LM$yjD z?9mJ3yqzhZe=;To6ca?&)~#!;R$u!|?N+krdUXE;WGJE~-JiSL@5&omza1g{-gTcHr}T7|)bU@86s&E% z%YWP*40{;aDapYY6S&}QX2Mc0)5vG+|>jPdWXKq*B_^}$!Bmo-i4k{LYYut6nK6mabY0nc}lyS0p2iL3Oq$P zjIBHd@y%?1nLjC>J6Rvz=F~~I=hvSu%0Z|Ww>LrGbuf6{|~ z;cJ1Q*Z|jwDQGh!Hg80{*%)jagKeYOGjZHsEC~_(F%MQXyCP0jgH=DzDY6+)V_R8T zk)2Eu@`pvae+_blvO<-|NWH*f&VjCor~PDSd~)nsgMV1IO~=E_U=5e4@dObYOzxQ1 zrFCQ0SznVb#$J2vux_5@n;4FDTF`$lG$VH4Y`3u_sQ-rIN&1?h*iXy(;YZHAM)oY- zd#?%okVtpuK3r+M4R>lD!+;l0e3VKmycwa~s7){Dwt1e*f^2qUl|@z%8Gbfr4wWhd zsnO=m-8YNqe-q!V&&Y=e1`&g(2`IkvrkNGSdIf5x%L?A6h)f<7#g_vj$j?wYnt9=7&N$ig#^p+#} zq^CQF^Bp9UH`w`n+>Y0LOk{Qzu4TOiv5~sWu}9pYPGG6u4wK@sVuDV3pjnTBE-v}B z{&ZWTXiTUl7oygI;bMCDZW?JB_4z399(tAMg-hUu$BD>of%?=v>YO!PFgW%XpRKin z5392}iF^dC!F8DmlOu%Ln>C??eq-dQA4&qgNpP$jo;E7yu7=XlWE(@(8~d5oMvUO~ zK_+x5FW@Cz)|5VN=!B8?^LaCYzZCMu{L_3IYhW*B>^&THvBb8!Z}8fbs^{};wUE`V zRgPz&d3quRwb6Vm@<*rd2lQ*wNbkSllYhD2|B*@c-}4Cv%U{R+|Kv`sPpR|R{iTzw z59(L&%D54FL88%3Hk^tj~POlet@5At}Uw!P~ zkfwk?92q~gZqHwXy1J-NS-1!IIWnuvV37Pv3n;R;M6*g?KP&`23=G8 zxoB@=Y$CFc?y&{&1ATS_W=;+S0!1S=9q5i36!TSxV|(QI5Vr-wrq51an=`Hhk?okV zPB5dE^Rk9ZDWlYUE^{(F9)*ph&+v>-2KTf2pA0hZG(@EwD(mkg%3qa^<`l z3S@}=MoKo5?=+Oo%xoEJD3pYy0z{{owP4odN&qkb>}aY&~Z=HOOUoEGFjGgQWy6qOM996cXd@l-L!E_py3{I#QkRkDj?_ zHD_o{DuAlfgCqjj5lqWSlBVGWF=3$V%7(PKjrfkk5`vqAgkZ(M-+gH z9Y0zZ+{!EV_!MrM0BmOyFiSY|Xf(CZEP=|F2})e!Knp^LJ{#n_8kLjF(FZ~|(d1p* zz6fHe?Pa(F$~@#i-Uh8ExHTg^sK*bjPuRh;+)|~dlTSaiwpd+vpL8k-I_kF)M28we z`QT{k>7jKNURq|s0;IEYxBRA7K&5RQs4d}Jbs|n}bYjq?-{_#EF{DEI(fTPbFx{3xezYnbL# znl+yN0{-#iZOZv`=$KxZJ(rfb4%O&B|5P&Pw94-@&ot}-`XphTQHi{K!LKSaJKQ`%{ z)DP~R#6Vy+o#EpV+@IFKfI|t9gi}pZ7+eCr$QO3?h7s`Bvo2^Kp(Lo z5ce}HILL{-sxDgS28|!kO-J)gK7;>A)vRr=D5@zz6dll8C!_15Dby>Om^McUtsgoR z*#fDchW78OLD=IKR85l+-;olK1JO*e3z8NVsMufz3vzU1!AN+z-+cpy)E`_!w zGhOO6xP+SnjSnu&Zgs27U*Xx8J#q_i7EdBH-Sr7SNi7DiMKO;Q+~gAOT;c2I-FX-C zOU6__gl1PVazx8Rk0fws$2;wsL|4w$zEnm7Cal3;x+KWdM?z5J8Ug^Q>Ve*%!LvAR zLC4}0x9XPAQy$b50gC33Ch|~I9$FM;!AMlOeyCpZVGs76VaO=c&~;`}w#3S41sf25 zhIktTW^IDFePe`a89jGaxzp@w&G0r@C^hCEJZO(NRWKSHuT^P*RqOTzrjYB18_=&D zX3jGh7k1xR@Bh><*s<`?UCzR@M(*&Dxpp%B8`H7Urx&&JD^3T;kF z9>;41r>hUw+k|}vRt@iG*#-J~X|q+e;9d*I%7LK<@6y4|`{mMo7k4L5&*$gUBU9#u zu2?MnxARA3$w(8Xk)`^7-bMiL-UC=1eJZDbmLcX9cdAYmQLeV)B( zq)s#W@;oR(p5tnzMH`Sj+f&Y=j=UKZ7h)DQ1fSwZ!y>Y6C9>qbEvP)(G<8wb2^=O< z>NL&Iu6!}rQuW_b%jXo+noC{NAf>i}?C zwi`(ZUz1i+9eXoWk6>ssE=ybDuu&TcJQgYLU7Zw;c&)I}EMdv1-M&HTBB*caS8}Tp z#V`pqftg(cTOtHo9eD;0yyMZ2gm@dKnhX!Q-!)^$tTHb}sL2-Up%2IBoJV#s&Ua8RlW_b9Lv-HPE4;tG*<*yol|}N27bv_;TDm?u3N= z*5->l^{*Q??DA|GK)h3(iJNgX4r~}ko5I0xuL1@_hsa(KaQF{l`)1}LIS3jEfNr5wntK*JA9L6#sNjtW8L07fM+E4{1g zfUrsBEVnD?f$A(TFR`n-r-jt2a$DQ9MHyTy8fvYE*zWft7=S>nXzWNN^A&WPu_zZw*0 zF3cnsMmU|R4>CVcnmB?5GrB0J$A|x=>3zHW`+Af0xY4VT|D!T?*}LY1iQWr!>j&Vg z;xhi<;DFo5!Fnf z=mqxO88mBzj|Q0^i#plHPAwRSC6MDk-iCwZFO+LS1Cc%y2DMQej(? zq4-uysn3~O`KbR<(G5uN>FznU%edit`KZ2fzW+pyy&gHtZg?S#B=202D4M_))UGJBjcqihkqgD?bQ16W!4(>T%54x zh&f@gsiK2h?V}N#XFX%kEIw>jhPH91F)gU|gYK4WGNSaZ3=g}0D6cxF;alt4>V zBvHs53X?{8Nc4t!m{tX7(i%r1}0sqju&2<6gY*L#t1 z!4J86h;32qR4?YrwHR0#T5|hpr275_OavvW{;%L-{fC_Nzaw)$4Cg@$KMV!%fNnNC1Rdr zrty|koPR!beO!0^_&^dSA|izkj@cRY*7p|u`cmsL4qF)J<1l+>60&Zs8j-U{)Q*op z#1Pac1>zCiE$j!ud{&UCsW2XZY@HBc+!9Hqa3h6~*mU1`I2yT;RhB{Px%?@1Y+z|D z--HYnX8Ij4QFwFn?HO6L7s9X=VXxCEVJDe!uy@2VGZGhO9H)O0n24i+tIs~F8r6s8GDb5zd&XU;r8I$Ma#v0G^g#e zbB6NEES~QpI>K-5Fv=kCJ&8%eWP};S?Lnsg6nB4hkxH^@dRpFjGT*B3xu{GC4_c>joXj6~_IPs8P~d68Zc0RI}km$))+fSp{p(YpiX;wsCr@ zGzvKKn`ACUB*%Rf`)8nBeTB|d1NqYl;e`LW+qN9+Rg3xQ#Je6e<1Jr z3FraYazW7T>7Rv8gfP(ZfW5CbRxKgSd)SdMyWTeq5 z&WSzp}ub-?!Qc<8Y>QH)L4<=ST%u1*t zh)4MLqIUkNl-piDmO2?9;v&ec&Jn6% zcBxNUuSsHCDCt;e*3j_U%-jL=x2YXGU!}Wda{+*r$A=mtS_hw3_^wXK-+G%IA4H`j zWOk(od`E(=p(@QupHP^X>wIa+H0o&d|5%=1wd&?_9o&r7Wk5E~Zu5zI3JSN(EMUl? z<5^qH<5vA~M3&nr>o1Qks{A6@--Cg7Ax7XF7}=%i`UpqnlfdYlN*ubwi6|uIFpe=3 zvOD1XWL*3zUJIeOu$nhxE2vXjxNbYffSBx)0TBS04Z(=wd z{1Bvlb5W`5BO-Q6e3Q3ts+yTc?*IDQzl0n;e1ft0 zF5mNYOQX(9P}d7esU3K94(Q#N*oYs4W`i(z#RtwN7H_W3A>7ojP*M8Rk6rPZE zCM2X3YfmShyhBmJ0xhGi#9nUs_b1FCXW#++y`_|l3LkBLYH9@0=^(!`Yh7O9k-UPf z!EXZx#z?jOGDpIsLPP1A4RZ4kC0^LfAilvZzeI zMuUV_Y>?~Up9}WsTnBv^b*xV(LQ9eD6+yn~q!hn;MBZ^7bT7Dw#REzpG%Hxr`NqgK z#15Km9{KzWZ9fNmm_32_PU@`BO?y&xCe1oN;VSdZz-r(m45aj<0?vmcZ%)QGde9}# z!NhH|eh$Km4~Kf9kIXB7eEz23YvR|=3|Exv-Hn^MB(-E?)*aWxHXnEW3|4-^2^jrZ zRa^w3Dw~A@W0}p~*(1LT@KCBn{%xqw$_)wK{R)SJSy`@1FRUKEvFq&ex50s6dCmpr65Tdtixa) z{^J0hdG+sEz&_(j;{^B&1(vRr02=t}EEa-bEq&F%9{Sh4v0USHq(}u;ow#r&Xaxr! zENCX0gFDAg?Lao${)al4iL}YdgakpEV$iE9rh?&tCX( zsGNf*%Wbt+)&T7u`EB&3vsEapUrO2iiFZjoGvFuF-Q(L2`HFl&n7U9aOEZ?hu3`%n zx}8AG(qFrqe`2R;)`>W8`@;0YI_Dd!?0Gz|N(&#rF53xF zi-9Zt3Vy}Qj0Pi5M;GOPDlBy?&*rp0kXQm5Ki^1xHFPv0X`G5SmZW`UR*IpXy$w;2 zXHn9Ok5f}9E@Kgm6Q8kj0~?f#?Z$2D_M$avK1`e{ag-!9rjzPYm<0M><@#HU(o=3p zl;jd2M@l$?k8{Zzh&I7a?skd zcIU#9sSOT|bcbzQn(nf<-mSs{H5rdJ##t;xB+VI8ZS`M8NaUn$cdhxHkr~DI!d3SV z^6EN5k_iGx$3dBfDOjP3PR|*} z_uJnfq{Jtoer(5XgWb1PS)qBMH)|VHn7xJ+uH#)!k^S7Ner5@=&CzC4>J;X74%I{3 zfwibzr|8=GaUs2|KK@X5n{t=G=HK7W)bDVl{-!kGy=;Hehi$m_$1=6?O`+$ORpfye zmqgJm3>JC46Z`Wk^!vi`cnd`!*|Vp(?Y=bqxZAL+R-@fq7-+ls-i z${YHzjH?_eMr_ssGY#~OrnoMj5eQ9ty{Dzf@+Ll+i=Yeyw#dHqBPp)p7+Kb4$8=syjP0TWbKGAjUq6}2qWIO>Cp$qgq+2j1Ap{D- zhCK?3#@b{q`_dXVhAhPnLOoB4jw^2ZP#K*qXHj0#?)5TdPFDlAsx&vG3JX_9HsmJO zT+XDFrhnC={8r}OaprD&;8GANIeEgvQ4EM`!jO`!CbK^O+!EF~0e?yNs;exo5TyCJ zybeeE5cLfcT*f(*kdCg~lh`S zkDH()h>c|VCi<8n+|gOcxiBXsrQ3)s>nmy9hkSjeBYSO4(;3LtW)Iiy`pm}E~_=G>he{iYAPijvQFN;Yr-c!z0vLmWp8`yj08O6&jUhk+4&coS*#FUu_7FaY8M)F(gPoTAT=f z)rW4ye59_84wWT+Q^m5C)y(X`SB@``d6~*aPs?`;+a6>bN7_TSok!1$H#yE=KhT~% zd$SgL%}(pG#b^1FtN@^`zfy^`Gm@v&*OqW`X)6x8Wyjkcj{w(g2kX?jV1|JpFxP ze|{PiC~uO4-L^z>slx5V0O9S#u2shUv2UKh!9N8*Zv z(yav zON1}xT=GfU?8g0_k=P4pA9ZiQ9BWRN1Fi-$O{U=hhY*kchzuRIjWuy8E9(py*uXlx zHl8q#LvQ7)m#d@dTeL_uu7QpmI`yDt+4=OG<;DzN^2}3h!Xc5KxcW-=xy9ZyE(HyB zxY|0^a5>f&;ZBYbRHYqEPRTQz(vYWd*iB~|@E#3#G5{|RU}{oQ00;U3!tK`=9KJiz z?pETtH?4Bpx|oBXyIv9~fqcFJV1EGIA9rw05J}iW1`{7?`#vhF?z5I(NC9j95UM5Y z)IU)Hf_eMm-Z<9eoVmPBFEw=!DH-OI?h5CVCD;*j*4H9aF9gm{zH%o%rS`7Y68y= z72`kUHU0NL3n=X6sS&CR5nIB5Rg`)BIGW;S<4TIBt00aSM7CWk$;1bcrx0fSXS1lD zej4^v>cFNu;3&0)m1#|mz@=aLeZ6Z7aIC%t)6(F;uN?mmW#`xRZQHhO z+dOUCwr$(CZQI^6UpDh*XOrDuP?gkEN!4}Bp^1=}g@htG3%QMPC320~2QLlMbk~w2 z$4B>EN35MH0m$$!uApw>;b7pRmu9OI04^N{ksTe*haOBFs0&Q}IBum+oDWPOmctD% zGY~e4rB_lL6`dRv94sZgvSMPQLfn1(K<&0ZH^4Pt|Ii4|el8Oj4dpkWZGBJ-6&pzr zT|2jzi%3kW;uN&n!J$Q3Qjt>WZs;lLX$y-DKvL7KWN}0`CLVaKGqe|&VK?2)77WLG z4+N4Pg_gK;>TA+{b;;4Lq7vp=Hkl58_$LYjehQcv9e72tFjU~tUhLM!*gkEsLt%(K z$H~%^W|+C4`C=;Y$gK;0vq&mN@pHv=HY?UmEON_z0z$Uo-tft;-r%~_YB)%Got~(J znc$Ir*gMt`)JG8Xqwv|k<`@fd?RUC94!-`=2;g?WpN9o(0&w^WU{_GIbb?6S`u62- zwxXJ0shdY(8kB`J1@gO@cdZxo>U8s8!Fr?f>hZ4ATW?vEMO!-XsNw(k}PA_{_3~6W2zDVezfdMCH zu3^u?@#UEzlx>54tw_9%-hF{*&2>qdoqMOuY8BURUi4lbVwYVLAQ2}wr>%1s%pMK! zO5fL*^(iAU z!F0?cj@%3g2Di0!>)>A$_$a2TP-8%>6DSK3`cA$c9P8!4)s560_zIY1s{!j6Ziwr& zXMj7dy8OWOXnk-zn6yV6;g(L(5BEfUrU;D`Vw6?V6$JgcVlNMjh$msF%g{L}P7Q&P zP#b710e#c3(U>VDGfII(!A15JcG4Kxu%bN!Wl^%L8D%TRg)tzT!Em{7qY#`1>WV(8 z>vojae4Jt1qS7@gV{n68=PzrJ;PrJ17R=(5G8*QjWD#`lLLVeIdG+zTbeqG!pmkrT zYyd{-(tz7i{)_3r#h}H1X`^YH0T=m7)=%Y~I6@{cFqlI0(6tURP7;o8L=EM2FHcJ1 zkrGpAwDtX&tJOCi?u2zBxkP7TS3HA zRZA9qEeJ-lxMS*Qs!M}i+o+y}(QR(jun7MHkL5hi&*vf_FBw@gMJ$qvISJIU!$c2; z(2maO#l+e`h`x0Tbdtg&q~ZbGVHi(t#V8dS-oU{+ai^xqs;r2)^MxeH^p}`@yKTmo zYYLMwxPHC$2E8HNlbz7iXX(~$Lyh|n`G{8rN}j^t>?;O4=bTgIqG&~ymlP#1)oNC? z@g|}<17?Uc#6hl2BcTs;LY;%)BYj5rC&fa(rhqvmH#u+X*{q?IgvfkP6Lx4y_&=ZNY@F%Ip^AQ@))JX^vU`dtO5VkV`fRqk zBNuQTKS3?vfoj9n$=gStL2>x}1*)98rOWNG*X^7S51RKGv9Ipe*>evm1Qj{SgJOS} z?JIC83pbC3Neu>KkF~k^VsAxZ0>~IXh)d{6HJnnoVn$9Q?M}-cT8R5Ohn;5L>gZ5D z#)ZP_@%I?3jY|T*G8|U7w-A5MIgkXN95&0(?AgPMYWtkQ)x-Xt&qw*hbhkNXazvJ; z-+t6=u^<3)Hv5`-O(1dBg-Fj;?dk-gOGN<(9Ul;6&4uFpz7IvaiC>$WVWaWo5h<-n zZ4|59Nv_Ld3n{sV&(kM8yO}c zPN+k{Jq(!qO{qjHv|eWfhN}z8M9jtc6AA9VU{Dp#?2@|Hzml!ZiZD2kX3^aRdI7s8 zasoE@Akx)(JWm}=OK@^mN5KPHh`B9v`;$?{@=(VBbxYC2wa?Lu2IG3X{-HE<>569s zq(q$FY6y#OrEZ5!{~Ks%GaHNM#EQyU>)NSPa~sY7Vs-{`Wq%r;8kA2T@Imbv?@DHH zd)2`i_5nD|8*KNbT_CB89=EoC8hUgKt))cz>fWA^t~6faQ2N(S*nki8s?>nYPnZi_ zNe5ctEsbLDWQ0m(A4GdkZ+MbUmGZG%F<25K#_fDgf5W5bJm#A@FV|uv{;3}-f+}bgv|`zWY2{5zVo5iNv02|3dM> zm!4!pF1K>@b%AW+CGl1bo7#f#Jy}s&WyBUeusBX&9WeH8=sajZbr0dNvrkOQ8_v; z7&vWq_n46S7PfQ)lDbM1Fhc|^kETo81-ZlVmh+ei!fA+` zAQBuPtFJyzS{U%BsQxU}Gl8)#ZS~4tIz>iNTlpkGOUf3|v|;|cwRaCH95-Y;G4@mc zb~1;sXa;HHY_tHe*C}e=zFumeKNdhLNFcVEZWK|1qGAHFbA_zvHl9<4%9tQF?UP~T zs=m3<$k0ML6gLG83DLj67vL4O2Xfjm(Br0iYr}PX^Qd37rG}&y4wPmlzZH7uK5P0@ zLWvk%Kc@WTrp)EQz?uC^ucmsWDBryJ-|tv7nNT#e#g1*_GEf%tSW`oo?jMZR>Urpm zsdUD+@ZTUpSt*5mWLuM{&v!RS^Y;^Wb{M#KSLC{`rLI~@x&C9Huc2pfws+qK{PMJ@ zn0shf(yLYHx(>U(;+IM`EmLd5%WB~Yv+$cXTx_O|C7y!Yh1d{HqmG$9zdI&#nojs| zIpqYY1aD(PL#=^%y2A3b%m(O9OSUuXUYbtd8_pltqwSDFi8lqj>}XJ4lAVvqs*p{u zcCA4kf*qK@1)wxfHJlamnJ;?&wM9G`!Njgho$tnqEDqgwswAMrQ_eq(_uA|sK_~dB zYLcfHEJd895wxVd;HVULHY?&pIYn=WE2C}2Z$TnxkF$jpaNb zTksNa2Dg7{>684p^Eg>kpQ`Vt948yJYjxt?z-p=NdZ4EkN|RvFY>qbLrFnHkku=kn^842lS|k0};7TLhLfuufrh9agg!Tvkogp7>;n& zMSz3*U}*WZKRkdoXbJ0?ULK7p&i(F(g2AOdfll*=7EHm_#rat+KetMbX`z_WZui;A z@Ui$V362K$eB4nQ)%RS4F?ABLTU$?8l4ZzAmX4IvKs)3EwcIL(C8bP&I9L+K=SeG% z>_13%#BI%XF4uWeW7kv94KLT+Rl%#IS}(C3JogX#8}^!~Qs{94z}5A*-R6=CKQ-Ic z`kU}^GjCwO@BhF*@m!Iqj%+rXaB$$ylh95u63qLjAlZZ7-V@QL0gb`SdWQncywnsrJ(R*A;)-_|9Tu?%SI@FbQ)Zg1{v{%>naQx;? zB!K=w%7Y;QVl7u;xI#|LBW9r@O-MFk7?(J@cz?umJajU50n5qF)u|$^=SD>=*bCvT zT6u1C9;^JB;pOJ03Y^jxjHJME6*eatW>tS6+nv$)Mn3^1YR%t%{k@aQF z$RwGKlpZ)dEDT5{F412^owuiYp;W5ue4m%mbIx30q7}v3wz=x7g*8=16%M;&H?ef2 z=76HqK%I9|a#UPgxO)K8e2wadJ2%Hmi5Ycq5VmT^DiWC7A*!>gYM_BCjk3`A0P5oI z;zuHlhFn`e@Hz5+JYO^+Rv(}~%Aw<{pv0G!D7eM0IlZz0*RWo9166xtOSJpYCh1*g zDD8{3Z-P%hl6|L<%K4?WO=WHNAmDJ@O|Bap3YU}Y*^ygrD%R`u?@gR+Eli!)AnEjNJV5l6?rX9T@J8fQonDeT|L`2}0Aq&;JsZ z^6jH3%#Z>RD>W`bu)lS`ZaZJUUv1;*3fraTn#tC1wdtt#u%uHT4^I}LaSw!@iVjmA z01#QYt;)&t$LhRx1Mp~N}GIfjJ=MiE>!^&%Te zqKHO2C}-Aos}EQP2R;gOCet#0f}hdxu*ainO%~hj!2}i?t!~%6Qy&1FTA)zxVH9A6 zC>VFZ#jf8RKtq3%B|EI~_ln10_eD$&a46nJZL49At4|DeR*l*$cx}G={5`J?ksdn} zz*Nv5J>SBQ$ij zPA}PknK05azf9h)vR{&~*MvwTAoo^7R6o7Mf98ce!RPZFYs zG|A#4xsFc=U9_WXX^36! zKp{9S>Cy<$PHmVQEvikFy?qm~?_%ZTG< zV)6|e&JP5!mGe%WV%Yrt{=&?i)IuV30t@No{Q6)n0x-FlIJee!*XD8gyt}!TGE{={ z|A=wYmC)hAn?h77)Dl53E(Ab5KzUHIfQt=g*s~hty%d^*Ty1?fwr+!2gr&mi0eexBsO%Al0pAbh*mz41<#B zz{$X9LP0^JqoZS>qo?_^FZlyPVWTtWQInQ{!IjX08SnMsA3&Je0|025Lh=0%_we7m zb^pI=1T+1A2C0`dAeEI?J9q&T0#OSc!wlhg$lj=D{)%sESye7mt^}w8f&!Z08bieK zrwNb(a|vU>JaUgk1;|61<5ZS4$8@{4`PkT4XD9PnNzKt-D&>NzTGiC zzG|G=S^s^#_FLPP1qlj=BtU>D=|1_`n3$e<%+_I@Ao?yqU8mC~h=pK&Vs}v8Y8;E& zM-MOp#kLt5kK%y-V{K&WJu!7;i-g;N#G#Qc43$d)A#h|o_&l&Qk73I6&Q)HzPCXd7 zXgZ5X=&#=pocw$}A1c#@O8yG(NkmUBiBTf$T-V&{rcDtm8wCpusWO3mb&WEo50cD*)gsVx4L>-0-wRr;R`0ZB?marWa;3ed zL4}mur2j!(LqBeTj#|**6I$nA2ER|H=qFFo&^B zu>JrIezgQuBl?*Mn!oL91Zau=h^j>yNq0%_plv`U3X2lr%>z18wr68AH#5p)(fb5! z25ms{Njv+5xSu3x3>BRxHVpl1GajELO_gTIR^(zC+DdWEH$qKWJ8?Q0w&++181 zKg`Q7SvK?TYEh31xNQdv2zMOX6y^S;QxqD=BVUZxXPpKoW>9i(4p)`9<^99$TM`6V zaI#fywSX0acX*oKS=sI1_DMq8)y|Jd{XToys@?^}8{c(t%oyAMA`faM(ps7NQAhJ; zB*8g=>W&(iy^n8=w0se7hqiehs?Ah%Fh7amc@v{+kBaJeVQ@H4C@*2z$Rc5pPEL{A zzhKR6kq!Gj5gA>|*0@8q!1pHUsSQ1p{+jkWG7eVF^z2p3D;3W>00;@s0(3s*mn*74HZhC2f~0ct}2ac=KPTKHT@PG8boQ* z;DRts(r0jZ07Sxraw9%1H#KFFebk-AeJ)Le-s_G%zyklgMzL~IeS(I8gN|3pbC)RB zXmJ1*MkO>`caWHMn5jnxpyf^wD-E^&Kw)Cfe5MVY^{hJ<17^T>XW6P&=uZU+!}P8V zM)WWQsSdWFSfxZzOyE2blEkh}M7MFoUY#uN*W-oB#ZB)B-jrk^pfGf~h1FkujA9;GmuxML86eZ!*!UiZ*Z*c}UjL)aOfXv!m<*lf5z{Jc= z+mNRqZ2HiHONgj>7ihu^LR+U%b}I=X#a6k9R!(OuAr`Sz(vRhC(L)W}+Ny`z=Ue%P zhq2l*u%1bC`%*C#)t|@E^76#51%KG;9ijw&L zeQ08>`HW#07e-G8PQN>RPQPzX&BkQV{cVYaIDR*7*i0J?Y;G5@WF~rnB{;#OfzJK} zxl>)9q?qDI={C!|!D|aoX?s1Oel8+&+J|DFQ$ibrTNt3!gG%|2w3vEgvgS{_d_d1V zRK;7fcZxi;nuDEP*E_@*-)K98dpkYezFK?t3}2R{1itpynVj2t0`i#gP&?tJS9sx% zZK29;3HsH5cFnO^SVDoj_q+6(jyDq+Tl8C`i2Z-X<*aybznbri!9E8*QkV_B{^st3PD?8GB{+-@IXFoqja~>_UkgcvF!x7~!3i_GMO^o;~)n zj(yZEJ=YLdcl{4N0zDN$w}r0I^Zx8z$cpX@E?%7joYo=H+B|gcrWX zUoap*4vb;t99x_-!j41TE{_!ea%LpJf(ab~soY3t1K(!x9ihuy1CkGhf5Lu8@`8%U z%l$$tnwtx2ViBFR961NP*gFQje@Vo9g-Prq1xPXET|#e6fV2jo^(dHNX*DYsrNnv! zSn#f59XF?o%$eI=oQXEJ`+T`o{oGFSgZN9RNt@{Z#tgMU$ibII*T@)t3cj9dOR-#! zzvExioI7$Mo8dT_;Ad0JPK%T`0UeWTQ140knYt(dR+Jf$yErA&d*9(GOSJ~l0Aa1j z7ymiDEh+;sYmXk$o;Uwn#Hx{snVXrBF}Riewc9eV<(w2d{GAW1ocIDq@VGQ6$oa@&3Z+OO+QBVSY7^`M%(l_bL$Q8Z1Jmvv|612N znNr9MxwCpl+agTxL3BHx1((w{fZ=*UZ1a(}=VOV6P4}>?Bo+TUslB|B>^Z8p7_Pi1 zDTML%I&gWDma6>K4ag&ZQgorz5l>zw`+VTIq!j3Loxlcv(m`DN`5~gmsfr zS8c02g8Gij>OzIjROvOBTrI7p!1Al4plGbz0+Q(L;?YjeNlBT}|6{$53iHT^sG72L z$yo&N-;E4U_x&624G2c!PNII8yw;%&7(>?Y5I6_#NCL(8H84LUXrdrH8s($m)%m)p zQk4}j9hc-40qIR}y%GlrsDgP}djLt_!XpXr=r>gCoe)FJlKJhLYq~M-A zZjWYa8wG5BAqcYeek4k0(7IKuKC${qO~u#Z>>#jV>l;wx#@WD}MVh2M9~@Zzp^iQS4gMguy4P1k3LilHrlBEi=0@ij_)2Px#z3Az>}Jk zlE~lE580u79q%fDH%f6|jF2AhKCU z8Tlx8U-}J9*>Rw50?+B9&KVTL=FE`&3{LrR1`Mv)vig|F97Ld&v!4YsbK|D{aocb7 zr;F}=ck+M-6K|xqHsJIp*`ml1n&i2Bxq>H(mbC;w$#pm3=#N&Swwj*Ox^hn8B()*5 zb1~?#bFyr<(irNIMMFYSy2pAuS9;nTu}2Vd`V{391uf-K?1V~DNq&uW*+^4MSwn-Y zr(VLg)Z3?6iIbmtn!<~hSalAvP~nog4l_DlqZZtYPM66sUTYOxUr8*5ZDFKSjJvA| zuK(odv;agwEWa{Tfnl0}4VKQhiR4bqIrJLls8=WN6@DQiN%@fl;(AG-QA*cX25OVi zXv)>n%~;X_f}_ii>1wmHepU-%@HF*!UeqEp^gyG6b5P!me08Hg(K z^)odiGYm@()be0&n07)NdcPbCa{;c5^{#AkIyUgyQ7@W%R(9eqX}W@x%J0+sYUX9+ zI`b=ar^ujyP9Or+=e)I)_yrT^HXX!ue{i^MRIFd`UDloDDuld5mT$VCg`^IV06>rqi>V^L8j>XS7swnc?j zl}qKo))V?Nx*R&&4VA^g%jcRK&YVj1`?ZA>*fyq*)B*@}=s(aA%(HV2cFBD!9XyA)mr3dAJ$1cR@})F8DtU_;G9Tc@Bn7npCs$zm zZ?UTXdv=rMKjKsW#Wv*WmFZOx9H7;YCi>?3`iAo6`T)SD_^$>4fZ^;>fzc4$81R9? z!TbU8fJnjM0E8{TK>vq}|2H$||1Z>FVf|0wGOh8?VyV`d1CWl7PiaI={330+t%*|o zxd3V|l22Qmw^B?gh+pMTjF19?fYbt62|;`X)DRYCRc*L@fTU>?DAHM`Q{U3O+~W89 zDdW{O+x-Vs$ZvD!*DvRD_tZ3<-fU*-z2)_D=eCas8yf;Q_Oyr4KKQjG)wD{pTN9Va z&vd5BWufk^3_tjJL(-M-ll?G2UAT@p7G<*@$?F9YJP;b5&Z)k&jh%ZfjHY+OkLeDN z3_|w<{l@&1JEr@bG5izSAa#5N&?}W9s4r#@@h}V$gfNHLm(=$V@wY9@EzG-giB1@^ z5K4_?`Z?iVJjabAzO`0T~%WS4AedQ1-JhpU*kDOhxMpZOKD!@>X_$Mb}IVXX| zk%bMDHk~#${wirApg@r1L0%&7iAADIu|i&Q;9t*-`ms_@0>4I^VnGTOA<0&)ASMZ{ zqNEV<7O2bZONf@8~bk4BHxD#(oH@$10@3*aP=N;suNnKl7rd_}-OVN%Y&4%K+7g1`cZ z8|LhkVOA#FokhFo=FYJwERS;W~tbO^dZ@b^gjQdRh>NWMdp zLzad#_muT95cojh&qH7bEQWvTgV#i_@LiGmgOVUfjD^_|1%#32g<%k-%ECYh!^uf8 z!$=RL4p?i$SEnvdbQ-h_W;uer-Vu0JU)d4Xg~9DGYDa7vbPBh3ug%s&*K(cHZsn)k z4tl3l(9yvIp~kB`hx=l2UgbxPYsJ5JhP zVmw@>$I_^bfJaxN|HOKEXeVOrr^ww?u$UD5Sb4YGm(x29l#GsaB;Kgp_~1hG)PzwE ziIGk8ZK~M8F%POVZdyPWsh0`2G{~$~=rap+MnEwl3PaY^{KClXFa+O+*GO5=v!uFW z&e;a$v*KYyp-7j%4KgXv&AWTR;Wk=?gh{^ojQ*C+&gTuM`%tIP1^3J0e(|2IQcb#cyI1Y{glftJt zV3@wSnPbk3Rd8z)oJa6JscvC?(C;pI$>n1XJ-^iP%o;TneTN6gsZ(-bSQUgCQT1v3(&s+ z``T*f=tYqShMU`&ZAfW7)Eu!AIJukK*u@!5^mv=LwzU*&2s@buTAA);dhBZDtr^Z* z;!8ppo&4MlrPxR)>_dEk+I;ZrkA+Gr&@0T#M8)&o=cOq#LE?a?ZHOW}Ke>+y2qJg{BmHmD6AFG8{>1=|K(UC z4FC-o$tv`(K2t9&-<~^zxCszF5%0HkF8iqbf^%@NaS+N(kkE|rD1S^i=jLgYp!F2} zxA`)l7)NTYlq31?7cqGr*2C+pgnRG!sMc*r3Uq#|g^5Aj<;rH8!odajv|X&|FZ6(F zu>qPQW}S|&Z7UJ=A7&;p)|R{yno1~>17q3kwhU_`Sn^cJNQ8aR0DYw+2NbBVsqGA! zft~<#QW&{$k(K2_%=8Mn-pX!^0@&fHbJKa7et{9^I;3>C97blCGJ5^9*r_gFX!6hN zb@@HWOe1uki5l%@xZ;aHt)7u+J!_xO<)#RK+ivI+1twuwHozQ&bX25v6Q@|Ono;Gl zLUnqwfo%El-tr?{P4uzf`q^=TA`^(&$%5XY_bjgb?`HPb=;xo|o9Z8<_m1B8Q#S~4%&^ShGr&k45u;~{wOi4 z{2kr#{kYv8eQ9PUvXntDnuKCM9R6V8r zafcMe)~Qf$Jr>88!03eJa@PFzjNw?Hx_VBhXzm_aRcXNK@BI$W4ahZ{7BV`gOy>IS zKnc0xq`1Md4fAg5zJ2u)LNW*B)eg*(sNx@c*YPWI z%js;i8(#BRpaso0GP7nf-zc@(-F}|PF0At`>HdyFH@~<$b*O#mdRloqZ^%xl-Rjv1 zU$Eh$ntR5(=yQ=6J8QQtj&*O4U#r(=C|s>|wQ9ql=YD$0b*fgKuVY|zTdj+mIn@El zCCKr(?^VQ_6hFR^?uhA2G`X1$hdJwf7N);|@DnW^xmV-jNE+-_RG-*6(vIOdMIRG5 zPC5Cix8X!_oprasNNTeFSUW3*RRRf?H8;lbDz?RxyfnlxuK-+Qi!UQ1ZKZ`>m|rRg z-@XN?g8A z&RZ?Z8M14(*9TI!4cTYB`dF~QBpXy!^MKY^OShXpnqCe-p1$Sr0Z@K@mM&i zc+654ZPs(#YCom|LWxl3KI|ze>dicUs*GGlD9N_!mDNuq zN>s{`se;Omyo#QE1od|L8d2?<#xDO}(wa`Y)#r2MS%7So@WOHaoo$ER>1J&O76KwF zc(dD8I`hQkEBa#L`_oFn#E4>%>;{~-@o%LdV;gzZ81<$~z#SU?@q|bWflyXLT8ZS5 zXuG2W$fv>0q}*z;DQScghVoI*5wQv{-|Q_Hj>KhT(cz`#pUekE}I+mF2s z)KL)mage;dpfxz``=Qlnu1-4*Hv?jS1|PY?#&ru*niytv{rI+|UFh-Aw^is4r&Us| zh&m1<`s`;_qbcmv0sc}S;F#qgTsjJ|+Bh=?cmf>N-mID2-Pd|~Q&(s* zbhLMKq%BNoRrdu5^E(H4TpmKI9L&E^GSmHuYn}#9-rd@`cyy_&qik%et4?Qq;ik0f zBbsRJ%kVd^sajgs`B}zK|3e zD<}m}aWLj(*@4T5HFu7Bz8c{u)Lwrkl^7i#7~8D9 zpur!Ro>bKx{=A+cc>5QB!59hXmbW#Oc;Ct&fn;pYxCS;pGx-uO53AY(kHQ0{VWtCH@#j$V>okDUkhuxF?zL8A1K{7~H%=Zz<6H0zX_J=_}Al zUk3+U;#LMv2SPj%RT@zKE&)7M0XHQvI~uHM6m1nEikXZS9nWKOXYjqli{-ZFzw$6b z${pLaz+#gs-Jz34B9dSvUT#2XPCP1*v_1I!9PYz|*T<#?>}l#TGH9wF0b~!e>@yl;&ol}nf+C1&*)FC z4AR*8whAG1g3}UFWsIH=|7dEWCe~?V;9*#0AfY2rqg7NkOSgTGFPjjAGEeo#f6ArqsFh7zxiEbl7H27j9%D1Jv`xyj?6yFeMLyhtd0b* zlf~+N7EG+6FVDJ+<@Lhh?h|nSMVQ0?z*I>|M$QoKY_vQyyo0-nk*a^poOQMqabMxw}w}K=E`bDUnu75*jp|V%i7&DX_>-8#&+* zZ=_<9BATF9Wozk!vkzSey;`4W^TH+B0v-ll>VC0)7zAR3%psmkhr>W~-C9p{Pv}#X zC*3C-XBJ)Q7g6O1ATvnNuj_+Zf6qvF8^F8q3lRe0^ zPXI?-$K9_7N)BfV)vV5YTs~%x%Qd7YZVJ1us% zMMqW*B@7O-Bo?^7G?Q{|caT#|(6n*edqxKn$UH~^O zW8G}rG|0NMQ&&gR2vSXYG=fq~!b4@bw)?zMGIG`ilw>_3YYpd0>(JaFtOF8FuURzdtxbtm!cWu}(JrL9eH$zOV25SA$^Wd*Z%AYk$`sTDIBuxzA+3Pv6*Qw;lOIpkvCiu^hLn zkNm97`Mp2gNYx!rh4pdCY|vaHiBr2+bbSfQ%%R9f=u-Gq^9R9 zQS84?gISSyt8rt0dmp(@tx9+GZwCv0$XM9a6y%yn-1q=qJvwG4U=;Xz1Tp`_rt%uA z9pKGXFtaYBJW=F`gt&0ogr9cW$UHeaSDDL8?|)B`_IzB77;7s`g>>NEJ8NwRR^7x zkZ?o|6d}8Wr5vw+8Dtlkz6y^1+0^W)!pI282rCiT@Mb8LE>g#v*ILJ|v-;UvZ>G2f zrzu;r4*`SldU+1(B?W^-efe-By&CLTXyl!5*_Y<{mAPel!)jKA}XAMmSWg;?E6%aS^35U3S zn5G3DqR+GNq>&Xdc%{YNQzk?hdu!QHBZ?0hiK7K!c7f9x>7i7QtZ;meLg`V6mg5 zWW%615H}ruKwFbg7HQMA9_Ag+Or4ukTsHoL*rY~#0SGJ7{-bp`u4yC3n0{R+WMAl` z`q(#t5MS|j=?HSKqq4I+r|`1$I3=1lI+vF|P;x@Q@GW{22jp7_hNXue-Z+J`&wa97 z1B#Wac+`&ox?FdJa3yNFi7M_xzcOj%kx`2#$7uHFbBBa_8Ywyoq!ldbmWYj+6qAB` z^(l5ssU{@hAD5cuJ#OkXegZh|%L21T@geW=L9Z!nP8` z%up{P8su~EE8B=mg|!3xp*u9!??x6HKf&v=a<3qPao1n8!Y#P`c|)B5iR z(yNr+7k?ibihq8jQgvQBJq{m|vCQ9t^6u`k1@^&XaJcOE*5-lt1 zMR2|GhiLIkkxLmcJmH{p5g~0dH?4SlZ{9szIyRI-%|gk$lm1+xpL zA&rl%Eb9zRDdl0|l#s~s>~nuC6fU9jJ)DcbYQnWEpxAJN{n=*7c9EU4%(&-8@OYJ% zJ@^kaC^zEUh*b9k6H5;$sK1X#nJD1!IjGG=E|P`I6KzF24)MHPz}%Tp7%=F`H~V+Qu&P_%7HE{OVPCF;9GX`C2+b5C6wYV1y3C~D5zr1W%=)`aWqDUMq|;jHqNX8kB4#CveUfY|l-uB2DhJH4HGqv#^1VX$#TO%o-Di-{no}`#&fvXsBHs z&dVn0s2P^fU$U%r4;eG6@qY0Je!+B+yA1vZ_Wa-C{r`9H&HNuR{{I5s8Ps+dJs+0a z#WuSg*y!lk=)&mu{K0@+`k;ma%nYIcTjt;e|L4^h{@b?1f7O-I{Tp9Va<-w9#b;st zS6t@kK|HR$R8d`E#<4C@sYh4aDZ-0YK}-qg?R7 zxMSEDSnEQpU!RXEZdkF=WMt%IdHEfk6_+b2N;-i?SsJA6W{UKGm=iNN`qQ1K3`*D^ zXPAmL00cQuwe>aW`-=@UfjnoJ%>={xiolrz#sSVql=2b<89+J9>vaG*2a^m09VU}N zLF9scCcsMS zK<2PWqep$Gx`TP=5DbaO0*5h~59(GCVCZoow8~(*tBueF#Lyv|8CtM)|Jv%;288|!@$;d_*w6n~2*jD)vBywZp)hHn=u*W1!}knu9lcbw;)PYz!u z4T*8)UHnjH(s40s>qgdKJAR)p5iG18$r4(=NjXAF6>^T_i`JsG&uc?ts4#O}Zp^0+ zl~B&#h@P7*nV~B%|uqmL%*cXr!9Hj2gpC0(0Rkj=>VU#`It$8~+ zYc~}fT-}wmmv%P}-dZ}!;gQzl_S&>l+l@PA&SoFA%X^Q0v~$R&Z5C-ofqAjR3^e2u zhSQ@EqgXIRY~&~R`D@FLoZ-4>nAXKh{(Vut?>?FCh=52CxcTTWevTJ;Gbm=7 zU%t`mn)sNb4A|Cvn^(Kq@Hs*LeYM2XL~pLdOCPmXAH=G4opzgBxR%_GRU5e;VEJ~Z zbsbLN!;V6e?aa$)^K53hF7kEJ(bIFPAvt9^-2$6oIFdDrde#J3sg2~B%SE#@MQc1j zFc5LjoIanfSd9M)=Pwp)OMZ9%8dKJGREVt&r@(FPSLZeHX07d2EY^X%=RtK>&}*j_ z7YGwKx+Z|AMGFnbA%iY-F%@dN_;bTKFsQmO*jFJ*oEk5G*~pc6+~x}ae~gVBZ`@tr zptukME8?6>=vy4N&!Y=)Xw~g(cS^mV%}LzvAVUmVBP7;u-WS4HB9}g=Ao_$O!FxOD z5?myQTVPaAtXyjJFFOfBfTN{DqOHjMh+X?gJZP<_y^c$ifYylEuv^Fgt*q#Yq2~f; zS;@f46QGl;KaW_|0?mpVz!p@cQIQC46t`HfyU>{b_g0DD6?u*)rPEtFp9uZ&(o#tL z>`#H`VF4M5+WkCUUbyIf?8>&fe<?BylFcE-r7+Ozka%~x|fmfo(fksmy< z*za-ij~BairvnRL)rP128{K&Eu^{ArnTyx>wq4dQt8{?u?Zx>H3gT&j?^2UV(;m)E z$#AHMkLRvz1_ew+E0!fhxOIC5Jo|drlg!hAK(MHZ>z`UiU*jgKWQ_b-%4Qr7EUlV_ zyLiNkCdZ@sPR)y-EQ)FziYiY0K-z3+CMKwf)EN<6-KZ#);#KBA*_<54ur{K#_?9jh z-=bz(P&Oh#sG9@aej#K39!5)&rq9g+1X+W$5CKOJhb#N}5S*4;W%I_uIM)vCPk*hL&x-&;R;ERR=N zw0FJvMq%fWUw-DR=DL=5YOgRTc`ke^ciZBXty9CeoX- z0<0W!jnBNEk|&sElj_hS`QAuQ(t*pXqWGnLW=x4hs%J}B$_gVBL9M5YW{ae#%($~G zb<$GN^9JWjFS*WE^|4ZJH1;VurY0}(-ssHd1x7xfXKej&a`UEdpAY_Q-~7Gb@p$=! z1>YxU{o@u2&sSOZcjl3;dn>a`UOdySIx8C^H{(Lnr?78N*1q+P{5*Hc+plrz*=M7! z?aI40D?ll3?*a)&AJ1=0s{=v_#C^Aaa^`a z?f7oA%30FsR8X405+8-XCLbGmB!04J9^$-Zv)z-u>qExo^V^g+trc!ibDHvj#Z#?h zv4?v1Jkgic&#T$x{jJi!cE6e&&tH7Jew7{P-RSw(_D#$3|EejW_CR8l|EvFT^S4=V z5PrE(`lVlqvg~xOE=DFJ#>oyxE}CbtoGiLg8zfhe`+4SWeX_EU^{4TxdnH*%!{Q(J*0yogE`7fmy;LNI2E^~AJaNwPNMhZr;_>Ra$z12+hytf~dqe$z+>HtRRrGoMuODqkjpP%M030i5iS?6?QN~}e@@r#2JENb0n zSGX_TpLngV#xJD))2^&23F|}yr~_E{GvA+ znL-l9l{5~iy*a2k=YXAz125k~KKBA1-V7)A=I0+yC;r$nY5jp0x8-kM-|?h$VdmBB z6)$&Rci7CoKKrcldTrhKy?I;ze3sIz<(yu(?#w?%@gDIRnDJ3ul2}wyQIwj-Wo&9- NXu+kb>gw;t1pw*mAdvt7 literal 0 HcmV?d00001 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 a655108..bdd3d18 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 @@ -104,12 +104,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( robot_nav_2d_msgs::Twist2D twist; traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom - robot::Rate r(50); while (robot::ok() && traj_->hasMoreTwists()) { twist = traj_->nextTwist(); - // ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta); - r.sleep(); } drive_cmd.x = sqrt(twist.x * twist.x); @@ -120,105 +117,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( return result; } - // Dynamically adjust look ahead distance based on the speed - double lookahead_dist = this->getLookAheadDistance(twist); - // Return false if the transformed global plan is empty - robot_geometry_msgs::Pose2D front = transform_plan_.poses.size() > 3 ? (*(transform_plan_.poses.begin() + 1)).pose : transform_plan_.poses.front().pose; - lookahead_dist += fabs(front.y); - // Get lookahead point and publish for visualization - auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); - - // Carrot distance squared - const double carrot_dist2 = - (carrot_pose.pose.x * carrot_pose.pose.x) + - (carrot_pose.pose.y * carrot_pose.pose.y); - - // Find curvature of circle (k = 1 / R) - double curvature = 0.0; - if (carrot_dist2 > 2e-2) - { - curvature = 2.0 * carrot_pose.pose.y / carrot_dist2; - } - - // Constrain linear velocity - this->applyConstraints( - 0.0, lookahead_dist, curvature, twist, - this->costAtPose(pose.pose.x, pose.pose.y), - drive_cmd.x, sign_x); - - - if (std::hypot(carrot_pose.pose.x, carrot_pose.pose.y) > min_journey_squared_) - { - 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; - drive_cmd.theta = atan2(dy, dx); - - //[-π/2, π/2] - if (drive_cmd.theta > M_PI / 2) - drive_cmd.theta -= M_PI; - else if (drive_cmd.theta < -M_PI / 2) - drive_cmd.theta += M_PI; - - drive_cmd.theta = std::clamp(drive_cmd.theta, -min_vel_theta_, min_vel_theta_); - } - else - drive_cmd.theta = 0.0; - - // populate and return message - if (fabs(carrot_pose.pose.x) > 1e-9 || carrot_pose.pose.y > 1e-9) - drive_cmd.x *= fabs(cos(std::atan2(carrot_pose.pose.y, carrot_pose.pose.x))); - - double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; - const double vel_x_reduce = std::min(fabs(v_max), 0.3); - double journey_plan = compute_plan_.poses.empty() ? 0 : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1); - 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); - journey_plan += max_path_distance_; - drive_cmd.x = journey_plan >= d_reduce ? drive_cmd.x : (drive_cmd.x - v_min) * decel_factor + v_min; - - // drive_cmd.x = journey_plan > d_reduce ? drive_cmd.x : (drive_cmd.x - v_min) * sin((journey_plan / d_reduce) * (M_PI / 2)) + v_min; - - 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); - - if (drive_cmd.x != v_max) - { - double min = drive_cmd.x < v_max ? drive_cmd.x : v_max; - double max = drive_cmd.x > v_max ? drive_cmd.x : v_max; - drive_cmd.x = std::clamp(kf_->state()[0], min, max); - } - else - { - drive_cmd.x = std::clamp(kf_->state()[0], v_max, 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 = kf_->state()[3]; - result.velocity = drive_cmd; return result; } 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 fd47170..96046fc 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 @@ -1,4 +1,4 @@ -#include +#include #include mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {} @@ -77,7 +77,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize( x0 << 0, 0, 0, 0, 0, 0; kf_->init(robot::Time::now().toSec(), x0); - + 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); x_direction_ = y_direction_ = theta_direction_ = 0; this->initialized_ = true; @@ -104,12 +107,19 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams() 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); + // Final heading alignment (Arc Motion) + nh_priv_.param("use_final_heading_alignment", use_final_heading_alignment_, true); + nh_priv_.param("final_heading_xy_tolerance", final_heading_xy_tolerance_, 0.15); + nh_priv_.param("final_heading_angle_tolerance", final_heading_angle_tolerance_, 0.05); + nh_priv_.param("final_heading_min_velocity", final_heading_min_velocity_, 0.05); + nh_priv_.param("final_heading_kp_angular", final_heading_kp_angular_, 1.5); + nh_priv_.param("rot_stopped_velocity", rot_stopped_velocity_, 0.01); nh_priv_.param("trans_stopped_velocity", trans_stopped_velocity_, 0.01); if (trans_stopped_velocity_ < 0.02) trans_stopped_velocity_ = 0.5 * min_approach_linear_velocity_; if (trans_stopped_velocity_ > min_approach_linear_velocity_) - min_approach_linear_velocity_ = trans_stopped_velocity_ * 2.0; + trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.01; // Regulated linear velocity scaling nh_priv_.param("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false); @@ -302,7 +312,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs: 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) + if ((unsigned int)(compute_plan_.poses.size() > 1) && + journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1) >= costmap_robot_->getCostmap()->getResolution()) { const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose; int index; @@ -320,6 +331,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs: if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) x_direction = dir_path > 0 ? FORWARD : BACKWARD; } + } else { @@ -403,14 +415,28 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( prevous_drive_cmd_ = drive_cmd; return result; } - 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); + { + geometry_msgs::PoseStamped lookahead_point; + lookahead_point.header.stamp = ros::Time::now(); + lookahead_point.header.frame_id = carrot_pose_stamped.header.frame_id; + lookahead_point.pose.position.x = carrot_pose_stamped.pose.position.x; + lookahead_point.pose.position.y = carrot_pose_stamped.pose.position.y; + lookahead_point.pose.position.z = carrot_pose_stamped.pose.position.z; + lookahead_point.pose.orientation.x = carrot_pose_stamped.pose.orientation.x; + lookahead_point.pose.orientation.y = carrot_pose_stamped.pose.orientation.y; + lookahead_point.pose.orientation.z = carrot_pose_stamped.pose.orientation.z; + lookahead_point.pose.orientation.w = carrot_pose_stamped.pose.orientation.w; + lookahead_point_pub_.publish(lookahead_point); + } + 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 path_distance_to_rotate = journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1); allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; double angle_to_heading; @@ -428,18 +454,27 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( } else { - double v_target = this->adjustSpeedWithHermiteTrajectory(velocity, transformed_plan, drive_cmd.x, sign_x); - robot_nav_2d_msgs::Twist2D drive_target; - drive_target.x = v_target; - transformed_plan = this->generateHermiteTrajectory(transformed_plan.poses.back()); 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); - - const double journey_plan = transformed_plan.poses.empty() ? distance_allow_rotate : journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1); - this->computePurePursuit( + + // === Final Heading Alignment Check === + double xy_error = 0.0, heading_error = 0.0; + if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error)) + { + // 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); + } + else + { + // Normal Pure Pursuit + this->computePurePursuit( carrot_pose, drive_target, velocity, @@ -448,24 +483,43 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( sign_x, 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_); - const double vel_x_reduce = 0.3; - const double scale = fabs(velocity.x) * lookahead_time_ * 0.9; - const double min_S = min_lookahead_dist_ + max_path_distance_, max_S = max_lookahead_dist_ + max_path_distance_; - 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 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 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 > d_reduce ? drive_cmd.x : vel_reduce; - robot::log_info("journey_plan: %f, max_path_distance_: %f, d_reduce: %f, vel_reduce: %f", journey_plan, max_path_distance_, d_reduce, vel_reduce); + 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_); if (this->nav_stop_) { @@ -504,70 +558,18 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( result.poses.reserve(transformed_plan.poses.size()); for (const auto &pose_stamped : transformed_plan.poses) { + result.poses.push_back(pose_stamped.pose); if(fabs(pose_stamped.pose.x - carrot_pose.pose.x) < 1e-6 && fabs(pose_stamped.pose.y - carrot_pose.pose.y) < 1e-6 && fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6) - break; - result.poses.push_back(pose_stamped.pose); + break; } + result.velocity = drive_cmd; prevous_drive_cmd_ = drive_cmd; return result; } -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 (journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_) - return min_speed_xy_ * sign_x; - - // 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; - } - 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 robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &drive_target, @@ -580,12 +582,17 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( { // 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; + const double L2_min = 0.05; // m^2, chỉnh theo nhu cầu (0.02–0.1) + const double L2_safe = std::max(L2, L2_min); + const double L = std::sqrt(L2_safe); + + const double kappa = 2.0 * carrot_pose.pose.y / L2_safe; // 3) Adjust speed using Hermite trajectory curvature + remaining distance double v_target = adjustSpeedWithHermiteTrajectory(velocity, trajectory, drive_target.x, sign_x); + const double L_min = 0.1; // m, chỉnh theo nhu cầu + double scale_close = std::clamp(L / L_min, 0.0, 1.0); + v_target *= scale_close; const double y_abs = std::fabs(carrot_pose.pose.y); const double y_soft = 0.1; @@ -606,7 +613,25 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( // 5) Angular speed from curvature double w_target = v_target * kappa; - w_target = std::clamp(w_target, -max_vel_theta_, max_vel_theta_); + if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_) + { + 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); + + // robot heading in base_link = 0, so error = heading_ref + double w_heading = k_heading * angles::normalize_angle(heading_ref); + + w_target += w_heading; + } + else + { + w_target = 0.0; + } + } + w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta)); // 6) Apply acceleration limits (linear + angular) const double dv = std::clamp(v_target - velocity.x, -acc_lim_x_ * dt, acc_lim_x_ * dt); @@ -616,151 +641,42 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( 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) + 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; - 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); + 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; + double path_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); + // 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; 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); + : 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; + + // 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); + // else if(fabs(velocity.x) < min_speed_xy_) + // { + // ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta); + // ROS_INFO_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); + // } return result; } @@ -771,6 +687,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an 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_; 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); @@ -779,8 +696,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an 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_; + 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; @@ -810,14 +727,161 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an // === 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_); + ? 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; } +bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading( + const robot_nav_2d_msgs::Path2D &trajectory, + const robot_nav_2d_msgs::Pose2DStamped &goal, + const robot_nav_2d_msgs::Twist2D &velocity, + double &xy_error, + double &heading_error) +{ + if (!use_final_heading_alignment_) + return false; + + if (trajectory.poses.empty()) + return false; + + // Calculate distance to goal (last point in trajectory is closest to goal in robot frame) + const auto &last_pose = trajectory.poses.back().pose; + xy_error = std::hypot(last_pose.x, last_pose.y); + + // 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 = angles::normalize_angle(goal_robot_frame.pose.theta); + } + else + { + // Fallback: use last pose heading in trajectory + heading_error = angles::normalize_angle(last_pose.theta); + } + + // Check if we're close enough to goal position to start final heading alignment + double distance_threshold = final_heading_xy_tolerance_; + + // Dynamic threshold: increase when moving fast to allow smooth transition + double velocity_factor = std::max(0.0, std::fabs(velocity.x) * 0.5); + distance_threshold += velocity_factor; + + bool near_goal = xy_error < distance_threshold; + bool heading_not_reached = std::fabs(heading_error) > final_heading_angle_tolerance_; + + return near_goal && heading_not_reached; +} + +void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading( + const double &xy_error, + const double &heading_error, + const robot_nav_2d_msgs::Twist2D &velocity, + const double &sign_x, + const double &dt, + robot_nav_2d_msgs::Twist2D &cmd_vel) +{ + // ============================================================ + // Arc Motion Controller for Final Heading Alignment + // ============================================================ + // + // Strategy: + // 1. Linear velocity: proportional to xy_error, decreasing as we approach goal + // 2. Angular velocity: proportional to heading_error with feedforward + // 3. Coordinate both to arrive at goal with correct heading + // + // Key insight: For arc motion, the relationship is: + // arc_length = radius * angle + // v = omega * radius + // + // We want to cover xy_error distance while rotating heading_error angle + // So ideal radius = xy_error / |heading_error| (when heading_error != 0) + // ============================================================ + + const double abs_heading_error = std::fabs(heading_error); + const double heading_sign = (heading_error > 0) ? 1.0 : -1.0; + + // --- Linear velocity calculation --- + // Base velocity proportional to distance, with minimum for smooth motion + double v_base = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error); + v_base = std::max(v_base, final_heading_min_velocity_); + v_base = std::min(v_base, min_speed_xy_); + + // Scale down when heading error is large (prioritize rotation) + double heading_scale = 1.0; + if (abs_heading_error > 0.1) // ~5.7 degrees + { + // Reduce linear velocity when heading error is significant + heading_scale = std::clamp(0.3 / abs_heading_error, 0.3, 1.0); + } + + double v_target = v_base * heading_scale * sign_x; + + // --- Angular velocity calculation --- + double omega_target = 0.0; + + if (abs_heading_error > final_heading_angle_tolerance_) + { + // Method 1: Proportional control + double omega_p = final_heading_kp_angular_ * heading_error; + + // Method 2: Arc-based feedforward (coordinate linear and angular motion) + double omega_arc = 0.0; + if (xy_error > 0.01 && abs_heading_error > 0.01) + { + // 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; + } + + // Blend proportional and arc-based control + // Use more arc-based when far, more proportional when close + 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; + + // 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_) + { + omega_target = heading_sign * omega_min; + } + } + + // --- Apply acceleration limits --- + // Linear + double v_current = velocity.x; + double dv_max = acc_lim_x_ * dt; + double dv = std::clamp(v_target - v_current, -dv_max, dv_max); + cmd_vel.x = v_current + dv; + + // Angular + double omega_current = velocity.theta; + double domega_max = max_vel_theta_; + double domega = std::clamp(omega_target - omega_current, -domega_max, domega_max); + cmd_vel.theta = omega_current + domega; + + // --- Apply velocity limits --- + cmd_vel.x = std::clamp(cmd_vel.x, -min_speed_xy_, min_speed_xy_); + cmd_vel.theta = std::clamp(cmd_vel.theta, -max_vel_theta_, max_vel_theta_); + + // --- Safety: ensure we can stop --- + double max_v_to_stop = std::sqrt(2.0 * std::fabs(decel_lim_x_) * xy_error); + if (std::fabs(cmd_vel.x) > max_v_to_stop) + { + cmd_vel.x = std::copysign(max_v_to_stop, cmd_vel.x); + } + + cmd_vel.y = 0.0; + + ROS_DEBUG_THROTTLE(0.2, "[FinalHeading] xy=%.3f, heading=%.3f, v=%.3f, omega=%.3f", + xy_error, heading_error, cmd_vel.x, cmd_vel.theta); +} + 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 @@ -832,67 +896,109 @@ double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const rob 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; +// } + 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; - } + if (global_plan.poses.size() < 2) + return std::prev(global_plan.poses.end()); 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); + // Góc đầu và ngưỡng gãy (fixed) + const auto &p0 = global_plan.poses[0].pose; + const auto &p1 = global_plan.poses[1].pose; + double start_angle = atan2(p1.y - p0.y, p1.x - p0.x); + double turn_threshold = M_PI_2 * 0.6; // ~54deg, chỉnh theo nhu cầu - // 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) + // Dừng nếu gãy góc + for (unsigned int i = 1; i < global_plan.poses.size(); ++i) { - 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()); + const auto &a = global_plan.poses[i - 1].pose; + const auto &b = global_plan.poses[i].pose; + double current_angle = atan2(b.y - a.y, b.x - a.x); + double delta = angles::normalize_angle(current_angle - start_angle); + goal_index = i; + if (fabs(delta) >= turn_threshold) + break; } + // Chọn điểm lookahead theo khoảng cách + 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 (goal_pose_it == global_plan.poses.end()) + goal_pose_it = std::prev(global_plan.poses.end()); + return goal_pose_it; } @@ -942,19 +1048,112 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf return true; } -robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose) +double mkt_algorithm::diff::PredictiveTrajectory::calculateMaxKappa(const robot_nav_2d_msgs::Path2D &path, const double preview_distance) +{ + double traveled = 0.0; + double max_kappa = 0.0; + for (size_t i = 1; i + 1 < path.poses.size(); ++i) + { + const auto &p0 = path.poses[i - 1].pose; + const auto &p1 = path.poses[i].pose; + const auto &p2 = path.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_distance) + break; + } + return max_kappa; +} + +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 (journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_) + return min_speed_xy_ * sign_x; + + double preview_distance = std::clamp(std::fabs(velocity.x) * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_); + + double max_kappa = calculateMaxKappa(trajectory, preview_distance); + 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 (fabs(decel_lim_x_) > 1e-6) + { + const double remaining = journey(trajectory.poses, 0, trajectory.poses.size() - 1); + const double v_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, remaining)); + v_limit = std::min(v_limit, v_stop); + } + + return std::copysign(v_limit, sign_x); +} + +robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTrajectory( + const robot_nav_2d_msgs::Path2D &path, + const robot_nav_2d_msgs::Twist2D &drive_target, + const robot_nav_2d_msgs::Twist2D &velocity, + const double &sign_x, + robot_nav_2d_msgs::Twist2D &drive_cmd) +{ + if (path.poses.empty()) + { + drive_cmd.x = 0.0; + drive_cmd.theta = 0.0; + return robot_nav_2d_msgs::Path2D(); + } + double max_kappa = calculateMaxKappa(path); + const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05)); + drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x); + drive_cmd.theta = max_vel_theta_; + if (max_kappa <= straight_threshold) + { + if(journey(path.poses, 0, path.poses.size() - 1) <= min_journey_squared_) + { + drive_cmd.theta = 0.05; + // return path; + } + return generateHermiteTrajectory(path.poses.back(), sign_x); + + } + else + { + if(fabs(drive_cmd.x) < min_speed_xy_) + drive_cmd.x = std::copysign(min_speed_xy_, sign_x); + return generateHermiteQuadraticTrajectory(path.poses.back(), sign_x); + } +} + +robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory( + const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x) { 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); + const double L = std::hypot(x, 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; @@ -964,12 +1163,13 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer 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); - + const double Lnegative = sign_x < 0 ? -L : L; for (int i = 0; i <= samples; ++i) { double t = static_cast(i) / samples; double t2 = t * t; @@ -981,8 +1181,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer 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)); + double px = h10 * Lnegative + h01 * x + h11 * Lnegative * std::cos(theta); + double py = h01 * y + h11 * Lnegative * std::sin(theta); // First derivatives for heading double dh00 = 6 * t2 - 6 * t; @@ -990,15 +1190,15 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer 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 dx = dh10 * Lnegative + dh01 * x + dh11 * Lnegative * std::cos(theta); + double dy = dh01 * y + dh11 * Lnegative * 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.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading; pose.header.stamp = hermite_trajectory.header.stamp; pose.header.frame_id = hermite_trajectory.header.frame_id; hermite_trajectory.poses.push_back(pose); @@ -1006,6 +1206,70 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer return hermite_trajectory; } +robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteQuadraticTrajectory( + const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x) +{ + robot_nav_2d_msgs::Path2D trajectory; + trajectory.poses.clear(); + trajectory.header.stamp = pose.header.stamp; + trajectory.header.frame_id = pose.header.frame_id; + + const double x = pose.pose.x; + const double y = pose.pose.y; + const double theta = sign_x < 0 ? angles::normalize_angle(pose.pose.theta + M_PI) : pose.pose.theta; + const double L = std::hypot(x, y); + if (L < 1e-6) + { + 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 = trajectory.header.stamp; + pose_stamped.header.frame_id = trajectory.header.frame_id; + trajectory.poses.push_back(pose_stamped); + return trajectory; + } + + int samples = L/costmap_robot_->getCostmap()->getResolution(); + if (samples < 1) { + samples = 1; + } + trajectory.poses.reserve(samples + 1); + + // Quadratic Hermite with end tangent aligned to carrot_pose heading: + // p(0) = 0, p(1) = goal, p'(1) = v1 + const double v1x = L * std::cos(theta); + const double v1y = L * std::sin(theta); + const double ax = v1x - x; + const double ay = v1y - y; + const double bx = 2.0 * x - v1x; + const double by = 2.0 * y - v1y; + + trajectory.poses.reserve(samples + 1); + for (int i = 0; i <= samples; ++i) + { + double t = static_cast(i) / samples; + double t2 = t * t; + + double px = ax * t2 + bx * t; + double py = ay * t2 + by * t; + + double dx = 2.0 * ax * t + bx; + double dy = 2.0 * ay * t + by; + double heading = std::atan2(dy, dx); + + robot_nav_2d_msgs::Pose2DStamped pose_out; + pose_out.pose.x = px; + pose_out.pose.y = py; + pose_out.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading; + pose_out.header.stamp = trajectory.header.stamp; + pose_out.header.frame_id = trajectory.header.frame_id; + trajectory.poses.push_back(pose_out); + } + + return 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, @@ -1182,7 +1446,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_na } std::vector -mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector footprint, double resolution) +mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(const robot_geometry_msgs::Pose2D &pose, const std::vector &footprint, const double &resolution) { // Dịch sang tọa độ tuyệt đối @@ -1235,34 +1499,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, co 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(); diff --git a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp index 5af06df..f11251d 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp @@ -194,7 +194,6 @@ namespace mkt_plugins void KinematicParameters::setMaxTheta(double value) { max_vel_theta_ = fabs(value) <= fabs(original_max_vel_theta_) + EPSILON? value : original_max_vel_theta_; - // ROS_WARN_THROTTLE(10, "vtheta %f %f %f", value, original_max_vel_theta_, max_vel_theta_); } void KinematicParameters::setAccTheta(double value) diff --git a/src/Algorithms/Packages/global_planners/custom_planner b/src/Algorithms/Packages/global_planners/custom_planner index 49afcce..e0f6738 160000 --- a/src/Algorithms/Packages/global_planners/custom_planner +++ b/src/Algorithms/Packages/global_planners/custom_planner @@ -1 +1 @@ -Subproject commit 49afcce5b2247793282fe5e94d0e27f062562325 +Subproject commit e0f6738c3161b7a4a9418d960d7e419a43f7d521 diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index f4725da..f261959 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -221,7 +221,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) try { robot::PluginLoaderHelper loader; - std::string path_file_so = loader.findLibraryPath("CustomPlanner"); + std::string path_file_so = loader.findLibraryPath(global_planner); planner_loader_ = boost::dll::import_alias( path_file_so, global_planner, boost::dll::load_mode::append_decorations); @@ -257,7 +257,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) try { robot::PluginLoaderHelper loader; - std::string path_file_so = loader.findLibraryPath("LocalPlannerAdapter"); + std::string path_file_so = loader.findLibraryPath(local_planner); controller_loader_ = boost::dll::import_alias( path_file_so, local_planner, boost::dll::load_mode::append_decorations);