From 575e190988af0fcd4d61c3cfdfe6d06cf5a2d608 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Wed, 21 Jan 2026 16:00:36 +0700 Subject: [PATCH] update --- .../Libraries/mkt_algorithm/CMakeLists.txt | 2 +- .../diff/diff_predictive_trajectory.h | 21 +- .../src/diff/diff_predictive_trajectory.cpp | 261 ++-- .../diff/diff_predictive_trajectory_copy.cpp | 1169 ----------------- .../mkt_plugins/one_d_velocity_iterator.h | 3 - .../mkt_plugins/src/xy_theta_iterator.cpp | 1 - .../src/pnkx_local_planner.cpp | 12 +- 7 files changed, 191 insertions(+), 1278 deletions(-) delete mode 100644 src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory_copy.cpp diff --git a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt index b9eda74..792d5da 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_copy.cpp + src/diff/diff_predictive_trajectory.cpp src/diff/diff_rotate_to_goal.cpp src/diff/diff_go_straight.cpp # src/diff/pure_pursuit.cpp diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index 1a909d7..026366f 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 @@ -3,8 +3,6 @@ #include #include - -#include #include #include #include @@ -12,8 +10,6 @@ #include #include #include -#include -#include #include #include #include @@ -44,7 +40,7 @@ namespace mkt_algorithm * @param nh NodeHandle to read parameters from */ virtual void initialize( - robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; + robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override; /** * @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories @@ -101,11 +97,6 @@ namespace mkt_algorithm */ virtual void getParams(); - /** - * @brief Initialize Kalman filter - */ - virtual void initKalmanFilter(); - /** * @brief Dynamically adjust look ahead distance based on the speed * @param velocity @@ -210,6 +201,8 @@ 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 @@ -229,6 +222,11 @@ 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_; robot::NodeHandle nh_, nh_priv_; @@ -278,6 +276,9 @@ 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/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 5e27a89..d555b69 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,27 +1,23 @@ #include -#include -#include -#include - +#include #define LIMIT_VEL_THETA 0.33 +mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {} void mkt_algorithm::diff::PredictiveTrajectory::initialize( robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) { if (!initialized_) { - nh_ = robot::NodeHandle("~"); - nh_priv_ = robot::NodeHandle(nh, name); + nh_ = robot::NodeHandle("~/"); + nh_priv_ = robot::NodeHandle("~/" + 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) { @@ -44,64 +40,55 @@ 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(); + + // kalman + last_actuator_update_ = robot::Time::now(); + n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta] + m_ = 2; // measurements: x, y, theta + double dt = control_duration_; + + // Khởi tạo ma trận + A = Eigen::MatrixXd::Identity(n_, n_); + C = Eigen::MatrixXd::Zero(m_, n_); + Q = Eigen::MatrixXd::Zero(n_, n_); + R = Eigen::MatrixXd::Identity(m_, m_); + P = Eigen::MatrixXd::Identity(n_, n_); + + for (int i = 0; i < n_; i += 3) + { + A(i, i + 1) = dt; + A(i, i + 2) = 0.5 * dt * dt; + A(i + 1, i + 2) = dt; + } + + C(0, 0) = 1; + C(1, 3) = 1; + Q(2, 2) = 0.1; + Q(5, 5) = 0.6; + + R(0, 0) = 0.1; + R(1, 1) = 0.2; + + P(3, 3) = 0.4; + P(4, 4) = 0.4; + P(5, 5) = 0.4; + + kf_ = boost::make_shared(dt, A, C, Q, R, P); + Eigen::VectorXd x0(n_); + x0 << 0, 0, 0, 0, 0, 0; + kf_->init(robot::Time::now().toSec(), x0); + + + x_direction_ = y_direction_ = theta_direction_ = 0; this->initialized_ = true; robot::log_info("[%s:%d]\n PredictiveTrajectory Initialized successfully", __FILE__, __LINE__); } } -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")); + robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link")); nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); nh_priv_.param("angle_threshold", angle_threshold_, M_PI / 8); nh_priv_.param("index_samples", index_samples_, 0); @@ -135,12 +122,12 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams() 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."); + robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__); use_cost_regulated_linear_velocity_scaling_ = false; } double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); control_duration_ = 1.0 / control_frequency; + window_size_ = (size_t)(control_frequency * 3.0); if (traj_) { @@ -298,6 +285,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs: return false; } + x_direction = x_direction_; y_direction = y_direction_ = 0; theta_direction = theta_direction_; @@ -346,6 +334,9 @@ 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; @@ -375,7 +366,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( 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; @@ -390,15 +381,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( } drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5); double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; - 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__); @@ -414,10 +397,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( 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); @@ -440,11 +423,75 @@ 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); - // 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); - + 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_) { @@ -458,7 +505,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( result.velocity = drive_cmd; return result; } - + Eigen::VectorXd y(2); y << drive_cmd.x, drive_cmd.theta; @@ -709,7 +756,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf 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"); + throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); } double dist_thresh_sq = dist_behind_robot * dist_behind_robot; @@ -737,7 +784,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf } catch (const tf3::TransformException &ex) { - robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s", __FILE__, __LINE__, ex.what()); + robot::log_debug("[%s:%d]\n Cannot prune path since no transform is available: %s\n", __FILE__, __LINE__, ex.what()); return false; } return true; @@ -774,7 +821,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( 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"); + throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); } // we'll discard points on the plan that are outside the local costmap @@ -853,23 +900,23 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( } catch (tf3::LookupException &ex) { - robot::log_error("[%s:%d]\n No Transform available Error: %s", __FILE__, __LINE__, ex.what()); + robot::log_error("[%s:%d]\n No Transform available Error: %s\n", __FILE__, __LINE__, ex.what()); return false; } catch (tf3::ConnectivityException &ex) { - robot::log_error("[%s:%d]\n Connectivity Error: %s", __FILE__, __LINE__, ex.what()); + robot::log_error("[%s:%d]\n Connectivity Error: %s\n", __FILE__, __LINE__, ex.what()); return false; } catch (tf3::ExtrapolationException &ex) { - robot::log_error("[%s:%d]\n Extrapolation Error: %s", __FILE__, __LINE__, ex.what()); + robot::log_error("[%s:%d]\n Extrapolation Error: %s\n", __FILE__, __LINE__, ex.what()); if (global_plan.poses.size() > 0) - robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str()); + robot::log_error("[%s:%d]\n Robot Frame: %s Plan Frame size %d: %s\n", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str()); return false; } - return true; + return true; } void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits( @@ -912,7 +959,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_na 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; @@ -1016,6 +1062,49 @@ 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; @@ -1093,11 +1182,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0; } - 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 +} \ No newline at end of file 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 deleted file mode 100644 index 7a82d34..0000000 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory_copy.cpp +++ /dev/null @@ -1,1169 +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("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("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); - nh_priv_.param("angle_threshold", angle_threshold_, M_PI / 8); - nh_priv_.param("index_samples", index_samples_, 0); - nh_priv_.param("follow_step_path", follow_step_path_, false); - - nh_priv_.param("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false); - nh_priv_.param("lookahead_dist", lookahead_dist_, 0.25); - nh_priv_.param("min_lookahead_dist", min_lookahead_dist_, 0.3); - nh_priv_.param("max_lookahead_dist", max_lookahead_dist_, 0.9); - nh_priv_.param("lookahead_time", lookahead_time_, 1.5); - nh_priv_.param("min_journey_squared", min_journey_squared_, std::numeric_limits::infinity()); - nh_priv_.param("max_journey_squared", max_journey_squared_, std::numeric_limits::infinity()); - - nh_priv_.param("use_rotate_to_heading", use_rotate_to_heading_, false); - nh_priv_.param("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785); - - nh_priv_.param("rot_stopped_velocity", rot_stopped_velocity_, 0.0); - nh_priv_.param("trans_stopped_velocity", trans_stopped_velocity_, 0.0); - if (trans_stopped_velocity_ <= min_approach_linear_velocity_ || trans_stopped_velocity_ - min_approach_linear_velocity_ < 0.02) - trans_stopped_velocity_ = min_approach_linear_velocity_ + 0.05; - - // Regulated linear velocity scaling - nh_priv_.param("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false); - nh_priv_.param("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9); - nh_priv_.param("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25); - - // Inflation cost scaling (Limit velocity by proximity to obstacles) - nh_priv_.param("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true); - nh_priv_.param("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0); - nh_priv_.param("cost_scaling_dist", cost_scaling_dist_, 0.6); - nh_priv_.param("cost_scaling_gain", cost_scaling_gain_, 1.0); - 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; - - 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; - for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it) - { - double dx = it->pose.x - carrot_pose_it->pose.x; - double dy = it->pose.x - carrot_pose_it->pose.y; - 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); - - 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; - - const auto &plan_back_pose = compute_plan_.poses.back(); - 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; - 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 (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); - 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); - } - - 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()); -} - -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::computeDecelerationFactor(double remaining_distance, double decel_distance) -{ - if (remaining_distance >= decel_distance) - { - return 1.0; // Full speed - } - - // Smooth transition using cosine function - double ratio = fabs(remaining_distance / decel_distance); - return 0.5 * (1.0 + cos(M_PI * (1.0 - ratio))); // Smooth from 1 to 0 -} - -double mkt_algorithm::diff::PredictiveTrajectory::getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, - double journey_plan) -{ - double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x + - carrot_pose.pose.y * carrot_pose.pose.y); - - // Avoid division by zero and handle backward motion - if (carrot_distance < 1e-3) - return journey_plan; - - // Project remaining path onto carrot direction - double alignment = fabs(carrot_pose.pose.x / carrot_distance); // cos(angle) - return journey_plan * std::max(0.0, alignment); // Only positive projection -} - -bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::vector &angle_history, double current_angle, - double amplitude_threshold, size_t window_size) -{ - - angle_history.push_back(current_angle); - if ((unsigned int)angle_history.size() > window_size) - angle_history.erase(angle_history.begin()); - - if ((unsigned int)angle_history.size() < 2) - return false; - - double max_angle = *std::max_element(angle_history.begin(), angle_history.end()); - double min_angle = *std::min_element(angle_history.begin(), angle_history.end()); - - double amplitude = max_angle - min_angle; - // if(fabs(amplitude) > amplitude_threshold) - // ROS_INFO_THROTTLE(0.1,"%f %f %f %d %d", amplitude, max_angle , min_angle, (unsigned int)angle_history.size(), (unsigned int)window_size); - return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0; -} - -score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create() -{ - return std::make_shared(); -} - -// 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_plugins/include/mkt_plugins/one_d_velocity_iterator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h index b49cd6b..e9ecf68 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h @@ -120,9 +120,6 @@ namespace mkt_plugins std::string name, double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time, int num_samples) : name_(name) { - if(name_ == "x_it_") - robot::log_warning("[%s:%d] one_d_velocity_iterator: [%s] %f %d %f %f %f %f %f %d", - __FILE__, __LINE__, name_.c_str(), current, direct, min, max, acc_limit, decel_limit, acc_time, num_samples); // if (current < min) // { // current = min; 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 41eb3c6..ab7a690 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp @@ -14,7 +14,6 @@ void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameter void XYThetaIterator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt) { - robot::log_info("[%s:%d] xy_theta_iterator: %f %f %f %f", __FILE__, __LINE__, current_velocity.x, current_velocity.y, current_velocity.theta, dt); x_it_ = std::make_shared("x_it_", current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(), kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_); diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp index 120494f..23940e0 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -214,7 +214,10 @@ void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &pa { return; } + // robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(local_plan_.poses.front()); + // pnkx_local_planner::transformGlobalPlan(tf_, local_plan_, local_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, path); path = local_plan_; + } void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) @@ -234,12 +237,9 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose } // Update time stamp of goal pose - // goal_pose_.header.stamp = pose.header.stamp; - // robot::log_info("pose: %f %f %f", pose.pose.x, pose.pose.y, pose.pose.theta); robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), local_goal_pose = this->transformPoseToLocal(goal_pose_); - // robot::log_info("local_start_pose: %f %f %f", local_start_pose.pose.x, local_start_pose.pose.y, local_start_pose.pose.theta); - // robot::log_info("local_goal_pose: %f %f %f", local_goal_pose.pose.x, local_goal_pose.pose.y, local_goal_pose.pose.theta); + if (!pnkx_local_planner::transformGlobalPlan( tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_)) { @@ -309,14 +309,14 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg { traj = nav_algorithm_->calculator(pose, velocity); local_plan_.header.stamp = robot::Time::now(); - robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now()); + robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now()); local_plan_ = robot_nav_2d_utils::pathToPath(path); } else { traj = rotate_algorithm_->calculator(pose, velocity); local_plan_.header.stamp = robot::Time::now(); - robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now()); + robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now()); local_plan_ = robot_nav_2d_utils::pathToPath(path); }