From 1fa6af01fdd0114536e4d30d5fe9c7a04a0a45f5 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Wed, 28 Jan 2026 17:43:50 +0700 Subject: [PATCH] done thuat toan --- config/pnkx_local_planner_params.yaml | 18 +- .../score_algorithm/src/score_algorithm.cpp | 2 +- .../Libraries/mkt_algorithm/CMakeLists.txt | 3 +- .../diff/diff_predictive_trajectory.h | 48 +- .../diff/diff_predictive_trajectory_.h | 314 ---- .../mkt_algorithm/diff/diff_rotate_to_goal.h | 6 - .../include/mkt_algorithm/diff/pure_pursuit.h | 52 - .../src/diff/diff_go_straight.cpp | 5 +- .../src/diff/diff_predictive_trajectory.cpp | 678 +++++---- .../src/diff/diff_predictive_trajectory_.cpp | 1329 ----------------- .../src/diff/diff_rotate_to_goal.cpp | 74 +- .../mkt_algorithm/src/diff/pure_pursuit.cpp | 183 --- .../src/pnkx_local_planner.cpp | 26 +- 13 files changed, 435 insertions(+), 2303 deletions(-) delete mode 100644 src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory_.h delete mode 100644 src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h delete mode 100644 src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory_.cpp delete mode 100644 src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index c6fcdd7..3a861f4 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -1,9 +1,9 @@ +yaw_goal_tolerance: 0.017 +xy_goal_tolerance: 0.03 +min_approach_linear_velocity: 0.06 + LocalPlannerAdapter: library_path: liblocal_planner_adapter - yaw_goal_tolerance: 0.017 - xy_goal_tolerance: 0.03 - min_approach_linear_velocity: 0.06 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) - PNKXLocalPlanner: # Algorithm library_path: libpnkx_local_planner @@ -76,7 +76,7 @@ MKTAlgorithmDiffPredictiveTrajectory: library_path: libmkt_algorithm_diff avoid_obstacles: false xy_local_goal_tolerance: 0.01 - angle_threshold: 0.4 + angle_threshold: 0.6 index_samples: 60 follow_step_path: true @@ -85,9 +85,9 @@ MKTAlgorithmDiffPredictiveTrajectory: # only when false: lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) # only when true: - min_lookahead_dist: 0.4 # The minimum lookahead distance (m) threshold. (default: 0.3) + min_lookahead_dist: 0.5 # 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: 2.0 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + lookahead_time: 1.9 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2) max_lateral_accel: 2.0 # Max lateral accel for speed reduction on curves (m/s^2) @@ -99,8 +99,8 @@ MKTAlgorithmDiffPredictiveTrajectory: angular_decel_zone: 0.1 # stoped - rot_stopped_velocity: 0.1 - trans_stopped_velocity: 0.1 + rot_stopped_velocity: 0.05 + trans_stopped_velocity: 0.03 # 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) diff --git a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp index 8e0a7b5..62e6851 100644 --- a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp +++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp @@ -255,7 +255,7 @@ 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; + // std::cout << "ScoreAlgorithm: Invalid sub_goal_index, setting to " << sub_goal_index << std::endl; } else { diff --git a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt index 33fb380..1f54a64 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt @@ -81,10 +81,9 @@ endif() # Libraries # ======================================================== add_library(${PROJECT_NAME}_diff SHARED - src/diff/diff_predictive_trajectory_.cpp + src/diff/diff_predictive_trajectory.cpp src/diff/diff_rotate_to_goal.cpp src/diff/diff_go_straight.cpp - # src/diff/pure_pursuit.cpp ) if(BUILDING_WITH_CATKIN) 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 026366f..f4f8ac8 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 @@ -113,13 +113,6 @@ namespace mkt_algorithm std::vector::iterator getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan); - /** - * @brief Create carrot message - * @param carrot_pose - * @return carrot message - */ - robot_geometry_msgs::PointStamped createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose); - /** * @brief Prune global plan * @param tf @@ -151,6 +144,8 @@ 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 Should rotate to path * @param carrot_pose @@ -199,7 +194,24 @@ namespace mkt_algorithm 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, double &sign_x); + 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, + const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Path2D &trajectory, + const double &min_approach_linear_velocity, + const double &sign_x, + 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); @@ -211,21 +223,11 @@ namespace mkt_algorithm */ double costAtPose(const double &x, const double &y); - void updateCostAtOffset( - TFListenerPtr tf, const std::string &robot_base_frame, const robot_nav_2d_msgs::Pose2DStamped &base_pose, - double x_offset, double y_offset, double &cost_left, double &cost_right); - double computeDecelerationFactor(double remaining_distance, double decel_distance); double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan); - bool detectWobbleByCarrotAngle(std::vector& angle_history, double current_angle, - double amplitude_threshold = 0.3, size_t window_size = 20); - - void publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose); - - std::vector angle_history_; - size_t window_size_; + double estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan); bool initialized_; bool nav_stop_; @@ -240,8 +242,6 @@ namespace mkt_algorithm robot_nav_2d_msgs::Twist2D prevous_drive_cmd_; double x_direction_, y_direction_, theta_direction_; - double max_robot_pose_search_dist_; - double global_plan_prune_distance_{1.0}; // Lookahead bool use_velocity_scaled_lookahead_dist_; @@ -249,6 +249,7 @@ namespace mkt_algorithm double lookahead_dist_; double min_lookahead_dist_; double max_lookahead_dist_; + double max_lateral_accel_; // journey double min_journey_squared_{1e9}; @@ -265,6 +266,7 @@ namespace mkt_algorithm 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_; @@ -276,10 +278,6 @@ namespace mkt_algorithm double inflation_cost_scaling_factor_; double cost_scaling_dist_, cost_scaling_gain_; - double cost_left_goal_, cost_right_goal_; - double cost_left_side_ , cost_right_side_; - double center_cost_; - // Control frequency double control_duration_; std::vector footprint_spec_; 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 deleted file mode 100644 index f12c23b..0000000 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory_.h +++ /dev/null @@ -1,314 +0,0 @@ -#ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ -#define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace mkt_algorithm -{ - namespace diff - { - /** - * @class PredictiveTrajectory - * @brief Standard PredictiveTrajectory-like ScoreAlgorithm - */ - class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm - { - public: - PredictiveTrajectory() : initialized_(false), nav_stop_(false) {}; - - virtual ~PredictiveTrajectory(); - - // Standard ScoreAlgorithm Interface - /** - * @brief Initialize parameters as needed - * @param nh NodeHandle to read parameters from - */ - virtual void initialize( - robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; - - /** - * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories - * - * Subclasses may overwrite. Return false in case there is any error. - * - * @param pose Current pose (costmap frame) - * @param velocity Current velocity - * @param goal The final goal (costmap frame) - * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points - */ - virtual bool prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, - const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan, - double &x_direction, double &y_direction, double &theta_direction) override; - - /** - * @brief Calculating algorithm - * @param pose - * @param velocity - * @param traj - */ - virtual mkt_msgs::Trajectory2D calculator( - const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; - - /** - * @brief Reset all data - */ - virtual void reset() override; - - /** - * @brief Stoping move navigation - */ - virtual void stop() override; - - /** - * @brief resume move navigation after stopped - */ - virtual void resume() override; - - /** - * @brief Create a new PredictiveTrajectory instance - * @return A pointer to the new PredictiveTrajectory instance - */ - static score_algorithm::ScoreAlgorithm::Ptr create(); - - protected: - inline double sign(double x) - { - return x < 0.0 ? -1.0 : 1.0; - } - - /** - * @brief Initialize parameters - */ - virtual void getParams(); - - /** - * @brief Dynamically adjust look ahead distance based on the speed - * @param velocity - * @return look ahead distance - */ - double getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity); - - /** - * @brief Get lookahead point on the global plan - * @param lookahead_dist - * @param global_plan - * @return lookahead point - */ - std::vector::iterator - getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan); - - /** - * @brief Prune global plan - * @param tf - * @param pose - * @param global_plan - * @param dist_behind_robot - * @return true if plan is pruned, false otherwise - */ - bool pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, - robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot); - - /** - * @brief Transforms the global plan of the robot from the planner frame to the local frame (modified). - * - * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h - * such that the index of the current goal pose is returned as well as - * the transformation between the global plan and the planning frame. - * @param tf A reference to a tf buffer - * @param global_plan The plan to be transformed - * @param pose The pose of the robot - * @param costmap A reference to the costmap being used so the window size for transforming can be computed - * @param global_frame The frame to transform the plan to - * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!] - * @param[out] transformed_plan Populated with the transformed plan - * @return \c true if the global plan is transformed, \c false otherwise - */ - bool transformGlobalPlan( - TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose, - const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length, - robot_nav_2d_msgs::Path2D &transformed_plan); - - robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose); - - /** - * @brief Should rotate to path - * @param carrot_pose - * @param angle_to_path - * @return true if should rotate, false otherwise - */ - bool shouldRotateToPath( - const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x); - - /** - * @brief Rotate to heading - * @param angle_to_path - * @param velocity The velocity of the robot - * @param cmd_vel The velocity commands to be filled - */ - void rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel); - - /** - * @brief the robot is moving acceleration limits - * @param velocity The velocity of the robot - * @param cmd_vel The velocity commands - * @param cmd_vel_result The velocity commands result - */ - void moveWithAccLimits( - const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result); - - /** - * @brief Stop the robot taking into account acceleration limits - * @param pose The pose of the robot in the global frame - * @param velocity The velocity of the robot - * @param cmd_vel The velocity commands to be filled - * @return True if a valid trajectory was found, false otherwise - */ - bool stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel); - - /** - * @brief Apply constraints - * @param dist_error - * @param lookahead_dist - * @param curvature - * @param curr_speed - * @param pose_cost - * @param linear_vel - * @param sign - */ - void applyConstraints( - const double &dist_error, const double &lookahead_dist, - const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed, - const double &pose_cost, double &linear_vel, const double &sign_x); - - void computePurePursuit( - const score_algorithm::TrajectoryGenerator::Ptr &traj, - const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, - const robot_nav_2d_msgs::Twist2D &velocity, - const double &min_approach_linear_velocity, - const double &journey_plan, - const double &sign_x, - const double &lookahead_dist_min, - const double &lookahead_dist_max, - const double &lookahead_dist, - const double &lookahead_time, - const double &dt, - robot_nav_2d_msgs::Twist2D &drive_cmd - ); - - double adjustSpeedWithHermiteTrajectory( - const robot_nav_2d_msgs::Twist2D &velocity, - const robot_nav_2d_msgs::Path2D &trajectory, - double v_target, - const double &sign_x - ); - - std::vector interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector footprint, double resolution); - - /** - * @brief Cost at pose - * @param x - * @param y - * @return cost - */ - double costAtPose(const double &x, const double &y); - - double computeDecelerationFactor(double remaining_distance, double decel_distance); - - double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan); - - double estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan); - - - std::vector angle_history_; - size_t window_size_; - - bool initialized_; - bool nav_stop_; - robot::NodeHandle nh_, nh_priv_; - std::string frame_id_path_; - std::string robot_base_frame_; - - robot_nav_2d_msgs::Pose2DStamped goal_; - robot_nav_2d_msgs::Path2D global_plan_; - robot_nav_2d_msgs::Path2D compute_plan_; - robot_nav_2d_msgs::Path2D transform_plan_; - robot_nav_2d_msgs::Twist2D prevous_drive_cmd_; - - double x_direction_, y_direction_, theta_direction_; - double max_robot_pose_search_dist_; - double global_plan_prune_distance_{1.0}; - - // Lookahead - bool use_velocity_scaled_lookahead_dist_; - double lookahead_time_; - double lookahead_dist_; - double min_lookahead_dist_; - double max_lookahead_dist_; - double max_lateral_accel_; - - // journey - double min_journey_squared_{1e9}; - double max_journey_squared_{1e9}; - - // Rotate to heading - bool use_rotate_to_heading_; - double rotate_to_heading_min_angle_; - - double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_; - double min_path_distance_, max_path_distance_; - - // Regulated linear velocity scaling - bool use_regulated_linear_velocity_scaling_; - double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_; - double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_; - - double rot_stopped_velocity_, trans_stopped_velocity_; - - double min_approach_linear_velocity_; - double regulated_linear_scaling_min_radius_; - double regulated_linear_scaling_min_speed_; - - bool use_cost_regulated_linear_velocity_scaling_; - double inflation_cost_scaling_factor_; - double cost_scaling_dist_, cost_scaling_gain_; - - double cost_left_goal_, cost_right_goal_; - double cost_left_side_ , cost_right_side_; - double center_cost_; - - // Control frequency - double control_duration_; - std::vector footprint_spec_; - - robot::Time last_actuator_update_; - boost::shared_ptr kf_; - int m_, n_; - Eigen::MatrixXd A; - Eigen::MatrixXd C; - Eigen::MatrixXd Q; - Eigen::MatrixXd R; - Eigen::MatrixXd P; - - }; // class PredictiveTrajectory - - } // namespace diff - -} // namespace mkt_algorithm - -#endif //_NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h index 06ab8b0..4af845f 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h @@ -59,12 +59,6 @@ namespace mkt_algorithm */ static score_algorithm::ScoreAlgorithm::Ptr create(); - protected: - /** - * @brief Initialize parameters - */ - virtual void getParams() override; - }; // class RotateToGoalDiff } // namespace diff diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h deleted file mode 100644 index 20d5d86..0000000 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__ -#define _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace mkt_algorithm -{ - namespace diff - { - class PurePursuit - { - public: - void computePurePursuit( - const score_algorithm::TrajectoryGenerator::Ptr &traj, - const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, - const robot_nav_2d_msgs::Twist2D &velocity, - const double &min_approach_linear_velocity, - const double &journey_plan, - const double &sign_x, - const double &lookahead_dist_min, - const double &lookahead_dist_max, - const double &lookahead_dist, - const double &lookahead_time, - const double &dt, - robot_nav_2d_msgs::Twist2D &drive_cmd - ); - - - private: - void 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 getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, - const double &journey_plan); - - double computeDecelerationFactor(const double &effective_journey, const double &d_reduce); - - // properties - double max_lateral_accel_; - }; - } -} -#endif \ No newline at end of file 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 cd6b87b..a655108 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 @@ -7,15 +7,14 @@ void mkt_algorithm::diff::GoStraight::initialize( { if (!initialized_) { - nh_ = robot::NodeHandle("~"); + nh_ = nh; nh_priv_ = robot::NodeHandle(nh, name); name_ = name; tf_ = tf; traj_ = traj; costmap_robot_ = costmap_robot; - + nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0); this->getParams(); - nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); std::vector footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector(); if (footprint.size() > 1) 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 4dbab9f..fd47170 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,6 +1,5 @@ -#include +#include #include -#define LIMIT_VEL_THETA 0.33 mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {} @@ -9,14 +8,14 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize( { if (!initialized_) { - nh_ = robot::NodeHandle("~/"); + nh_ = nh; nh_priv_ = robot::NodeHandle(nh, name); name_ = name; tf_ = tf; traj_ = traj; costmap_robot_ = costmap_robot; + nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0); this->getParams(); - nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); if (footprint.size() > 1) @@ -105,10 +104,12 @@ 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); - nh_priv_.param("rot_stopped_velocity", rot_stopped_velocity_, 0.0); - nh_priv_.param("trans_stopped_velocity", trans_stopped_velocity_, 0.0); - if (trans_stopped_velocity_ <= min_approach_linear_velocity_ || trans_stopped_velocity_ - min_approach_linear_velocity_ < 0.02) - trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.05; + 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; // Regulated linear velocity scaling nh_priv_.param("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false); @@ -120,6 +121,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams() nh_priv_.param("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0); nh_priv_.param("cost_scaling_dist", cost_scaling_dist_, 0.6); nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); + nh_priv_.param("max_lateral_accel", max_lateral_accel_, 1.0); if (inflation_cost_scaling_factor_ <= 0.0) { robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__); @@ -127,7 +129,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams() } double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); control_duration_ = 1.0 / control_frequency; - window_size_ = (size_t)(control_frequency * 3.0); if (traj_) { @@ -145,6 +146,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams() traj_.get()->getNodeHandle().param("min_vel_theta", min_vel_theta_, 0.0); traj_.get()->getNodeHandle().param("acc_lim_theta", acc_lim_theta_, 0.0); traj_.get()->getNodeHandle().param("decel_lim_theta", decel_lim_theta_, 0.0); + traj_.get()->getNodeHandle().param("min_speed_xy", min_speed_xy_, 0.0); + traj_.get()->getNodeHandle().param("max_speed_xy", max_speed_xy_, 0.0); } } @@ -284,7 +287,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs: robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__); return false; } - + const auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_); + if(fabs(carrot_pose.pose.y) > 0.2) + { + lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist); + if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_)) + { + robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__); + return false; + } + } x_direction = x_direction_; y_direction = y_direction_ = 0; @@ -319,7 +331,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs: for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it) { double dx = it->pose.x - carrot_pose_it->pose.x; - double dy = it->pose.x - carrot_pose_it->pose.y; + double dy = it->pose.y - carrot_pose_it->pose.y; distance_it += std::hypot(dx, dy); if (distance_it > costmap_robot_->getCostmap()->getResolution()) { @@ -373,14 +385,15 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( robot_nav_2d_msgs::Twist2D drive_cmd; double sign_x = sign(x_direction_); + robot_nav_2d_msgs::Twist2D twist; traj_->startNewIteration(velocity); while (robot::ok() && traj_->hasMoreTwists()) { twist = traj_->nextTwist(); } - drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5); double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; + drive_cmd.x = std::min(sqrt(twist.x * twist.x), fabs(v_max)); robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; if (transformed_plan.poses.empty()) { @@ -390,35 +403,16 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( prevous_drive_cmd_ = drive_cmd; return result; } - else - { - result.poses.clear(); - result.poses.reserve(transformed_plan.poses.size()); - for (const auto &pose_stamped : transformed_plan.poses) - { - result.poses.push_back(pose_stamped.pose); - } - } double lookahead_dist = getLookAheadDistance(velocity); - double tolerance = hypot(pose.pose.x - compute_plan_.poses.front().pose.x, pose.pose.y - compute_plan_.poses.front().pose.y); - - if (transformed_plan.poses.empty()) - { - robot::log_warning("[%s:%d]\n Transformed plan is empty after compute lookahead point", __FILE__, __LINE__); - return result; - } auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); - bool allow_rotate = false; nh_priv_.param("allow_rotate", allow_rotate, false); - robot_geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose; const double distance_allow_rotate = min_journey_squared_; - const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y); - const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1); - + 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); allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; + double angle_to_heading; if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x)) { @@ -434,18 +428,30 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( } else { - const double vel_x_reduce = std::min(fabs(v_max), 0.3); - double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; - carrot_dist2 = std::max(carrot_dist2, 0.05); - double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; - - const auto &plan_back_pose = compute_plan_.poses.back(); - double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y); - post_cost = std::max(post_cost, center_cost_); - this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x); + 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); + } + 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( + carrot_pose, + drive_target, + velocity, + transformed_plan, + min_approach_linear_velocity_, + sign_x, + dt, + drive_cmd); + 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_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale; + 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))); @@ -458,51 +464,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( double vel_reduce = sign_x > 0 ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min) : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min); - drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce; - - double v_theta = drive_cmd.x * curvature; - double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); - if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_)) - { - carrot_dist2 *= 0.6; - curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; - v_theta = drive_cmd.x * curvature; - } - if (fabs(v_theta) > LIMIT_VEL_THETA) - { - robot_nav_2d_msgs::Twist2D cmd_vel, cmd_result; - cmd_vel.x = sign_x > 0 - ? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1)) - : std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1)); - cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5); - this->moveWithAccLimits(velocity, cmd_vel, cmd_result); - drive_cmd.x = std::copysign(cmd_result.x, sign_x); - v_theta = drive_cmd.x * curvature; - } - - if (journey_plan < min_journey_squared_) - { - if (transform_plan_.poses.size() > 2) - { - robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back(); - robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2]; - double dx = end.pose.x - start.pose.x; - double dy = end.pose.y - start.pose.y; - v_theta = atan2(dy, dx); - if (v_theta > M_PI_2) - v_theta -= M_PI; - else if (v_theta < -M_PI_2) - v_theta += M_PI; - // v_theta *= 0.5; - v_theta = std::clamp(v_theta, -0.02, 0.02); - } - else - v_theta = 0.0; - } - double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8; - double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt; - double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt; - drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth); + 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); if (this->nav_stop_) { @@ -517,31 +480,240 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( return result; } - Eigen::VectorXd y(2); - y << drive_cmd.x, drive_cmd.theta; + // Eigen::VectorXd y(2); + // y << drive_cmd.x, drive_cmd.theta; - // Cập nhật lại A nếu dt thay đổi - for (int i = 0; i < n_; ++i) - for (int j = 0; j < n_; ++j) - A(i, j) = (i == j ? 1.0 : 0.0); - for (int i = 0; i < n_; i += 3) - { - A(i, i + 1) = dt; - A(i, i + 2) = 0.5 * dt * dt; - A(i + 1, i + 2) = dt; - } - kf_->update(y, dt, A); + // // Cập nhật lại A nếu dt thay đổi + // for (int i = 0; i < n_; ++i) + // for (int j = 0; j < n_; ++j) + // A(i, j) = (i == j ? 1.0 : 0.0); + // for (int i = 0; i < n_; i += 3) + // { + // A(i, i + 1) = dt; + // A(i, i + 2) = 0.5 * dt * dt; + // A(i + 1, i + 2) = dt; + // } + // kf_->update(y, dt, A); - drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); - drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x); - drive_cmd.theta = std::clamp(kf_->state()[3], -LIMIT_VEL_THETA, LIMIT_VEL_THETA); + // drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); + // drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x); + // drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_); } + result.poses.clear(); + result.poses.reserve(transformed_plan.poses.size()); + for (const auto &pose_stamped : transformed_plan.poses) + { + if(fabs(pose_stamped.pose.x - carrot_pose.pose.x) < 1e-6 && + fabs(pose_stamped.pose.y - carrot_pose.pose.y) < 1e-6 && + fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6) + break; + result.poses.push_back(pose_stamped.pose); + } result.velocity = drive_cmd; 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, + const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Path2D &trajectory, + const double &min_approach_linear_velocity, + const double &sign_x, + const double &dt, + robot_nav_2d_msgs::Twist2D &drive_cmd) +{ + // 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; + + // 3) Adjust speed using Hermite trajectory curvature + remaining distance + double v_target = adjustSpeedWithHermiteTrajectory(velocity, trajectory, drive_target.x, sign_x); + + const double y_abs = std::fabs(carrot_pose.pose.y); + const double y_soft = 0.1; + if (y_abs > y_soft) + { + double scale = y_soft / y_abs; // y càng lớn => scale càng nhỏ + scale = std::clamp(scale, 0.2, 1.0); // không giảm quá sâu + v_target *= scale; + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = v_target; + this->moveWithAccLimits(velocity, cmd, result); + v_target = result.x; + } + + // 4) Maintain minimum approach speed + if (std::fabs(v_target) < min_approach_linear_velocity) + v_target = std::copysign(min_approach_linear_velocity, sign_x); + + // 5) Angular speed from curvature + double w_target = v_target * kappa; + w_target = std::clamp(w_target, -max_vel_theta_, max_vel_theta_); + + // 6) Apply acceleration limits (linear + angular) + const double dv = std::clamp(v_target - velocity.x, -acc_lim_x_ * dt, acc_lim_x_ * dt); + const double dw = std::clamp(w_target - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt); + + drive_cmd.x = velocity.x + dv; + drive_cmd.theta = velocity.theta + dw; +} + +double mkt_algorithm::diff::PredictiveTrajectory::estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan) { + if (plan.poses.size() < 2) return 0.0; + const auto& p1 = plan.poses[plan.poses.size() - 2]; + const auto& p2 = plan.poses[plan.poses.size() - 1]; + return std::atan2(p2.pose.y - p1.pose.y, p2.pose.x - p1.pose.x); +} + +void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( + const double &dist_error, const double &lookahead_dist, + const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity, + const double &pose_cost, double &linear_vel, const double &sign_x) +{ + double curvature_vel = linear_vel; + double cost_vel = linear_vel; + double approach_vel = linear_vel; + + + if (use_regulated_linear_velocity_scaling_) + { + const double &min_rad = regulated_linear_scaling_min_radius_; + const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad; + if (radius < min_rad) + { + curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = curvature_vel; + this->moveWithAccLimits(velocity, cmd, result); + curvature_vel = result.x; + linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + } + } + + if (use_cost_regulated_linear_velocity_scaling_ && + pose_cost != static_cast(robot_costmap_2d::NO_INFORMATION) && + pose_cost != static_cast(robot_costmap_2d::FREE_SPACE)) + { + const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * + std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) + + inscribed_radius; + + if (min_distance_to_obstacle < cost_scaling_dist_) + { + cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; + } + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = cost_vel; + this->moveWithAccLimits(velocity, cmd, result); + cost_vel = result.x; + linear_vel = std::min(cost_vel, curvature_vel); + } + // ss << linear_vel << " "; + // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + // if the actual lookahead distance is shorter than requested, that means we're at the + // end of the path. We'll scale linear velocity by error to slow to a smooth stop. + // This expression is eq. to + // (1) holding time to goal, t, constant using the theoretical + // lookahead distance and proposed velocity and + // (2) using t with the actual lookahead + // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). + + double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr + ? 2.0 * costmap_robot_->getCostmap()->getResolution() + : 0.1; + if (dist_error > dist_error_limit) + { + double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0; + double unbounded_vel = approach_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) + { + approach_vel = min_approach_linear_velocity_; + } + else + { + approach_vel *= velocity_scaling; + } + + // Use the lowest velocity between approach and other constraints, if all overlapping + robot_nav_2d_msgs::Twist2D cmd, result; + cmd.x = approach_vel; + this->moveWithAccLimits(velocity, cmd, result); + approach_vel = result.x; + linear_vel = std::min(linear_vel, approach_vel); + } + + // Limit linear velocities to be valid + double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x); + double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x); + double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); + double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); + double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y); + double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y); + double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel); + linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel); + linear_vel = sign_x * linear_vel; +} + + bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x) { @@ -574,7 +746,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( } } } - // ROS_INFO_THROTTLE(0.1,"path_angle %.3f %.3f", path_angle, std::atan2(carrot_pose.pose.y, carrot_pose.pose.x)); // 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 @@ -590,22 +761,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( : std::clamp(heading_rotate + heading_linear, 0.3, M_PI_2); bool result = use_rotate_to_heading_ && (is_stopped || sign(angle_to_path) * sign_x < 0) && fabs(angle_to_path) > heading_rotate; - - // if (result) - // { - // ROS_WARN_THROTTLE(0.1, "x %.3f %.3f theta %.3f %.3f", - // velocity.x, trans_stopped_velocity_, velocity.theta, max_vel_theta_ + rot_stopped_velocity_); - // ROS_WARN_THROTTLE(0.1, "%x %.3f %.3f %.3f %.3f %.3f \n", - // is_stopped, path_angle, std::atan2(carrot_pose.pose.y, carrot_pose.pose.x), angle_to_path, heading_rotate, sign_x); - // } - // else - // { - // ROS_INFO_THROTTLE(0.1, "x %.3f %.3f theta %.3f %.3f", - // velocity.x, trans_stopped_velocity_, velocity.theta, max_vel_theta_ + rot_stopped_velocity_); - // ROS_INFO_THROTTLE(0.1, "%x %.3f %.3f %.3f %.3f %.3f \n", - // is_stopped, path_angle,std::atan2(carrot_pose.pose.y, carrot_pose.pose.x) ,angle_to_path, heading_rotate, sign_x); - // } - return result; } @@ -631,7 +786,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an double target_angular_speed = max_vel_theta; if (fabs(ang_diff) < angular_decel_zone) { - // ROS_WARN_THROTTLE(0.2,"%f %f %f %f", ang_diff, angular_decel_zone, zone_begin, max_vel_theta); double cosine_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / angular_decel_zone))); double cosine_speed = max_vel_theta * cosine_factor; @@ -641,9 +795,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an // Ensure minimum speed to avoid stalling target_angular_speed = std::max(target_angular_speed, reduce_vel_theta); } - // else - // ROS_INFO_THROTTLE(1,"%f %f %f %f", ang_diff, angular_decel_zone, max_vel_theta, reduce_vel_theta); - // Apply direction double v_theta_desired = std::copysign(target_angular_speed, ang_diff); @@ -694,48 +845,48 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_ auto goal_pose_it = std::prev(global_plan.poses.end()); return goal_pose_it; } + unsigned int goal_index = (unsigned)global_plan.poses.size() - 1; + // double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; + // double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; + // double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x; + // double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y; - double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; - double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; - double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x; - double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y; + // // make sure that atan2 is defined + // double start_angle = atan2(start_direction_y, start_direction_x); + // double end_angle = atan2(end_direction_y, end_direction_x); + // double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2); - // make sure that atan2 is defined - double start_angle = atan2(start_direction_y, start_direction_x); - double end_angle = atan2(end_direction_y, end_direction_x); - double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2); + // for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++) + // { + // if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) + // { + // const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; + // const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; - for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++) - { - if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) - { - const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; - const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; + // if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) + // { + // double current_angle = atan2(current_direction_y, current_direction_x); + // goal_index = i; + // if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_)) + // continue; - if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) - { - double current_angle = atan2(current_direction_y, current_direction_x); - goal_index = i; - if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_)) - continue; - - if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold) - { - goal_index = i; - break; - } - } - } - } + // if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold) + // { + // goal_index = i; + // break; + // } + // } + // } + // } // Find the first pose which is at a distance greater than the lookahead distance auto goal_pose_it = std::find_if( - global_plan.poses.begin(), global_plan.poses.begin() + goal_index, - [&](const auto &ps) - { - return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist; - }); + global_plan.poses.begin(), global_plan.poses.begin() + goal_index, + [&](const auto &ps) + { + return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist; + }); // If the number of poses is not far enough, take the last pose if (goal_pose_it == global_plan.poses.end()) @@ -745,16 +896,6 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_ return goal_pose_it; } -robot_geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose) -{ - robot_geometry_msgs::PointStamped carrot_msg; - carrot_msg.header = carrot_pose.header; - carrot_msg.point.x = carrot_pose.pose.x; - carrot_msg.point.y = carrot_pose.pose.y; - carrot_msg.point.z = 0.5; // publish right over map to stand out - return carrot_msg; -} - bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot) { if (global_plan.poses.empty()) @@ -801,6 +942,70 @@ 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) +{ + robot_nav_2d_msgs::Path2D hermite_trajectory; + hermite_trajectory.poses.clear(); + hermite_trajectory.header.stamp = pose.header.stamp; + hermite_trajectory.header.frame_id = pose.header.frame_id; + // Characteristic length (can be tuned) + const double x = pose.pose.x; + const double y = pose.pose.y; + const double theta = pose.pose.theta; + double L = std::sqrt(x * x + y * y); + if (L < 1e-6) { + // Degenerate: return single pose + robot_nav_2d_msgs::Pose2DStamped pose_stamped; + pose_stamped.pose.x = 0.0; + pose_stamped.pose.y = 0.0; + pose_stamped.pose.theta = 0.0; + pose_stamped.header.stamp = pose.header.stamp; + pose_stamped.header.frame_id = pose.header.frame_id; + hermite_trajectory.poses.push_back(pose_stamped); + return hermite_trajectory; + } + int samples = L/costmap_robot_->getCostmap()->getResolution(); + if (samples < 1) { + samples = 1; + } + hermite_trajectory.poses.reserve(samples + 1); + + for (int i = 0; i <= samples; ++i) { + double t = static_cast(i) / samples; + double t2 = t * t; + double t3 = t2 * t; + + double h00 = 2 * t3 - 3 * t2 + 1; + double h10 = t3 - 2 * t2 + t; + double h01 = -2 * t3 + 3 * t2; + double h11 = t3 - t2; + + // Hermite interpolation + double px = h10 * L + h01 * x + h11 * (L * std::cos(theta)); + double py = h01 * y + h11 * (L * std::sin(theta)); + + // First derivatives for heading + double dh00 = 6 * t2 - 6 * t; + double dh10 = 3 * t2 - 4 * t + 1; + double dh01 = -6 * t2 + 6 * t; + double dh11 = 3 * t2 - 2 * t; + + double dx = dh10 * L + dh01 * x + dh11 * (L * std::cos(theta)); + double dy = dh01 * y + dh11 * (L * std::sin(theta)); + + double heading = std::atan2(dy, dx); + + robot_nav_2d_msgs::Pose2DStamped pose; + pose.pose.x = px; + pose.pose.y = py; + pose.pose.theta = heading; + pose.header.stamp = hermite_trajectory.header.stamp; + pose.header.frame_id = hermite_trajectory.header.frame_id; + hermite_trajectory.poses.push_back(pose); + } + return hermite_trajectory; +} + bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length, @@ -976,103 +1181,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_na return true; } -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, double &sign_x) -{ - double curvature_vel = linear_vel; - double cost_vel = linear_vel; - double approach_vel = linear_vel; - - // std::stringstream ss; - // ss << linear_vel << " "; - // limit the linear velocity by curvature - 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_); - } - } - // ss << linear_vel << " "; - // limit the linear velocity by proximity to obstacles - 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); - } - - // ss << linear_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; - // ss << linear_vel << " "; - // ROS_INFO_STREAM_THROTTLE(0.1, ss.str()); -} - std::vector mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector footprint, double resolution) { @@ -1127,24 +1235,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, co return static_cast(cost); } -void mkt_algorithm::diff::PredictiveTrajectory::updateCostAtOffset( - TFListenerPtr tf, const std::string &robot_base_frame, const robot_nav_2d_msgs::Pose2DStamped &base_pose, - double x_offset, double y_offset, double &cost_left, double &cost_right) -{ - robot_nav_2d_msgs::Pose2DStamped stamped_pose, transformed_pose; - stamped_pose = base_pose; - stamped_pose.pose.x += x_offset; - stamped_pose.pose.y += y_offset; - - if (robot_nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, transformed_pose)) - { - double cost = this->costAtPose(stamped_pose.pose.x, stamped_pose.pose.y); - if (transformed_pose.pose.y < 0) - cost_right = std::max(cost_right, cost); - else if (transformed_pose.pose.y > 0) - cost_left = std::max(cost_left, cost); - } -} double mkt_algorithm::diff::PredictiveTrajectory::computeDecelerationFactor(double remaining_distance, double decel_distance) { @@ -1173,26 +1263,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::getEffectiveDistance(const rob return journey_plan * std::max(0.0, alignment); // Only positive projection } -bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::vector &angle_history, double current_angle, - double amplitude_threshold, size_t window_size) -{ - - angle_history.push_back(current_angle); - if ((unsigned int)angle_history.size() > window_size) - angle_history.erase(angle_history.begin()); - - if ((unsigned int)angle_history.size() < 2) - return false; - - double max_angle = *std::max_element(angle_history.begin(), angle_history.end()); - double min_angle = *std::min_element(angle_history.begin(), angle_history.end()); - - double amplitude = max_angle - min_angle; - // if(fabs(amplitude) > amplitude_threshold) - // ROS_INFO_THROTTLE(0.1,"%f %f %f %d %d", amplitude, max_angle , min_angle, (unsigned int)angle_history.size(), (unsigned int)window_size); - return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0; -} - score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create() { return std::make_shared(); 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 deleted file mode 100644 index f3d8c5d..0000000 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory_.cpp +++ /dev/null @@ -1,1329 +0,0 @@ -#include -#include -#include - -mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {} - -void mkt_algorithm::diff::PredictiveTrajectory::initialize( - robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) -{ - if (!initialized_) - { - nh_ = robot::NodeHandle("~/"); - nh_priv_ = robot::NodeHandle(nh, name); - name_ = name; - tf_ = tf; - traj_ = traj; - costmap_robot_ = costmap_robot; - this->getParams(); - nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); - - std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); - if (footprint.size() > 1) - { - double min_length = 1e10; - double max_length = 0.0; - for (size_t i = 0; i < footprint.size(); ++i) - { - const robot_geometry_msgs::Point &p1 = footprint[i]; - const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point - double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0; - if (len > max_length) - { - max_length = len; - } - if (len < min_length) - { - min_length = len; - } - } - this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; - this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; - } - - // kalman - last_actuator_update_ = robot::Time::now(); - n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta] - m_ = 2; // measurements: x, y, theta - double dt = control_duration_; - - // Khởi tạo ma trận - A = Eigen::MatrixXd::Identity(n_, n_); - C = Eigen::MatrixXd::Zero(m_, n_); - Q = Eigen::MatrixXd::Zero(n_, n_); - R = Eigen::MatrixXd::Identity(m_, m_); - P = Eigen::MatrixXd::Identity(n_, n_); - - for (int i = 0; i < n_; i += 3) - { - A(i, i + 1) = dt; - A(i, i + 2) = 0.5 * dt * dt; - A(i + 1, i + 2) = dt; - } - - C(0, 0) = 1; - C(1, 3) = 1; - Q(2, 2) = 0.1; - Q(5, 5) = 0.6; - - R(0, 0) = 0.1; - R(1, 1) = 0.2; - - P(3, 3) = 0.4; - P(4, 4) = 0.4; - P(5, 5) = 0.4; - - kf_ = boost::make_shared(dt, A, C, Q, R, P); - Eigen::VectorXd x0(n_); - x0 << 0, 0, 0, 0, 0, 0; - kf_->init(robot::Time::now().toSec(), x0); - - - - x_direction_ = y_direction_ = theta_direction_ = 0; - this->initialized_ = true; - robot::log_info("[%s:%d]\n PredictiveTrajectory Initialized successfully", __FILE__, __LINE__); - } -} - -void mkt_algorithm::diff::PredictiveTrajectory::getParams() -{ - robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link")); - nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); - nh_priv_.param("angle_threshold", angle_threshold_, M_PI / 8); - nh_priv_.param("index_samples", index_samples_, 0); - nh_priv_.param("follow_step_path", follow_step_path_, false); - - nh_priv_.param("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false); - nh_priv_.param("lookahead_dist", lookahead_dist_, 0.25); - nh_priv_.param("min_lookahead_dist", min_lookahead_dist_, 0.3); - nh_priv_.param("max_lookahead_dist", max_lookahead_dist_, 0.9); - nh_priv_.param("lookahead_time", lookahead_time_, 1.5); - nh_priv_.param("min_journey_squared", min_journey_squared_, std::numeric_limits::infinity()); - nh_priv_.param("max_journey_squared", max_journey_squared_, std::numeric_limits::infinity()); - - nh_priv_.param("use_rotate_to_heading", use_rotate_to_heading_, false); - nh_priv_.param("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785); - - nh_priv_.param("rot_stopped_velocity", rot_stopped_velocity_, 0.0); - nh_priv_.param("trans_stopped_velocity", trans_stopped_velocity_, 0.0); - if (trans_stopped_velocity_ <= min_approach_linear_velocity_ || trans_stopped_velocity_ - min_approach_linear_velocity_ < 0.02) - trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.05; - - // Regulated linear velocity scaling - nh_priv_.param("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false); - nh_priv_.param("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9); - nh_priv_.param("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25); - - // Inflation cost scaling (Limit velocity by proximity to obstacles) - nh_priv_.param("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true); - nh_priv_.param("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0); - nh_priv_.param("cost_scaling_dist", cost_scaling_dist_, 0.6); - nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); - nh_priv_.param("max_lateral_accel", max_lateral_accel_, 1.0); - if (inflation_cost_scaling_factor_ <= 0.0) - { - robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__); - use_cost_regulated_linear_velocity_scaling_ = false; - } - double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); - control_duration_ = 1.0 / control_frequency; - window_size_ = (size_t)(control_frequency * 3.0); - - if (traj_) - { - traj_.get()->getNodeHandle().param("max_vel_x", max_vel_x_, 0.0); - traj_.get()->getNodeHandle().param("min_vel_x", min_vel_x_, 0.0); - traj_.get()->getNodeHandle().param("acc_lim_x", acc_lim_x_, 0.0); - traj_.get()->getNodeHandle().param("decel_lim_x", decel_lim_x_, 0.0); - - traj_.get()->getNodeHandle().param("max_vel_y", max_vel_y_, 0.0); - traj_.get()->getNodeHandle().param("min_vel_y", min_vel_y_, 0.0); - traj_.get()->getNodeHandle().param("acc_lim_y", acc_lim_y_, 0.0); - traj_.get()->getNodeHandle().param("decel_lim_y", decel_lim_y_, 0.0); - - traj_.get()->getNodeHandle().param("max_vel_theta", max_vel_theta_, 0.0); - traj_.get()->getNodeHandle().param("min_vel_theta", min_vel_theta_, 0.0); - traj_.get()->getNodeHandle().param("acc_lim_theta", acc_lim_theta_, 0.0); - traj_.get()->getNodeHandle().param("decel_lim_theta", decel_lim_theta_, 0.0); - } -} - -void mkt_algorithm::diff::PredictiveTrajectory::reset() -{ - if (this->initialized_) - { - robot::log_info("[%s:%d]\n PredictiveTrajectory is reset", __FILE__, __LINE__); - reached_intermediate_goals_.clear(); - start_index_saved_vt_.clear(); - this->clear(); - x_direction_ = y_direction_ = theta_direction_ = 0; - this->follow_step_path_ = false; - this->nav_stop_ = false; - last_actuator_update_ = robot::Time::now(); - prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); - - if (kf_) - { - Eigen::VectorXd x0(n_); - x0 << 0, 0, 0, 0, 0, 0; - kf_->init(robot::Time::now().toSec(), x0); - } - } -} - -void mkt_algorithm::diff::PredictiveTrajectory::stop() -{ - this->nav_stop_ = true; -} - -void mkt_algorithm::diff::PredictiveTrajectory::resume() -{ - if (this->initialized_) - { - if(!this->nav_stop_) - return; - prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); - last_actuator_update_ = robot::Time::now(); - - if (kf_) - { - Eigen::VectorXd x0(n_); - x0 << 0, 0, 0, 0, 0, 0; - kf_->init(robot::Time::now().toSec(), x0); - } - this->nav_stop_ = false; - } -} - -bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, - const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan, - double &x_direction, double &y_direction, double &theta_direction) -{ - if (!initialized_) - { - robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__); - return false; - } - - std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); - if (footprint.size() > 1) - { - double min_length = 1e10; - double max_length = 0.0; - for (size_t i = 0; i < footprint.size(); ++i) - { - const robot_geometry_msgs::Point &p1 = footprint[i]; - const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point - double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0; - if (len > max_length) - { - max_length = len; - } - if (len < min_length) - { - min_length = len; - } - } - this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; - this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; - } - - if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2) - { - robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size()); - return false; - } - this->getParams(); - - frame_id_path_ = global_plan.header.frame_id; - goal_ = goal; - global_plan_ = global_plan; - - // prune global plan to cut off parts of the past (spatially before the robot) - if (!this->pruneGlobalPlan(tf_, pose, global_plan_, 1.0)) - { - robot::log_error("[%s:%d]\n pruneGlobalPlan Failed", __FILE__, __LINE__); - return false; - } - - double S = std::numeric_limits::infinity(); - S = std::max(costmap_robot_->getCostmap()->getSizeInCellsX() * costmap_robot_->getCostmap()->getResolution() / 2.0, - costmap_robot_->getCostmap()->getSizeInCellsY() * costmap_robot_->getCostmap()->getResolution() / 2.0); - const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_; - S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S); - compute_plan_.poses.clear(); - - if ((unsigned int)global_plan_.poses.size() == 2) - { - double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x; - double dy = global_plan_.poses.back().pose.y - global_plan_.poses.front().pose.y; - if (hypot(dx, dy) <= 2.0 * costmap_robot_->getCostmap()->getResolution()) - { - compute_plan_ = global_plan_; - } - else - { - robot::log_error("[%s:%d]\n The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution()); - return false; - } - } - else - { - unsigned int start_index, goal_index; - if (!this->computePlanCommand(pose, velocity, S, global_plan_, start_index, goal_index, compute_plan_)) - { - robot::log_error("[%s:%d]\n computePlanCommand is Failed", __FILE__, __LINE__); - return false; - } - } - - double lookahead_dist = this->getLookAheadDistance(velocity); - transform_plan_.poses.clear(); - if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, sqrt(lookahead_dist * lookahead_dist + 0.25), transform_plan_)) - { - robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__); - return false; - } - - x_direction = x_direction_; - y_direction = y_direction_ = 0; - theta_direction = theta_direction_; - - if ((unsigned int)(compute_plan_.poses.size() > 1) && compute_plan_.poses.back().header.seq != global_plan_.poses.back().header.seq) - { - const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose; - int index; - for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--) - { - robot_geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose; - if (robot_nav_2d_utils::poseDistance(p2, pose) > xy_local_goal_tolerance_) - break; - } - const robot_geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose; - if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) - { - const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x); - const double dir_path = cos(fabs(path_angle - p2.theta)); - if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) - x_direction = dir_path > 0 ? FORWARD : BACKWARD; - } - } - else - { - try - { - auto carrot_pose_it = getLookAheadPoint(velocity, lookahead_dist, transform_plan_); - auto prev_carrot_pose_it = transform_plan_.poses.begin(); - double distance_it = 0; - for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it) - { - double dx = it->pose.x - carrot_pose_it->pose.x; - double dy = it->pose.y - carrot_pose_it->pose.y; - distance_it += std::hypot(dx, dy); - if (distance_it > costmap_robot_->getCostmap()->getResolution()) - { - prev_carrot_pose_it = it; - break; - } - } - - robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution() - ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose) - : robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D()); - - robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose); - - // teb_local_planner::PoseSE2 start_pose(front); - // teb_local_planner::PoseSE2 goal_pose(back); - // const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec()); - const double dir_path = 0.0; - if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9) - x_direction = dir_path > 0 ? FORWARD : BACKWARD; - } - catch (std::exception &e) - { - robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what()); - return false; - } - } - - x_direction_ = x_direction; - y_direction_ = y_direction; - theta_direction_ = theta_direction; - return true; -} - -mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( - const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) -{ - mkt_msgs::Trajectory2D result; - result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; - if (!traj_) - return result; - if (compute_plan_.poses.size() < 2) - { - robot::log_warning("[%s:%d]\n Local compute plan is available", __FILE__, __LINE__); - return result; - } - - robot::Time current_time = robot::Time::now(); - double dt = (current_time - last_actuator_update_).toSec(); - last_actuator_update_ = current_time; - - robot_nav_2d_msgs::Twist2D drive_cmd; - double sign_x = sign(x_direction_); - - robot_nav_2d_msgs::Twist2D twist; - traj_->startNewIteration(velocity); - while (robot::ok() && traj_->hasMoreTwists()) - { - twist = traj_->nextTwist(); - } - double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; - drive_cmd.x = std::min(sqrt(twist.x * twist.x), fabs(v_max)); - robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; - if (transformed_plan.poses.empty()) - { - robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__); - stopWithAccLimits(pose, velocity, drive_cmd); - result.velocity = drive_cmd; - prevous_drive_cmd_ = drive_cmd; - return result; - } - - double lookahead_dist = getLookAheadDistance(velocity); - auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); - - if(fabs(carrot_pose.pose.y) > 0.2) - { - lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist); - carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_); - } - - bool allow_rotate = false; - nh_priv_.param("allow_rotate", allow_rotate, false); - - const double distance_allow_rotate = min_journey_squared_; - const double path_distance_to_rotate = hypot(pose.pose.x - transformed_plan.poses.back().pose.x, pose.pose.y - transformed_plan.poses.back().pose.y); - const double journey_plan = transformed_plan.poses.empty() ? distance_allow_rotate : journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1); - - allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; - - double angle_to_heading; - if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x)) - { - if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_)) - { - if (!stopWithAccLimits(pose, velocity, drive_cmd)) - return result; - } - else - { - rotateToHeading(angle_to_heading, velocity, drive_cmd); - } - } - else - { - transformed_plan = this->generateHermiteTrajectory(transformed_plan.poses.back()); - lookahead_dist = getLookAheadDistance(velocity); - carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan); - this->computePurePursuit( - traj_, - carrot_pose, - velocity, - min_approach_linear_velocity_, - journey_plan, sign_x, - min_lookahead_dist_, - max_lookahead_dist_, - lookahead_dist, - lookahead_time_, - dt, drive_cmd); - - // const double vel_x_reduce = std::min(fabs(v_max), 0.3); - // const double scale = fabs(velocity.x) * lookahead_time_ * 0.9; - // const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale; - // double d_reduce = std::clamp(journey_plan, min_S, max_S); - // double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0); - // double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce))); - // double v_min = - // journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_; - // v_min *= sign_x; - - // double effective_journey = getEffectiveDistance(carrot_pose, journey_plan); - // double decel_factor = computeDecelerationFactor(effective_journey, d_reduce); - // double vel_reduce = sign_x > 0 - // ? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min) - // : std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min); - // drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce; - - if (this->nav_stop_) - { - if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_)) - { - if (!stopWithAccLimits(pose, velocity, drive_cmd)) - return result; - } - else - drive_cmd = {}; - result.velocity = drive_cmd; - return result; - } - - // Eigen::VectorXd y(2); - // y << drive_cmd.x, drive_cmd.theta; - - // // Cập nhật lại A nếu dt thay đổi - // for (int i = 0; i < n_; ++i) - // for (int j = 0; j < n_; ++j) - // A(i, j) = (i == j ? 1.0 : 0.0); - // for (int i = 0; i < n_; i += 3) - // { - // A(i, i + 1) = dt; - // A(i, i + 2) = 0.5 * dt * dt; - // A(i + 1, i + 2) = dt; - // } - // kf_->update(y, dt, A); - - // drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); - // drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x); - // drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_); - } - - result.poses.clear(); - result.poses.reserve(transformed_plan.poses.size()); - for (const auto &pose_stamped : transformed_plan.poses) - { - if(fabs(pose_stamped.pose.x - carrot_pose.pose.x) < 1e-6 && - fabs(pose_stamped.pose.y - carrot_pose.pose.y) < 1e-6 && - fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6) - break; - result.poses.push_back(pose_stamped.pose); - } - result.velocity = drive_cmd; - prevous_drive_cmd_ = drive_cmd; - return result; -} - -// void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( -// const score_algorithm::TrajectoryGenerator::Ptr &traj, -// const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, -// const robot_nav_2d_msgs::Twist2D &velocity, -// const double &min_approach_linear_velocity, -// const double &journey_plan, -// const double &sign_x, -// const double &lookahead_dist_min, -// const double &lookahead_dist_max, -// const double &lookahead_dist, -// const double &lookahead_time, -// const double &dt, -// robot_nav_2d_msgs::Twist2D &drive_cmd) -// { - // if (!traj) - // return; - // const double velocity_max = sign_x > 0 ? traj->getTwistLinear(true).x : traj->getTwistLinear(false).x; - // const double vel_x_reduce = std::min(fabs(velocity_max), 0.3); - // double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; - // carrot_dist2 = std::max(carrot_dist2, 0.05); - // double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; - - // if (max_lateral_accel_ > 1e-6) - // { - // const double curvature_abs = std::max(fabs(curvature), 1e-6); - // const double v_lateral_limit = sqrt(max_lateral_accel_ / curvature_abs); - // drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit); - // } - - - // double post_cost = 0.0; - // double distance_error = 0.0; - // robot_nav_2d_msgs::Twist2D twist = traj->getTwist(); - // this->applyConstraints(distance_error, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x); - - - // double d_reduce = std::clamp(journey_plan, lookahead_dist_min, lookahead_dist_max); - - // double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0); - // double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce))); - // double v_min = - // journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity) * cosine_factor_begin_reduce + min_approach_linear_velocity; - // v_min *= sign_x; - - // double v_theta = drive_cmd.x * curvature; - // double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); - - // carrot_dist2 *= 0.6; - // curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; - // v_theta = drive_cmd.x * curvature; - - // double limit_acc_theta = fabs(v_theta) > 0.15 ? 1.0 : 1.8; - // double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt; - // double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt; - // drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth); -// } - -double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajectory( - const robot_nav_2d_msgs::Twist2D &velocity, - const robot_nav_2d_msgs::Path2D &trajectory, - double v_target, - const double &sign_x) -{ - if (trajectory.poses.size() < 3) - return v_target; - - // Use speed-scaled preview distance to evaluate curvature - double preview_dist = std::clamp(std::fabs(velocity.x) * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_); - - double traveled = 0.0; - double max_kappa = 0.0; - for (size_t i = 1; i + 1 < trajectory.poses.size(); ++i) - { - const auto &p0 = trajectory.poses[i - 1].pose; - const auto &p1 = trajectory.poses[i].pose; - const auto &p2 = trajectory.poses[i + 1].pose; - - const double a = std::hypot(p1.x - p0.x, p1.y - p0.y); - const double b = std::hypot(p2.x - p1.x, p2.y - p1.y); - const double c = std::hypot(p2.x - p0.x, p2.y - p0.y); - traveled += a; - - if (a > 1e-6 && b > 1e-6 && c > 1e-6) - { - const double cross = (p1.x - p0.x) * (p2.y - p0.y) - (p1.y - p0.y) * (p2.x - p0.x); - const double area2 = std::fabs(cross); // 2 * area - const double kappa = 2.0 * area2 / (a * b * c); - max_kappa = std::max(max_kappa, std::fabs(kappa)); - } - - if (traveled >= preview_dist) - break; - } - double v_limit = std::fabs(v_target); - if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6) - { - const double v_curve = std::sqrt(max_lateral_accel_ / max_kappa); - v_limit = std::min(v_limit, v_curve); - } - - if (decel_lim_x_ > 1e-6) - { - const double remaining = journey(trajectory.poses, 0, trajectory.poses.size() - 1); - const double v_stop = std::sqrt(2.0 * decel_lim_x_ * std::max(0.0, remaining)); - v_limit = std::min(v_limit, v_stop); - } - - return std::copysign(v_limit, sign_x); -} - -void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( - const score_algorithm::TrajectoryGenerator::Ptr &traj, - const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, - const robot_nav_2d_msgs::Twist2D &velocity, - const double &min_approach_linear_velocity, - const double &journey_plan, - const double &sign_x, - const double &lookahead_dist_min, - const double &lookahead_dist_max, - const double &lookahead_dist, - const double &lookahead_time, - const double &dt, - robot_nav_2d_msgs::Twist2D &drive_cmd) -{ - if (!traj_) - return; - - // 1) Curvature from pure pursuit - const double L2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; - if (L2 < 1e-6) - return; - const double kappa = 2.0 * carrot_pose.pose.y / L2; - - // 2) Base speed from trajectory limits (forward/backward) - double v_max = sign_x > 0 ? traj->getTwistLinear(true).x : traj->getTwistLinear(false).x; - - // 3) Adjust speed using Hermite trajectory curvature + remaining distance - double v_target = adjustSpeedWithHermiteTrajectory(velocity, transform_plan_, v_max, sign_x); - - const double y_abs = std::fabs(carrot_pose.pose.y); - const double y_soft = 0.1; - if (y_abs > y_soft) - { - double scale = y_soft / y_abs; // y càng lớn => scale càng nhỏ - scale = std::clamp(scale, 0.2, 1.0); // không giảm quá sâu - v_target *= scale; - robot_nav_2d_msgs::Twist2D cmd, result; - cmd.x = v_target; - this->moveWithAccLimits(velocity, cmd, result); - v_target = result.x; - } - robot::log_info("v_max: %f, v_target: %f", v_max , v_target); - - // 4) Maintain minimum approach speed - if (std::fabs(v_target) < min_approach_linear_velocity) - v_target = std::copysign(min_approach_linear_velocity, sign_x); - - // 5) Angular speed from curvature - double w_target = v_target * kappa; - w_target = std::clamp(w_target, -max_vel_theta_, max_vel_theta_); - - // 6) Apply acceleration limits (linear + angular) - const double dv = std::clamp(v_target - velocity.x, -acc_lim_x_ * dt, acc_lim_x_ * dt); - const double dw = std::clamp(w_target - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt); - - drive_cmd.x = velocity.x + dv; - drive_cmd.theta = velocity.theta + dw; -} - -double mkt_algorithm::diff::PredictiveTrajectory::estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan) { - if (plan.poses.size() < 2) return 0.0; - const auto& p1 = plan.poses[plan.poses.size() - 2]; - const auto& p2 = plan.poses[plan.poses.size() - 1]; - return std::atan2(p2.pose.y - p1.pose.y, p2.pose.x - p1.pose.x); -} - -void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( - const double &dist_error, const double &lookahead_dist, - const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity, - const double &pose_cost, double &linear_vel, const double &sign_x) -{ - double curvature_vel = linear_vel; - double cost_vel = linear_vel; - double approach_vel = linear_vel; - - - if (use_regulated_linear_velocity_scaling_) - { - const double &min_rad = regulated_linear_scaling_min_radius_; - const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad; - if (radius < min_rad) - { - curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); - robot_nav_2d_msgs::Twist2D cmd, result; - cmd.x = curvature_vel; - this->moveWithAccLimits(velocity, cmd, result); - curvature_vel = result.x; - linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); - } - } - - if (use_cost_regulated_linear_velocity_scaling_ && - pose_cost != static_cast(robot_costmap_2d::NO_INFORMATION) && - pose_cost != static_cast(robot_costmap_2d::FREE_SPACE)) - { - const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius(); - const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * - std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) + - inscribed_radius; - - if (min_distance_to_obstacle < cost_scaling_dist_) - { - cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; - } - robot_nav_2d_msgs::Twist2D cmd, result; - cmd.x = cost_vel; - this->moveWithAccLimits(velocity, cmd, result); - cost_vel = result.x; - linear_vel = std::min(cost_vel, curvature_vel); - } - // ss << linear_vel << " "; - // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed - // if the actual lookahead distance is shorter than requested, that means we're at the - // end of the path. We'll scale linear velocity by error to slow to a smooth stop. - // This expression is eq. to - // (1) holding time to goal, t, constant using the theoretical - // lookahead distance and proposed velocity and - // (2) using t with the actual lookahead - // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). - - double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr - ? 2.0 * costmap_robot_->getCostmap()->getResolution() - : 0.1; - if (dist_error > dist_error_limit) - { - double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0; - double unbounded_vel = approach_vel * velocity_scaling; - if (unbounded_vel < min_approach_linear_velocity_) - { - approach_vel = min_approach_linear_velocity_; - } - else - { - approach_vel *= velocity_scaling; - } - - // Use the lowest velocity between approach and other constraints, if all overlapping - robot_nav_2d_msgs::Twist2D cmd, result; - cmd.x = approach_vel; - this->moveWithAccLimits(velocity, cmd, result); - approach_vel = result.x; - linear_vel = std::min(linear_vel, approach_vel); - } - - // Limit linear velocities to be valid - double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x); - double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x); - double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); - double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); - double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y); - double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y); - double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel); - linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel); - linear_vel = sign_x * linear_vel; -} - - -bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( - const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x) -{ - bool curvature = false; - double path_angle = 0; - if (!global_plan.poses.empty()) - { - if ((unsigned)global_plan.poses.size() > 2) - { - const double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; - const double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; - if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) - { - double start_angle = atan2(start_direction_y, start_direction_x); - for (unsigned int i = 1; i < (unsigned)global_plan.poses.size(); i++) - { - const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; - const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; - if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) - { - double current_angle = atan2(current_direction_y, current_direction_x); - if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) > 1e-9) - { - curvature = true; - break; - } - path_angle = current_angle; - } - } - } - } - } - // Whether we should rotate robot to rough path heading - angle_to_path = curvature ? path_angle : std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); - angle_to_path = sign_x < 0 - ? angles::normalize_angle(M_PI + angle_to_path) - : angles::normalize_angle(angle_to_path); - - double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y); - // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) - double heading_rotate = rotate_to_heading_min_angle_; - bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_); - heading_rotate = is_stopped - ? rotate_to_heading_min_angle_ + heading_linear - : std::clamp(heading_rotate + heading_linear, 0.3, M_PI_2); - - bool result = use_rotate_to_heading_ && (is_stopped || sign(angle_to_path) * sign_x < 0) && fabs(angle_to_path) > heading_rotate; - return result; -} - -void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel) -{ - const double dt = control_duration_; - const double theta_direction = angle_to_path > 0.0 ? 1.0 : -1.0; - double vel_yaw = theta_direction * fabs(velocity.theta); - double ang_diff = angle_to_path; - double max_vel_theta = traj_ ? fabs(traj_->getTwistAngular(true).z) : 0.3; - - double angular_decel_zone = 0.3; // rad - khoảng cách bắt đầu giảm tốc - nh_priv_.param("angular_decel_zone", angular_decel_zone, 0.5); - angular_decel_zone += std::clamp( - fabs(max_vel_theta) + (M_PI_2 - fabs(atan2(min_path_distance_, max_path_distance_))), 0.4, M_PI_2); - double zone_begin = std::max(angular_decel_zone * 0.2, 0.2); - double cosine_begin_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / zone_begin))); - - double reduce_vel_theta = std::min(max_vel_theta, 0.25 + min_vel_theta_); - reduce_vel_theta = fabs(ang_diff) > zone_begin ? reduce_vel_theta : (reduce_vel_theta - min_vel_theta_) * cosine_begin_factor + min_vel_theta_; - - // === ANGULAR DECELERATION ZONE === - double target_angular_speed = max_vel_theta; - if (fabs(ang_diff) < angular_decel_zone) - { - double cosine_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / angular_decel_zone))); - double cosine_speed = max_vel_theta * cosine_factor; - - // Choose deceleration profile - target_angular_speed = cosine_speed; // Smoothest option - - // Ensure minimum speed to avoid stalling - target_angular_speed = std::max(target_angular_speed, reduce_vel_theta); - } - // Apply direction - double v_theta_desired = std::copysign(target_angular_speed, ang_diff); - - // === ACCELERATION LIMITS === - double max_acc_vel = vel_yaw + fabs(acc_lim_theta_) * dt; - double min_acc_vel = vel_yaw - fabs(acc_lim_theta_) * dt; - double v_theta_samp = std::clamp(v_theta_desired, min_acc_vel, max_acc_vel); - - // === STOPPING CONSTRAINT === - double max_speed_to_stop = sqrt(2.0 * fabs(decel_lim_theta_) * fabs(ang_diff)); - v_theta_samp = std::copysign( - std::min(max_speed_to_stop, fabs(v_theta_samp)), ang_diff); - - // === FINAL LIMITS === - v_theta_samp = theta_direction > 0 - ? std::clamp(v_theta_samp, min_vel_theta_, max_vel_theta) - : std::clamp(v_theta_samp, -max_vel_theta, -min_vel_theta_); - - cmd_vel.x = 0.0; - cmd_vel.y = 0.0; - cmd_vel.theta = v_theta_samp; -} - -double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity) -{ - // If using velocity-scaled look ahead distances, find and clamp the dist - // Else, use the static look ahead distance - double lookahead_dist = lookahead_dist_; - if (use_velocity_scaled_lookahead_dist_) - { - lookahead_dist = fabs(velocity.x) * lookahead_time_; - lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); - } - - return lookahead_dist; -} - -std::vector::iterator -mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan) -{ - if (global_plan.poses.empty()) - { - throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty."); - } - - if ((unsigned)global_plan.poses.size() < 2) - { - auto goal_pose_it = std::prev(global_plan.poses.end()); - return goal_pose_it; - } - - unsigned int goal_index = (unsigned)global_plan.poses.size() - 1; - // double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x; - // double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y; - // double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x; - // double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y; - - // // make sure that atan2 is defined - // double start_angle = atan2(start_direction_y, start_direction_x); - // double end_angle = atan2(end_direction_y, end_direction_x); - // double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2); - - // for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++) - // { - // if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) - // { - // const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x; - // const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y; - - // if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) - // { - // double current_angle = atan2(current_direction_y, current_direction_x); - // goal_index = i; - // if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_)) - // continue; - - // if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold) - // { - // goal_index = i; - // break; - // } - // } - // } - // } - - // Find the first pose which is at a distance greater than the lookahead distance - auto goal_pose_it = std::find_if( - global_plan.poses.begin(), global_plan.poses.begin() + goal_index, - [&](const auto &ps) - { - return std::hypot(ps.pose.x, ps.pose.y) >= lookahead_dist; - }); - - // If the number of poses is not far enough, take the last pose - if (goal_pose_it == global_plan.poses.end()) - { - goal_pose_it = std::prev(global_plan.poses.end()); - } - return goal_pose_it; -} - -bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot) -{ - if (global_plan.poses.empty()) - return false; - if (tf == nullptr) - return false; - try - { - // let's get the pose of the robot in the frame of the plan - robot_nav_2d_msgs::Pose2DStamped robot; - if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot)) - { - throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); - } - - double dist_thresh_sq = dist_behind_robot * dist_behind_robot; - - // iterate plan until a pose close the robot is found - std::vector::iterator it = global_plan.poses.begin(); - std::vector::iterator erase_end = it; - while (it != global_plan.poses.end()) - { - double dx = robot.pose.x - it->pose.x; - double dy = robot.pose.y - it->pose.y; - double dist_sq = dx * dx + dy * dy; - if (dist_sq < dist_thresh_sq) - { - erase_end = it; - break; - } - ++it; - } - if (erase_end == global_plan.poses.end()) - return false; - - if (erase_end != global_plan.poses.begin()) - global_plan.poses.erase(global_plan.poses.begin(), erase_end); - } - catch (const tf3::TransformException &ex) - { - robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s\n", __FILE__, __LINE__, ex.what()); - return false; - } - return true; -} - -robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose) -{ - robot_nav_2d_msgs::Path2D hermite_trajectory; - hermite_trajectory.poses.clear(); - hermite_trajectory.header.stamp = pose.header.stamp; - hermite_trajectory.header.frame_id = pose.header.frame_id; - // Characteristic length (can be tuned) - const double x = pose.pose.x; - const double y = pose.pose.y; - const double theta = pose.pose.theta; - double L = std::sqrt(x * x + y * y); - if (L < 1e-6) { - // Degenerate: return single pose - robot_nav_2d_msgs::Pose2DStamped pose_stamped; - pose_stamped.pose.x = 0.0; - pose_stamped.pose.y = 0.0; - pose_stamped.pose.theta = 0.0; - pose_stamped.header.stamp = pose.header.stamp; - pose_stamped.header.frame_id = pose.header.frame_id; - hermite_trajectory.poses.push_back(pose_stamped); - return hermite_trajectory; - } - int samples = L/costmap_robot_->getCostmap()->getResolution(); - if (samples < 1) { - samples = 1; - } - hermite_trajectory.poses.reserve(samples + 1); - - for (int i = 0; i <= samples; ++i) { - double t = static_cast(i) / samples; - double t2 = t * t; - double t3 = t2 * t; - - double h00 = 2 * t3 - 3 * t2 + 1; - double h10 = t3 - 2 * t2 + t; - double h01 = -2 * t3 + 3 * t2; - double h11 = t3 - t2; - - // Hermite interpolation - double px = h10 * L + h01 * x + h11 * (L * std::cos(theta)); - double py = h01 * y + h11 * (L * std::sin(theta)); - - // First derivatives for heading - double dh00 = 6 * t2 - 6 * t; - double dh10 = 3 * t2 - 4 * t + 1; - double dh01 = -6 * t2 + 6 * t; - double dh11 = 3 * t2 - 2 * t; - - double dx = dh10 * L + dh01 * x + dh11 * (L * std::cos(theta)); - double dy = dh01 * y + dh11 * (L * std::sin(theta)); - - double heading = std::atan2(dy, dx); - - robot_nav_2d_msgs::Pose2DStamped pose; - pose.pose.x = px; - pose.pose.y = py; - pose.pose.theta = heading; - pose.header.stamp = hermite_trajectory.header.stamp; - pose.header.frame_id = hermite_trajectory.header.frame_id; - hermite_trajectory.poses.push_back(pose); - } - return hermite_trajectory; -} - -bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( - TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose, - const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length, - robot_nav_2d_msgs::Path2D &transformed_plan) -{ - // this method is a slightly modified version of base_local_planner/goal_functions.h - transformed_plan.poses.clear(); - transformed_plan.header.stamp = pose.header.stamp; - transformed_plan.header.frame_id = robot_base_frame; - - if (global_plan.poses.empty()) - return false; - - if (tf == nullptr) - return false; - - if (costmap == nullptr || costmap->getCostmap() == nullptr) - return false; - - try - { - if (global_plan.poses.empty()) - { - robot::log_error("[%s:%d]\n Received plan with zero length", __FILE__, __LINE__); - return false; - } - - // let's get the pose of the robot in the frame of the plan - robot_nav_2d_msgs::Pose2DStamped robot_pose; - if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) - { - throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); - } - - // we'll discard points on the plan that are outside the local costmap - double dist_threshold = std::max(costmap->getCostmap()->getSizeInCellsX() * costmap->getCostmap()->getResolution() / 2.0, - costmap->getCostmap()->getSizeInCellsY() * costmap->getCostmap()->getResolution() / 2.0); - - dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are - double sq_dist_threshold = dist_threshold * dist_threshold; - // located on the border of the local costmap - - int i = 0; - double sq_dist = std::numeric_limits::infinity(); - - // we need to loop to a point on the plan that is within a certain distance of the robot - bool robot_reached = false; - for (int j = 0; j < (int)global_plan.poses.size(); ++j) - { - double x_diff = robot_pose.pose.x - global_plan.poses[j].pose.x; - double y_diff = robot_pose.pose.y - global_plan.poses[j].pose.y; - double new_sq_dist = x_diff * x_diff + y_diff * y_diff; - if (robot_reached && new_sq_dist > sq_dist_threshold) - { - break; - } // force stop if we have reached the costmap border - - if (new_sq_dist < sq_dist) // find closest distance - { - sq_dist = new_sq_dist; - i = j; - if (sq_dist < costmap->getCostmap()->getResolution()) // 5 cm to the robot; take the immediate local minima; if it's not the global - robot_reached = true; // minima, probably means that there's a loop in the path, and so we prefer this - } - } - - double plan_length = 0; // check cumulative Euclidean distance along the plan - double x_direction_saved = 0.0; - // now we'll transform until points are outside of our distance threshold - while (i < (int)global_plan.poses.size() && sq_dist <= sq_dist_threshold && - (max_plan_length <= 0 || plan_length <= max_plan_length)) - { - robot_nav_2d_msgs::Pose2DStamped stamped_pose; - stamped_pose.pose = global_plan.poses[i].pose; - stamped_pose.header.frame_id = global_plan.header.frame_id; - - robot_nav_2d_msgs::Pose2DStamped newer_pose; - if (!robot_nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, newer_pose)) - { - ++i; - continue; - } - transformed_plan.poses.push_back(newer_pose); - - double x_diff = robot_pose.pose.x - global_plan.poses[i].pose.x; - double y_diff = robot_pose.pose.y - global_plan.poses[i].pose.y; - sq_dist = x_diff * x_diff + y_diff * y_diff; - - // Calculate distance to previous pose - if (i > 0 && max_plan_length > 0) - { - robot_geometry_msgs::Pose2D p1 = global_plan.poses[i].pose; - robot_geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].pose; - plan_length += std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2)); - if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) - { - const double egde_angle = atan2(p1.y - p2.y, p1.x - p2.x); - const double target_direction = cos(fabs(egde_angle - p1.theta)); - if (target_direction * x_direction_saved < 0.0) - { - plan_length = 0; - } - x_direction_saved = target_direction; - } - } - ++i; - } - } - catch (tf3::LookupException &ex) - { - robot::log_error("[%s:%d]\n No Transform available Error: %s\n", __FILE__, __LINE__, ex.what()); - return false; - } - catch (tf3::ConnectivityException &ex) - { - robot::log_error("[%s:%d]\n Connectivity Error: %s\n", __FILE__, __LINE__, ex.what()); - return false; - } - catch (tf3::ExtrapolationException &ex) - { - robot::log_error("[%s:%d]\n Extrapolation Error: %s\n", __FILE__, __LINE__, ex.what()); - if (global_plan.poses.size() > 0) - robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s\n", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str()); - - return false; - } - return true; -} - -void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits( - const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result) -{ - const double dt = control_duration_; - - // --- X axis --- - double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x); - double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x); - - double vx = cmd_vel.x > 0.0 ? std::min(max_vel_x, std::max(min_vel_x, cmd_vel.x)) : std::max(-max_vel_x, std::min(-min_vel_x, cmd_vel.x)); - - double max_acc_vx = velocity.x + fabs(acc_lim_x_) * dt; - double min_acc_vx = velocity.x - fabs(decel_lim_x_) * dt; - vx = std::clamp(vx, min_acc_vx, max_acc_vx); - - // --- Y axis --- - double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); - double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); - double vy = cmd_vel.y > 0.0 ? std::min(max_vel_y, std::max(min_vel_y, cmd_vel.y)) : std::max(-max_vel_y, std::min(-min_vel_y, cmd_vel.y)); - - double max_acc_vy = velocity.y + fabs(acc_lim_y_) * dt; - double min_acc_vy = velocity.y - fabs(decel_lim_y_) * dt; - vy = std::clamp(vy, min_acc_vy, max_acc_vy); - // --- Assign result --- - cmd_vel_result.x = vx; - cmd_vel_result.y = vy; - // cmd_vel_result.theta = vth; -} - -bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel) -{ - // slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible - // but we'll use a tenth of a second to be consistent with the implementation of the local planner. - const double dt = control_duration_; - double vx = sign(velocity.x) * std::max(0.0, (fabs(velocity.x) - fabs(decel_lim_x_) * dt)); - double vy = sign(velocity.y) * std::max(0.0, (fabs(velocity.y) - fabs(decel_lim_y_) * dt)); - - double vel_yaw = velocity.theta; - double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - fabs(decel_lim_theta_) * dt)); - - cmd_vel.x = vx; - cmd_vel.y = vy; - cmd_vel.theta = vth; - return true; -} - -std::vector -mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector footprint, double resolution) -{ - - // Dịch sang tọa độ tuyệt đối - std::vector abs_footprint; - double cos_th = std::cos(pose.theta); - double sin_th = std::sin(pose.theta); - for (auto pt : footprint) - { - pt.x *= 1.2; - pt.y *= 1.2; - robot_geometry_msgs::Point abs_pt; - abs_pt.x = pose.x + pt.x * cos_th - pt.y * sin_th; - abs_pt.y = pose.y + pt.x * sin_th + pt.y * cos_th; - abs_footprint.push_back(abs_pt); - } - - std::vector points; - for (size_t i = 0; i < abs_footprint.size(); ++i) - { - const robot_geometry_msgs::Point &start = abs_footprint[i]; - const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()]; - - double dx = end.x - start.x; - double dy = end.y - start.y; - double distance = std::hypot(dx, dy); - int steps = std::max(1, static_cast(std::floor(distance / resolution))); - - for (int j = 0; j <= steps; ++j) - { - if (j == steps && i != abs_footprint.size() - 1) - continue; // tránh lặp - double t = static_cast(j) / steps; - robot_geometry_msgs::Point pt; - pt.x = start.x + t * dx; - pt.y = start.y + t * dy; - points.push_back(pt); - } - } - return points; -} - -double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, const double &y) -{ - unsigned int mx, my; - unsigned char cost; - if (costmap_robot_->getCostmap()->worldToMap(x, y, mx, my)) - { - cost = costmap_robot_->getCostmap()->getCost(mx, my); - } - return static_cast(cost); -} - - -double mkt_algorithm::diff::PredictiveTrajectory::computeDecelerationFactor(double remaining_distance, double decel_distance) -{ - if (remaining_distance >= decel_distance) - { - return 1.0; // Full speed - } - - // Smooth transition using cosine function - double ratio = fabs(remaining_distance / decel_distance); - return 0.5 * (1.0 + cos(M_PI * (1.0 - ratio))); // Smooth from 1 to 0 -} - -double mkt_algorithm::diff::PredictiveTrajectory::getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, - double journey_plan) -{ - double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x + - carrot_pose.pose.y * carrot_pose.pose.y); - - // Avoid division by zero and handle backward motion - if (carrot_distance < 1e-3) - return journey_plan; - - // Project remaining path onto carrot direction - double alignment = fabs(carrot_pose.pose.x / carrot_distance); // cos(angle) - return journey_plan * std::max(0.0, alignment); // Only positive projection -} - -score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create() -{ - return std::make_shared(); -} - -BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory) \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp index f51df42..ae7241c 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp @@ -14,6 +14,7 @@ void mkt_algorithm::diff::RotateToGoal::initialize( tf_ = tf; traj_ = traj; costmap_robot_ = costmap_robot; + nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0); this->getParams(); x_direction_ = y_direction_ = theta_direction_ = 0; @@ -22,68 +23,13 @@ void mkt_algorithm::diff::RotateToGoal::initialize( } } -void mkt_algorithm::diff::RotateToGoal::getParams() -{ - robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link")); - nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); - nh_priv_.param("angle_threshold", angle_threshold_, M_PI / 8); - nh_priv_.param("index_samples", index_samples_, 0); - nh_priv_.param("follow_step_path", follow_step_path_, false); - - nh_priv_.param("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false); - nh_priv_.param("lookahead_dist", lookahead_dist_, 0.25); - nh_priv_.param("min_lookahead_dist", min_lookahead_dist_, 0.3); - nh_priv_.param("max_lookahead_dist", max_lookahead_dist_, 0.9); - nh_priv_.param("lookahead_time", lookahead_time_, 1.5); - nh_priv_.param("min_journey_squared", min_journey_squared_, std::numeric_limits::infinity()); - nh_priv_.param("max_journey_squared", max_journey_squared_, std::numeric_limits::infinity()); - - nh_priv_.param("use_rotate_to_heading", use_rotate_to_heading_, false); - nh_priv_.param("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785); - - nh_priv_.param("rot_stopped_velocity", rot_stopped_velocity_, 0.0); - nh_priv_.param("trans_stopped_velocity", trans_stopped_velocity_, 0.0); - nh_priv_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0); - - // Regulated linear velocity scaling - nh_priv_.param("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false); - nh_priv_.param("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9); - nh_priv_.param("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25); - - // Inflation cost scaling (Limit velocity by proximity to obstacles) - nh_priv_.param("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true); - nh_priv_.param("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0); - nh_priv_.param("cost_scaling_dist", cost_scaling_dist_, 0.6); - nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); - if (inflation_cost_scaling_factor_ <= 0.0) - { - robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__); - use_cost_regulated_linear_velocity_scaling_ = false; - } - double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); - control_duration_ = 1.0 / control_frequency; - if (traj_) - { - traj_.get()->getNodeHandle().param("max_vel_x", max_vel_x_, 0.0); - traj_.get()->getNodeHandle().param("min_vel_x", min_vel_x_, 0.0); - traj_.get()->getNodeHandle().param("acc_lim_x", acc_lim_x_, 0.0); - traj_.get()->getNodeHandle().param("decel_lim_x", decel_lim_x_, 0.0); - - traj_.get()->getNodeHandle().param("max_vel_y", max_vel_y_, 0.0); - traj_.get()->getNodeHandle().param("min_vel_y", min_vel_y_, 0.0); - traj_.get()->getNodeHandle().param("acc_lim_y", acc_lim_y_, 0.0); - traj_.get()->getNodeHandle().param("decel_lim_y", decel_lim_y_, 0.0); - - traj_.get()->getNodeHandle().param("max_vel_theta", max_vel_theta_, 0.0); - traj_.get()->getNodeHandle().param("min_vel_theta", min_vel_theta_, 0.0); - traj_.get()->getNodeHandle().param("acc_lim_theta", acc_lim_theta_, 0.0); - traj_.get()->getNodeHandle().param("decel_lim_theta", decel_lim_theta_, 0.0); - } -} - void mkt_algorithm::diff::RotateToGoal::reset() { + if (initialized_) + { this->clear(); + x_direction_ = y_direction_ = theta_direction_ = 0; + } } bool mkt_algorithm::diff::RotateToGoal::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, @@ -96,13 +42,17 @@ bool mkt_algorithm::diff::RotateToGoal::prepare(const robot_nav_2d_msgs::Pose2DS robot::log_error("This planner has not been initialized, please call initialize() before using this planner"); return false; } + if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2) + { + robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size()); + return false; + } this->getParams(); - global_plan_ = global_plan; - + frame_id_path_ = global_plan.header.frame_id; goal_ = goal; + double angle = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta); x_direction = y_direction = theta_direction = sign(angle); - ; return true; } diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp deleted file mode 100644 index 00f48ac..0000000 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp +++ /dev/null @@ -1,183 +0,0 @@ -#include - -void mkt_algorithm::diff::PurePursuit::computePurePursuit( - const score_algorithm::TrajectoryGenerator::Ptr &traj, - const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, - const robot_nav_2d_msgs::Twist2D &velocity, - const double &min_approach_linear_velocity, - const double &journey_plan, - const double &sign_x, - const double &lookahead_dist_min, - const double &lookahead_dist_max, - const double &lookahead_dist, - const double &lookahead_time, - const double &dt, - robot_nav_2d_msgs::Twist2D &drive_cmd) -{ - if (!traj) - return; - const double velocity_max = sign_x > 0 ? traj->getTwistLinear(true).x : traj->getTwistLinear(false).x; - const double vel_x_reduce = std::min(fabs(velocity_max), 0.3); - double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y; - carrot_dist2 = std::max(carrot_dist2, 0.05); - double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; - - if (max_lateral_accel_ > 1e-6) - { - const double curvature_abs = std::max(fabs(curvature), 1e-6); - const double v_lateral_limit = sqrt(max_lateral_accel_ / curvature_abs); - drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit); - } - - - double post_cost = 0.0; - double distance_error = 0.0; - robot_nav_2d_msgs::Twist2D twist = traj->getTwist(); - this->applyConstraints(distance_error, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x); - - - double d_reduce = std::clamp(journey_plan, lookahead_dist_min, lookahead_dist_max); - - double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0); - double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce))); - double v_min = - journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity) * cosine_factor_begin_reduce + min_approach_linear_velocity; - v_min *= sign_x; - - double 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; - - double v_theta = drive_cmd.x * curvature; - double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x); - - carrot_dist2 *= 0.6; - curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1; - v_theta = drive_cmd.x * curvature; - - double limit_acc_theta = fabs(v_theta) > 0.15 ? 1.0 : 1.8; - double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt; - double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt; - drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth); -} - -void mkt_algorithm::diff::PurePursuit::applyConstraints( - const double &dist_error, const double &lookahead_dist, - const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity, - const double &pose_cost, double &linear_vel, const double &sign_x) -{ - double curvature_vel = linear_vel; - double cost_vel = linear_vel; - double approach_vel = linear_vel; - - - if (use_regulated_linear_velocity_scaling_) - { - const double &min_rad = regulated_linear_scaling_min_radius_; - const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad; - if (radius < min_rad) - { - curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); - robot_nav_2d_msgs::Twist2D cmd, result; - cmd.x = curvature_vel; - this->moveWithAccLimits(velocity, cmd, result); - curvature_vel = result.x; - linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); - } - } - - if (use_cost_regulated_linear_velocity_scaling_ && - pose_cost != static_cast(robot_costmap_2d::NO_INFORMATION) && - pose_cost != static_cast(robot_costmap_2d::FREE_SPACE)) - { - const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius(); - const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * - std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) + - inscribed_radius; - - if (min_distance_to_obstacle < cost_scaling_dist_) - { - cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; - } - robot_nav_2d_msgs::Twist2D cmd, result; - cmd.x = cost_vel; - this->moveWithAccLimits(velocity, cmd, result); - cost_vel = result.x; - linear_vel = std::min(cost_vel, curvature_vel); - } - // ss << linear_vel << " "; - // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed - // if the actual lookahead distance is shorter than requested, that means we're at the - // end of the path. We'll scale linear velocity by error to slow to a smooth stop. - // This expression is eq. to - // (1) holding time to goal, t, constant using the theoretical - // lookahead distance and proposed velocity and - // (2) using t with the actual lookahead - // distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t). - - double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr - ? 2.0 * costmap_robot_->getCostmap()->getResolution() - : 0.1; - if (dist_error > dist_error_limit) - { - double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0; - double unbounded_vel = approach_vel * velocity_scaling; - if (unbounded_vel < min_approach_linear_velocity_) - { - approach_vel = min_approach_linear_velocity_; - } - else - { - approach_vel *= velocity_scaling; - } - - // Use the lowest velocity between approach and other constraints, if all overlapping - robot_nav_2d_msgs::Twist2D cmd, result; - cmd.x = approach_vel; - this->moveWithAccLimits(velocity, cmd, result); - approach_vel = result.x; - linear_vel = std::min(linear_vel, approach_vel); - } - - // Limit linear velocities to be valid - double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x); - double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x); - double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y); - double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y); - double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y); - double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y); - double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel); - linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel); - linear_vel = sign_x * linear_vel; -} - -double mkt_algorithm::diff::PurePursuit::getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, - double journey_plan) -{ -double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x + -carrot_pose.pose.y * carrot_pose.pose.y); - -// Avoid division by zero and handle backward motion -if (carrot_distance < 1e-3) -return journey_plan; - -// Project remaining path onto carrot direction -double alignment = fabs(carrot_pose.pose.x / carrot_distance); // cos(angle) -return journey_plan * std::max(0.0, alignment); // Only positive projection -} - - -double mkt_algorithm::diff::PurePursuit::computeDecelerationFactor(double remaining_distance, double decel_distance) -{ - if (remaining_distance >= decel_distance) - { - return 1.0; // Full speed - } - - // Smooth transition using cosine function - double ratio = fabs(remaining_distance / decel_distance); - return 0.5 * (1.0 + cos(M_PI * (1.0 - ratio))); // Smooth from 1 to 0 -} \ No newline at end of file diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp index b23b0b5..0e10f6c 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -120,7 +120,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, if (!rotate_algorithm_) { robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str()); - // exit(1); + exit(1); } rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_); robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str()); @@ -128,11 +128,12 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, catch (const std::exception &ex) { robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what()); - exit(1); + // exit(1); } std::string goal_checker_name; planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker")); + robot::log_info_at(__FILE__, __LINE__, "goal_checker_name: %s", goal_checker_name.c_str()); try { robot::PluginLoaderHelper loader; @@ -153,6 +154,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what()); exit(1); } + this->initializeOthers(); robot::log_info("[%s:%d]\n %s is sucessed", __FILE__, __LINE__, name.c_str()); initialized_ = true; @@ -181,10 +183,10 @@ void pnkx_local_planner::PNKXLocalPlanner::reset() { robot::log_info_at(__FILE__, __LINE__, "New Goal Received."); this->unlock(); - traj_generator_->reset(); - goal_checker_->reset(); - nav_algorithm_->reset(); - rotate_algorithm_->reset(); + if(traj_generator_) traj_generator_->reset(); + if(goal_checker_) goal_checker_->reset(); + if(nav_algorithm_) nav_algorithm_->reset(); + if(rotate_algorithm_) rotate_algorithm_->reset(); ret_nav_ = ret_angle_ = false; } @@ -214,8 +216,6 @@ void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &pa { return; } - // robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(local_plan_.poses.front()); - // pnkx_local_planner::transformGlobalPlan(tf_, local_plan_, local_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, path); path = local_plan_; } @@ -250,7 +250,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose double x_direction, y_direction, theta_direction; if (!ret_nav_) { - if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction)) + if (nav_algorithm_ && !nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction)) { robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str()); throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); @@ -258,7 +258,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose } else { - if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction)) + if (rotate_algorithm_ && !rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction)) { robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str()); throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare"); @@ -269,7 +269,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0; xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0; xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0; - traj_generator_->setDirect(xytheta_direct); + if(traj_generator_) traj_generator_->setDirect(xytheta_direct); } robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose, @@ -307,14 +307,14 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg } else if (!ret_nav_) { - traj = nav_algorithm_->calculator(pose, velocity); + if(nav_algorithm_) traj = nav_algorithm_->calculator(pose, velocity); local_plan_.header.stamp = robot::Time::now(); robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now()); local_plan_ = robot_nav_2d_utils::pathToPath(path); } else { - traj = rotate_algorithm_->calculator(pose, velocity); + if(rotate_algorithm_) traj = rotate_algorithm_->calculator(pose, velocity); local_plan_.header.stamp = robot::Time::now(); robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now()); local_plan_ = robot_nav_2d_utils::pathToPath(path);