From 3f9337046229b48493f9c9c9198aa07eb5214879 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Fri, 16 Jan 2026 17:48:57 +0700 Subject: [PATCH] update --- .../include/score_algorithm/score_algorithm.h | 5 - .../score_algorithm/trajectory_generator.h | 6 + .../Libraries/mkt_algorithm/CMakeLists.txt | 2 +- .../diff/diff_predictive_trajectory.h | 12 +- .../include/mkt_algorithm/diff/pure_pursuit.h | 26 +- .../src/diff/diff_go_straight.cpp | 1 - .../diff_predictive_trajectory copy 2.cpp | 1327 ----------------- .../src/diff/diff_predictive_trajectory.cpp | 247 +-- ...pp => diff_predictive_trajectory_copy.cpp} | 163 +- .../src/diff/diff_rotate_to_goal.cpp | 1 - .../mkt_algorithm/src/diff/pure_pursuit.cpp | 82 +- .../mkt_plugins/standard_traj_generator.h | 6 + .../include/mkt_plugins/velocity_iterator.h | 6 + .../include/mkt_plugins/xy_theta_iterator.h | 6 + .../src/standard_traj_generator.cpp | 5 + .../mkt_plugins/src/xy_theta_iterator.cpp | 9 + .../robot_actionlib_msgs/CMakeLists.txt | 4 +- .../robot_actionlib_msgs/package.xml | 3 - 18 files changed, 142 insertions(+), 1769 deletions(-) delete mode 100644 src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy 2.cpp rename src/Algorithms/Libraries/mkt_algorithm/src/diff/{diff_predictive_trajectory copy.cpp => diff_predictive_trajectory_copy.cpp} (88%) diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h index 5b6a05c..e70c7ad 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h @@ -153,11 +153,6 @@ namespace score_algorithm double old_xy_goal_tolerance_; double angle_threshold_; - bool enable_publish_; - // robot::Publisher reached_intermediate_goals_pub_; - // robot::Publisher current_goal_pub_; - // robot::Publisher closet_robot_goal_pub_; - // robot::Publisher transformed_plan_pub_, compute_plan_pub_; std::vector reached_intermediate_goals_; std::vector start_index_saved_vt_; diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h index 724666d..8dcb577 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h @@ -98,6 +98,12 @@ namespace score_algorithm */ virtual robot_nav_2d_msgs::Twist2D nextTwist() = 0; + /** + * @brief Get the current twist + * @return The current twist + */ + virtual robot_nav_2d_msgs::Twist2D getTwist() = 0; + /** * @brief Get all the twists for an iteration. * diff --git a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt index 792d5da..b9eda74 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt @@ -81,7 +81,7 @@ endif() # Libraries # ======================================================== add_library(${PROJECT_NAME}_diff SHARED - src/diff/diff_predictive_trajectory.cpp + src/diff/diff_predictive_trajectory_copy.cpp src/diff/diff_rotate_to_goal.cpp src/diff/diff_go_straight.cpp # src/diff/pure_pursuit.cpp diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index 8b49fa3..1a909d7 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -210,8 +210,6 @@ namespace mkt_algorithm const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed, const double &pose_cost, double &linear_vel, double &sign_x); - std::vector interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector footprint, double resolution); - /** * @brief Cost at pose * @param x @@ -231,13 +229,8 @@ namespace mkt_algorithm 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_; - bool initialized_; - bool nav_stop_, avoid_obstacles_; + bool nav_stop_; robot::NodeHandle nh_, nh_priv_; std::string frame_id_path_; std::string robot_base_frame_; @@ -285,9 +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_; diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h index 95202fe..9d03e91 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h @@ -23,26 +23,32 @@ namespace mkt_algorithm PurePursuit(); ~PurePursuit(); void computePurePursuit( - const double &velocity_min, - const double &velocity_max, - const double &linear_vel, + const score_algorithm::TrajectoryGenerator::Ptr &traj, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, - const double &min_dist2, - const double &curvature_dist2_floor, - const double &dist2_override); - + 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, + 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, double &sign_x); + 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); - bool detectWobbleByCarrotAngle(std::vector &angle_history, const double &carrot_angle, - const double &litude_threshold, const size_t &window_size); + // properties + double max_lateral_accel_; }; } } 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 469cd5e..cd6b87b 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 @@ -15,7 +15,6 @@ void mkt_algorithm::diff::GoStraight::initialize( costmap_robot_ = costmap_robot; this->getParams(); - nh_.param("publish_topic", enable_publish_, false); nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); std::vector footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector(); diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy 2.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy 2.cpp deleted file mode 100644 index c5c68b4..0000000 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy 2.cpp +++ /dev/null @@ -1,1327 +0,0 @@ -#include -#include -#include - -#define LIMIT_VEL_THETA 0.33 - -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("publish_topic", enable_publish_, false); - nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); - - footprint_spec_ = costmap_robot_->getRobotFootprint(); - 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; - } - this->initKalmanFilter(); - x_direction_ = y_direction_ = theta_direction_ = 0; - this->initialized_ = true; - robot::log_info("[%s:%d]\n PredictiveTrajectory Initialized successfully", __FILE__, __LINE__); - } -} - -mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() -{ - if (kf_) - { - kf_.reset(); - } -} - -void mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter() -{ - // 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); -} - -void mkt_algorithm::diff::PredictiveTrajectory::getParams() -{ - robot_base_frame_ = nh_priv_.param("robot_base_frame", std::string("base_link")); - nh_priv_.param("avoid_obstacles", avoid_obstacles_, false); - 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; - nh_priv_.param("cmd_smoothing_time", cmd_smoothing_time_, 0.2); - nh_priv_.param("max_lateral_accel", max_lateral_accel_, 0.6); - - // 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."); - use_cost_regulated_linear_velocity_scaling_ = false; - } - double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); - control_duration_ = 1.0 / control_frequency; - window_size_ = (size_t)(control_frequency * 3.0); - - if (traj_) - { - traj_.get()->getNodeHandle().param("max_vel_x", max_vel_x_, 0.0); - traj_.get()->getNodeHandle().param("min_vel_x", min_vel_x_, 0.0); - traj_.get()->getNodeHandle().param("acc_lim_x", acc_lim_x_, 0.0); - traj_.get()->getNodeHandle().param("decel_lim_x", decel_lim_x_, 0.0); - - traj_.get()->getNodeHandle().param("max_vel_y", max_vel_y_, 0.0); - traj_.get()->getNodeHandle().param("min_vel_y", min_vel_y_, 0.0); - traj_.get()->getNodeHandle().param("acc_lim_y", acc_lim_y_, 0.0); - traj_.get()->getNodeHandle().param("decel_lim_y", decel_lim_y_, 0.0); - - traj_.get()->getNodeHandle().param("max_vel_theta", max_vel_theta_, 0.0); - traj_.get()->getNodeHandle().param("min_vel_theta", min_vel_theta_, 0.0); - traj_.get()->getNodeHandle().param("acc_lim_theta", acc_lim_theta_, 0.0); - traj_.get()->getNodeHandle().param("decel_lim_theta", decel_lim_theta_, 0.0); - } -} - -void mkt_algorithm::diff::PredictiveTrajectory::reset() -{ - if (this->initialized_) - { - robot::log_info("[%s:%d]\n PredictiveTrajectory is reset", __FILE__, __LINE__); - reached_intermediate_goals_.clear(); - start_index_saved_vt_.clear(); - this->clear(); - x_direction_ = y_direction_ = theta_direction_ = 0; - this->follow_step_path_ = false; - this->nav_stop_ = false; - last_actuator_update_ = robot::Time::now(); - prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); - - if (kf_) - { - Eigen::VectorXd x0(n_); - x0 << 0, 0, 0, 0, 0, 0; - kf_->init(robot::Time::now().toSec(), x0); - } - } -} - -void mkt_algorithm::diff::PredictiveTrajectory::stop() -{ - this->nav_stop_ = true; -} - -void mkt_algorithm::diff::PredictiveTrajectory::resume() -{ - if (this->initialized_) - { - if(!this->nav_stop_) - return; - prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); - last_actuator_update_ = robot::Time::now(); - - if (kf_) - { - Eigen::VectorXd x0(n_); - x0 << 0, 0, 0, 0, 0, 0; - kf_->init(robot::Time::now().toSec(), x0); - } - this->nav_stop_ = false; - } -} - -bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, - const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan, - double &x_direction, double &y_direction, double &theta_direction) -{ - if (!initialized_) - { - robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__); - return false; - } - - std::vector footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector(); - if (footprint.size() > 1) - { - double min_length = 1e10; - double max_length = 0.0; - for (size_t i = 0; i < footprint.size(); ++i) - { - const robot_geometry_msgs::Point &p1 = footprint[i]; - const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point - double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0; - if (len > max_length) - { - max_length = len; - } - if (len < min_length) - { - min_length = len; - } - } - this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; - this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; - } - - if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2) - { - robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size()); - return false; - } - this->getParams(); - - frame_id_path_ = global_plan.header.frame_id; - goal_ = goal; - global_plan_ = global_plan; - - // prune global plan to cut off parts of the past (spatially before the robot) - if (!this->pruneGlobalPlan(tf_, pose, global_plan_, 1.0)) - { - robot::log_error("[%s:%d]\n pruneGlobalPlan Failed", __FILE__, __LINE__); - return false; - } - - double S = std::numeric_limits::infinity(); - S = std::max(costmap_robot_->getCostmap()->getSizeInCellsX() * costmap_robot_->getCostmap()->getResolution() / 2.0, - costmap_robot_->getCostmap()->getSizeInCellsY() * costmap_robot_->getCostmap()->getResolution() / 2.0); - const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_; - S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S); - compute_plan_.poses.clear(); - - if ((unsigned int)global_plan_.poses.size() == 2) - { - double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x; - double dy = global_plan_.poses.back().pose.y - global_plan_.poses.front().pose.y; - if (hypot(dx, dy) <= 2.0 * costmap_robot_->getCostmap()->getResolution()) - { - compute_plan_ = global_plan_; - } - else - { - robot::log_error("[%s:%d]\n The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution()); - return false; - } - } - else - { - unsigned int start_index, goal_index; - if (!this->computePlanCommand(pose, velocity, S, global_plan_, start_index, goal_index, compute_plan_)) - { - robot::log_error("[%s:%d]\n computePlanCommand is Failed", __FILE__, __LINE__); - return false; - } - } - - double lookahead_dist = this->getLookAheadDistance(velocity); - transform_plan_.poses.clear(); - if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_)) - { - robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__); - return false; - } - // else - // { - // robot::log_info("[%s:%d]\n Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_); - // } - - 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; - if (carrot_pose_it != transform_plan_.poses.begin()) - { - 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; - robot::log_info("[%s:%d] velocity in predictive trajectory: %f %f %f", __FILE__, __LINE__, velocity.x, velocity.y, velocity.theta); - traj_->startNewIteration(velocity); - while (robot::ok() && traj_->hasMoreTwists()) - { - twist = traj_->nextTwist(); - } - drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5); - robot::log_info("[%s:%d] twist: %f %f %f", __FILE__, __LINE__, twist.x, twist.y, twist.theta); - robot::log_info("--------------------------------"); - double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; - - robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; - 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); - } - - if (transformed_plan.poses.empty()) - { - robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__); - return result; - } - - 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); - - // double path_distance_to_rotate = hypot(transformed_plan.poses.front().pose.x, transformed_plan.poses.front().pose.y); - 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); - if (avoid_obstacles_) - allow_rotate = journey_plan >= distance_allow_rotate && - fabs(front.y) <= 0.2 && - (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution()); - else - 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 - { - 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; - - 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); - } - - 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); - - 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; - - 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); - - 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); - - // Check if Kalman filter's estimated velocity exceeds v_max - if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE || - cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE || - cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE || - cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE)) - { - drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0]; - } - else - { - 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); - } - - if (cmd_smoothing_time_ > 1e-6) - { - const double alpha = std::clamp(dt / (cmd_smoothing_time_ + dt), 0.0, 1.0); - drive_cmd.x = prevous_drive_cmd_.x + alpha * (drive_cmd.x - prevous_drive_cmd_.x); - drive_cmd.theta = prevous_drive_cmd_.theta + alpha * (drive_cmd.theta - prevous_drive_cmd_.theta); - } - - result.velocity = drive_cmd; - prevous_drive_cmd_ = drive_cmd; - return result; -} - -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; - } - } - } - } - } - // 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 - ? 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; - - // 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; -} - -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) - { - // 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; - - // 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); - } - // 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); - - // === 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; -} - -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()) - 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("PredictiveTrajectory: 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", __FILE__, __LINE__, ex.what()); - return false; - } - return true; -} - -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("PredictiveTrajectory: 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", __FILE__, __LINE__, ex.what()); - return false; - } - catch (tf3::ConnectivityException &ex) - { - robot::log_error("[%s:%d]\n Connectivity Error: %s", __FILE__, __LINE__, ex.what()); - return false; - } - catch (tf3::ExtrapolationException &ex) - { - robot::log_error("[%s:%d]\n Extrapolation Error: %s", __FILE__, __LINE__, ex.what()); - if (global_plan.poses.size() > 0) - robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s", __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)); - - // we do want to check whether or not the command is valid - cmd_vel.x = vx; - cmd_vel.y = vy; - cmd_vel.theta = vth; - 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) -{ - - // 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); -} - -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::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 -} - -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; -} - -void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose) -{ - const auto &plan_back_pose = compute_plan_.poses.back(); - // const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0; - // const double offset_min = this->min_path_distance_; - - auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); - for (const auto &point : points_rb) - { - double cost_goal = costAtPose(point.x, point.y); - double dx = point.x - pose.pose.x; - double dy = point.y - pose.pose.y; - double cos_yaw = cos(-pose.pose.theta); - double sin_yaw = sin(-pose.pose.theta); - double y_rel = dx * sin_yaw + dy * cos_yaw; - const double epsilon = min_path_distance_; - if (y_rel > epsilon) - { - cost_left_side_ = std::max(cost_left_side_, cost_goal); - } - else if (y_rel < -epsilon) - { - cost_right_side_ = std::max(cost_right_side_, cost_goal); - } - } - - unsigned int step_footprint = 10; - if ((unsigned int)(compute_plan_.poses.size() - 1) < 10) - { - auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); - for (const auto &point : points) - { - double cost_goal = costAtPose(point.x, point.y); - double dx = point.x - plan_back_pose.pose.x; - double dy = point.y - plan_back_pose.pose.y; - double cos_yaw = cos(-plan_back_pose.pose.theta); - double sin_yaw = sin(-plan_back_pose.pose.theta); - double y_rel = dx * sin_yaw + dy * cos_yaw; - const double epsilon = min_path_distance_; - if (y_rel > epsilon) - { - cost_left_goal_ = std::max(cost_left_goal_, cost_goal); - } - else if (y_rel < -epsilon) - { - cost_right_goal_ = std::max(cost_right_goal_, cost_goal); - } - else - center_cost_ = std::max(center_cost_, cost_goal); - } - } - else - { - for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint) - { - auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); - for (const auto &point : points) - { - double cost_goal = costAtPose(point.x, point.y); - double dx = point.x - compute_plan_.poses[i].pose.x; - double dy = point.y - compute_plan_.poses[i].pose.y; - double cos_yaw = cos(-compute_plan_.poses[i].pose.theta); - double sin_yaw = sin(-compute_plan_.poses[i].pose.theta); - double y_rel = dx * sin_yaw + dy * cos_yaw; - const double epsilon = min_path_distance_; - if (y_rel > epsilon) - { - cost_left_goal_ = std::max(cost_left_goal_, cost_goal); - } - else if (y_rel < -epsilon) - { - cost_right_goal_ = std::max(cost_right_goal_, cost_goal); - } - else - center_cost_ = std::max(center_cost_, cost_goal); - } - double dx = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.x - compute_plan_.poses[i].pose.x; - double dy = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.y - compute_plan_.poses[i].pose.y; - if (hypot(dx, dy) > 1.0) - break; - } - } -} - -score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create() -{ - return std::make_shared(); -} - -// Register this planner as a GlobalPlanner plugin -BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory) \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index bf87617..5e27a89 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,9 +1,11 @@ #include +#include #include #include #define LIMIT_VEL_THETA 0.33 + 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) { @@ -100,7 +102,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter() void mkt_algorithm::diff::PredictiveTrajectory::getParams() { robot_base_frame_ = nh_priv_.param("robot_base_frame", std::string("base_link")); - nh_priv_.param("avoid_obstacles", avoid_obstacles_, false); 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); @@ -140,7 +141,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_) { @@ -297,10 +297,6 @@ 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; } - // else - // { - // robot::log_info("[%s:%d]\n Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_); - // } x_direction = x_direction_; y_direction = y_direction_ = 0; @@ -350,11 +346,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs: 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; } @@ -391,15 +383,12 @@ 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; - robot::log_info("[%s:%d] velocity in predictive trajectory: %f %f %f", __FILE__, __LINE__, velocity.x, velocity.y, velocity.theta); traj_->startNewIteration(velocity); while (robot::ok() && traj_->hasMoreTwists()) { twist = traj_->nextTwist(); } drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5); - robot::log_info("[%s:%d] twist: %f %f %f", __FILE__, __LINE__, twist.x, twist.y, twist.theta); - robot::log_info("--------------------------------"); double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; @@ -433,12 +422,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( 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); - if (avoid_obstacles_) - allow_rotate = journey_plan >= distance_allow_rotate && - fabs(front.y) <= 0.2 && - (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution()); - else - allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; + + 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)) @@ -455,88 +440,24 @@ 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); - 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; + // std::shared_ptr pure_pursuit = std::make_shared(); + // pure_pursuit->computePurePursuit(traj_, carrot_pose, velocity, min_approach_linear_velocity_, journey_plan, sign_x, lookahead_dist, lookahead_time_, drive_cmd); + - 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; - - 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_)) + if (this->nav_stop_) { - 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) + if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_)) { - 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); + if (!stopWithAccLimits(pose, velocity, drive_cmd)) + return result; } else - v_theta = 0.0; + drive_cmd = {}; + result.velocity = drive_cmd; + return result; } - 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); - - 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; @@ -553,19 +474,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( } kf_->update(y, dt, A); - // Check if Kalman filter's estimated velocity exceeds v_max - if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE || - cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE || - cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE || - cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE)) - { - drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0]; - } - else - { - drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); - } - + 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); } @@ -1107,49 +1016,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( // 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) -{ - - // 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; @@ -1227,89 +1093,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0; } -void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose) -{ - const auto &plan_back_pose = compute_plan_.poses.back(); - // const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0; - // const double offset_min = this->min_path_distance_; - - auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); - for (const auto &point : points_rb) - { - double cost_goal = costAtPose(point.x, point.y); - double dx = point.x - pose.pose.x; - double dy = point.y - pose.pose.y; - double cos_yaw = cos(-pose.pose.theta); - double sin_yaw = sin(-pose.pose.theta); - double y_rel = dx * sin_yaw + dy * cos_yaw; - const double epsilon = min_path_distance_; - if (y_rel > epsilon) - { - cost_left_side_ = std::max(cost_left_side_, cost_goal); - } - else if (y_rel < -epsilon) - { - cost_right_side_ = std::max(cost_right_side_, cost_goal); - } - } - - unsigned int step_footprint = 10; - if ((unsigned int)(compute_plan_.poses.size() - 1) < 10) - { - auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); - for (const auto &point : points) - { - double cost_goal = costAtPose(point.x, point.y); - double dx = point.x - plan_back_pose.pose.x; - double dy = point.y - plan_back_pose.pose.y; - double cos_yaw = cos(-plan_back_pose.pose.theta); - double sin_yaw = sin(-plan_back_pose.pose.theta); - double y_rel = dx * sin_yaw + dy * cos_yaw; - const double epsilon = min_path_distance_; - if (y_rel > epsilon) - { - cost_left_goal_ = std::max(cost_left_goal_, cost_goal); - } - else if (y_rel < -epsilon) - { - cost_right_goal_ = std::max(cost_right_goal_, cost_goal); - } - else - center_cost_ = std::max(center_cost_, cost_goal); - } - } - else - { - for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint) - { - auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); - for (const auto &point : points) - { - double cost_goal = costAtPose(point.x, point.y); - double dx = point.x - compute_plan_.poses[i].pose.x; - double dy = point.y - compute_plan_.poses[i].pose.y; - double cos_yaw = cos(-compute_plan_.poses[i].pose.theta); - double sin_yaw = sin(-compute_plan_.poses[i].pose.theta); - double y_rel = dx * sin_yaw + dy * cos_yaw; - const double epsilon = min_path_distance_; - if (y_rel > epsilon) - { - cost_left_goal_ = std::max(cost_left_goal_, cost_goal); - } - else if (y_rel < -epsilon) - { - cost_right_goal_ = std::max(cost_right_goal_, cost_goal); - } - else - center_cost_ = std::max(center_cost_, cost_goal); - } - double dx = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.x - compute_plan_.poses[i].pose.x; - double dy = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.y - compute_plan_.poses[i].pose.y; - if (hypot(dx, dy) > 1.0) - break; - } - } -} score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create() { diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory_copy.cpp similarity index 88% rename from src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy.cpp rename to src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory_copy.cpp index bf87617..7a82d34 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory_copy.cpp @@ -16,7 +16,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize( traj_ = traj; costmap_robot_ = costmap_robot; this->getParams(); - nh_.param("publish_topic", enable_publish_, false); nh_.param("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1); footprint_spec_ = costmap_robot_->getRobotFootprint(); @@ -42,6 +41,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize( this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1; this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1; } + this->initKalmanFilter(); x_direction_ = y_direction_ = theta_direction_ = 0; this->initialized_ = true; @@ -100,7 +100,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter() void mkt_algorithm::diff::PredictiveTrajectory::getParams() { robot_base_frame_ = nh_priv_.param("robot_base_frame", std::string("base_link")); - nh_priv_.param("avoid_obstacles", avoid_obstacles_, false); 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); @@ -140,7 +139,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_) { @@ -433,13 +431,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( 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); - if (avoid_obstacles_) - allow_rotate = journey_plan >= distance_allow_rotate && - fabs(front.y) <= 0.2 && - (path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution()); - else - allow_rotate |= path_distance_to_rotate >= distance_allow_rotate; + 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)) { @@ -461,8 +454,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( 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_); + double post_cost = 0.0; this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x); const double scale = fabs(velocity.x) * lookahead_time_ * 0.9; @@ -483,12 +475,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( 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; @@ -552,19 +539,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( A(i + 1, i + 2) = dt; } kf_->update(y, dt, A); - - // Check if Kalman filter's estimated velocity exceeds v_max - if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE || - cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE || - cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE || - cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE)) - { - drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0]; - } - else - { - drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max)); - } + 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); @@ -1107,49 +1083,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( // 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) -{ - - // 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; @@ -1227,90 +1160,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0; } -void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose) -{ - const auto &plan_back_pose = compute_plan_.poses.back(); - // const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0; - // const double offset_min = this->min_path_distance_; - - auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); - for (const auto &point : points_rb) - { - double cost_goal = costAtPose(point.x, point.y); - double dx = point.x - pose.pose.x; - double dy = point.y - pose.pose.y; - double cos_yaw = cos(-pose.pose.theta); - double sin_yaw = sin(-pose.pose.theta); - double y_rel = dx * sin_yaw + dy * cos_yaw; - const double epsilon = min_path_distance_; - if (y_rel > epsilon) - { - cost_left_side_ = std::max(cost_left_side_, cost_goal); - } - else if (y_rel < -epsilon) - { - cost_right_side_ = std::max(cost_right_side_, cost_goal); - } - } - - unsigned int step_footprint = 10; - if ((unsigned int)(compute_plan_.poses.size() - 1) < 10) - { - auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); - for (const auto &point : points) - { - double cost_goal = costAtPose(point.x, point.y); - double dx = point.x - plan_back_pose.pose.x; - double dy = point.y - plan_back_pose.pose.y; - double cos_yaw = cos(-plan_back_pose.pose.theta); - double sin_yaw = sin(-plan_back_pose.pose.theta); - double y_rel = dx * sin_yaw + dy * cos_yaw; - const double epsilon = min_path_distance_; - if (y_rel > epsilon) - { - cost_left_goal_ = std::max(cost_left_goal_, cost_goal); - } - else if (y_rel < -epsilon) - { - cost_right_goal_ = std::max(cost_right_goal_, cost_goal); - } - else - center_cost_ = std::max(center_cost_, cost_goal); - } - } - else - { - for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint) - { - auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0); - for (const auto &point : points) - { - double cost_goal = costAtPose(point.x, point.y); - double dx = point.x - compute_plan_.poses[i].pose.x; - double dy = point.y - compute_plan_.poses[i].pose.y; - double cos_yaw = cos(-compute_plan_.poses[i].pose.theta); - double sin_yaw = sin(-compute_plan_.poses[i].pose.theta); - double y_rel = dx * sin_yaw + dy * cos_yaw; - const double epsilon = min_path_distance_; - if (y_rel > epsilon) - { - cost_left_goal_ = std::max(cost_left_goal_, cost_goal); - } - else if (y_rel < -epsilon) - { - cost_right_goal_ = std::max(cost_right_goal_, cost_goal); - } - else - center_cost_ = std::max(center_cost_, cost_goal); - } - double dx = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.x - compute_plan_.poses[i].pose.x; - double dy = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.y - compute_plan_.poses[i].pose.y; - if (hypot(dx, dy) > 1.0) - break; - } - } -} - score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create() { return std::make_shared(); 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 5644082..f51df42 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 @@ -25,7 +25,6 @@ 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("avoid_obstacles", avoid_obstacles_, false); 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); diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp index 8201214..993879b 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp @@ -1,13 +1,23 @@ #include +#define LIMIT_VEL_THETA 0.33 -void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity_min, - const double &velocity_max, - const double &linear_vel, + +void mkt_algorithm::diff::PurePursuit::computePurePursuit( + const score_algorithm::TrajectoryGenerator::Ptr &traj, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, - const double &min_dist2, - const double &curvature_dist2_floor, - const double &dist2_override) + 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, + 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); @@ -20,18 +30,19 @@ void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit); } - 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); - 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 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_; + 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); @@ -39,14 +50,49 @@ void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity 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; + 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); - 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) { - 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; + 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); + } diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h index 9c56b03..abe6e52 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h @@ -81,6 +81,12 @@ namespace mkt_plugins * @return Next valid velocity combination */ robot_nav_2d_msgs::Twist2D nextTwist() override; + + /** + * @brief Get the current velocity + * @return The current velocity + */ + robot_nav_2d_msgs::Twist2D getTwist() override; /** * @brief Generate a trajectory from start pose to goal diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h index 12fbe3c..2d1dbc8 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h @@ -54,6 +54,12 @@ public: * Should only be called when hasMoreTwists() returns true. */ virtual robot_nav_2d_msgs::Twist2D nextTwist() = 0; + + /** + * @brief Get the current velocity + * @return The current velocity + */ + virtual robot_nav_2d_msgs::Twist2D getTwist() = 0; }; // class VelocityIterator } // namespace mkt_plugins diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h index fd77c60..d650f7d 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h @@ -54,6 +54,12 @@ namespace mkt_plugins * Automatically iterates to the next valid velocity if current is invalid. */ robot_nav_2d_msgs::Twist2D nextTwist() override; + + /** + * @brief Get the current velocity + * @return The current velocity (x, y, theta) + */ + robot_nav_2d_msgs::Twist2D getTwist() override; protected: /** diff --git a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp index 28ee174..c077009 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp @@ -166,6 +166,11 @@ namespace mkt_plugins return velocity_iterator_->nextTwist(); } + robot_nav_2d_msgs::Twist2D StandardTrajectoryGenerator::getTwist() + { + return velocity_iterator_->getTwist(); + } + std::vector StandardTrajectoryGenerator::getTimeSteps(const robot_nav_2d_msgs::Twist2D &cmd_vel) { std::vector steps; diff --git a/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp index 6f7865f..41eb3c6 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp @@ -40,6 +40,15 @@ robot_nav_2d_msgs::Twist2D XYThetaIterator::nextTwist() return velocity; } +robot_nav_2d_msgs::Twist2D XYThetaIterator::getTwist() +{ + robot_nav_2d_msgs::Twist2D velocity; + velocity.x = x_it_->getVelocity(); + velocity.y = y_it_->getVelocity(); + velocity.theta = th_it_->getVelocity(); + return velocity; +} + bool XYThetaIterator::isValidVelocity() { return kinematics_->isValidSpeed(x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity()); diff --git a/src/Navigations/Libraries/robot_actionlib_msgs/CMakeLists.txt b/src/Navigations/Libraries/robot_actionlib_msgs/CMakeLists.txt index b01d981..0cc579e 100644 --- a/src/Navigations/Libraries/robot_actionlib_msgs/CMakeLists.txt +++ b/src/Navigations/Libraries/robot_actionlib_msgs/CMakeLists.txt @@ -30,7 +30,6 @@ if (NOT BUILDING_WITH_CATKIN) set(PACKAGES_DIR robot_std_msgs - utils robot_time ) @@ -41,7 +40,6 @@ else() # ======================================================== find_package(catkin REQUIRED COMPONENTS robot_std_msgs - utils robot_time ) @@ -50,7 +48,7 @@ else() catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS robot_std_msgs utils robot_time + CATKIN_DEPENDS robot_std_msgs robot_time DEPENDS Boost ) diff --git a/src/Navigations/Libraries/robot_actionlib_msgs/package.xml b/src/Navigations/Libraries/robot_actionlib_msgs/package.xml index 720d24b..f39a664 100644 --- a/src/Navigations/Libraries/robot_actionlib_msgs/package.xml +++ b/src/Navigations/Libraries/robot_actionlib_msgs/package.xml @@ -22,9 +22,6 @@ robot_std_msgs robot_std_msgs - utils - utils - robot_time robot_time \ No newline at end of file