HiepLM/pnkx_nav_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

done thuat toan

Browse Source
This commit is contained in:
HiepLM 2026-01-28 17:43:50 +07:00
parent 85789855a8
commit 1fa6af01fd
13 changed files with 435 additions and 2303 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
config/pnkx_local_planner_params.yaml
View File

@ -1,9 +1,9 @@
yaw_goal_tolerance: 0.017
xy_goal_tolerance: 0.03
min_approach_linear_velocity: 0.06
LocalPlannerAdapter:
library_path: liblocal_planner_adapter
yaw_goal_tolerance: 0.017
xy_goal_tolerance: 0.03
min_approach_linear_velocity: 0.06 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
PNKXLocalPlanner:
# Algorithm
library_path: libpnkx_local_planner
@ -76,7 +76,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
library_path: libmkt_algorithm_diff
avoid_obstacles: false
xy_local_goal_tolerance: 0.01
angle_threshold: 0.4
angle_threshold: 0.6
index_samples: 60
follow_step_path: true
@ -85,9 +85,9 @@ MKTAlgorithmDiffPredictiveTrajectory:
# only when false:
lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true:
min_lookahead_dist: 0.4 # The minimum lookahead distance (m) threshold. (default: 0.3)
min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 2.0 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
lookahead_time: 1.9 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2)
max_lateral_accel: 2.0 # Max lateral accel for speed reduction on curves (m/s^2)
@ -99,8 +99,8 @@ MKTAlgorithmDiffPredictiveTrajectory:
angular_decel_zone: 0.1
# stoped
rot_stopped_velocity: 0.1
trans_stopped_velocity: 0.1
rot_stopped_velocity: 0.05
trans_stopped_velocity: 0.03
# Regulated linear velocity scaling
use_regulated_linear_velocity_scaling: false # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)

2
src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
View File

@ -255,7 +255,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
{
sub_goal_index = (unsigned int)global_plan.poses.size() - 1;
sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq;
std::cout << "ScoreAlgorithm: Invalid sub_goal_index, setting to " << sub_goal_index << std::endl;
// std::cout << "ScoreAlgorithm: Invalid sub_goal_index, setting to " << sub_goal_index << std::endl;
}
else
{

3
src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt
View File

@ -81,10 +81,9 @@ endif()
# Libraries
# ========================================================
add_library(${PROJECT_NAME}_diff SHARED
src/diff/diff_predictive_trajectory_.cpp
src/diff/diff_predictive_trajectory.cpp
src/diff/diff_rotate_to_goal.cpp
src/diff/diff_go_straight.cpp
# src/diff/pure_pursuit.cpp
)
if(BUILDING_WITH_CATKIN)

48
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
View File

@ -113,13 +113,6 @@ namespace mkt_algorithm
std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan);
/**
* @brief Create carrot message
* @param carrot_pose
* @return carrot message
*/
robot_geometry_msgs::PointStamped createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose);
/**
* @brief Prune global plan
* @param tf
@ -151,6 +144,8 @@ namespace mkt_algorithm
const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
robot_nav_2d_msgs::Path2D &transformed_plan);
robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose);
/**
* @brief Should rotate to path
* @param carrot_pose
@ -199,7 +194,24 @@ namespace mkt_algorithm
void applyConstraints(
const double &dist_error, const double &lookahead_dist,
const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed,
const double &pose_cost, double &linear_vel, double &sign_x);
const double &pose_cost, double &linear_vel, const double &sign_x);
void computePurePursuit(
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const robot_nav_2d_msgs::Twist2D &drive_target,
const robot_nav_2d_msgs::Twist2D &velocity,
const robot_nav_2d_msgs::Path2D &trajectory,
const double &min_approach_linear_velocity,
const double &sign_x,
const double &dt,
robot_nav_2d_msgs::Twist2D &drive_cmd);
double adjustSpeedWithHermiteTrajectory(
const robot_nav_2d_msgs::Twist2D &velocity,
const robot_nav_2d_msgs::Path2D &trajectory,
double v_target,
const double &sign_x
);
std::vector<robot_geometry_msgs::Point> interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution);
@ -211,21 +223,11 @@ namespace mkt_algorithm
*/
double costAtPose(const double &x, const double &y);
void updateCostAtOffset(
TFListenerPtr tf, const std::string &robot_base_frame, const robot_nav_2d_msgs::Pose2DStamped &base_pose,
double x_offset, double y_offset, double &cost_left, double &cost_right);
double computeDecelerationFactor(double remaining_distance, double decel_distance);
double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan);
bool detectWobbleByCarrotAngle(std::vector<double>& 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<double> angle_history_;
size_t window_size_;
double estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan);
bool initialized_;
bool nav_stop_;
@ -240,8 +242,6 @@ namespace mkt_algorithm
robot_nav_2d_msgs::Twist2D prevous_drive_cmd_;
double x_direction_, y_direction_, theta_direction_;
double max_robot_pose_search_dist_;
double global_plan_prune_distance_{1.0};
// Lookahead
bool use_velocity_scaled_lookahead_dist_;
@ -249,6 +249,7 @@ namespace mkt_algorithm
double lookahead_dist_;
double min_lookahead_dist_;
double max_lookahead_dist_;
double max_lateral_accel_;
// journey
double min_journey_squared_{1e9};
@ -265,6 +266,7 @@ namespace mkt_algorithm
bool use_regulated_linear_velocity_scaling_;
double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_;
double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_;
double min_speed_xy_, max_speed_xy_;
double rot_stopped_velocity_, trans_stopped_velocity_;
@ -276,10 +278,6 @@ namespace mkt_algorithm
double inflation_cost_scaling_factor_;
double cost_scaling_dist_, cost_scaling_gain_;
double cost_left_goal_, cost_right_goal_;
double cost_left_side_ , cost_right_side_;
double center_cost_;
// Control frequency
double control_duration_;
std::vector<robot_geometry_msgs::Point> footprint_spec_;

314
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory_.h
View File

@ -1,314 +0,0 @@
#ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
#define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
#include <robot/robot.h>
#include <score_algorithm/score_algorithm.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PointStamped.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_core2/costmap.h>
#include <nav_grid/coordinate_conversion.h>
#include <angles/angles.h>
#include <robot_nav_msgs/Path.h>
#include <kalman/kalman.h>
#include <vector>
#include <robot_nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/conversions.h>
namespace mkt_algorithm
{
namespace diff
{
/**
* @class PredictiveTrajectory
* @brief Standard PredictiveTrajectory-like ScoreAlgorithm
*/
class PredictiveTrajectory : public score_algorithm::ScoreAlgorithm
{
public:
PredictiveTrajectory() : initialized_(false), nav_stop_(false) {};
virtual ~PredictiveTrajectory();
// Standard ScoreAlgorithm Interface
/**
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
*
* Subclasses may overwrite. Return false in case there is any error.
*
* @param pose Current pose (costmap frame)
* @param velocity Current velocity
* @param goal The final goal (costmap frame)
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
*/
virtual bool prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity,
const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan,
double &x_direction, double &y_direction, double &theta_direction) override;
/**
* @brief Calculating algorithm
* @param pose
* @param velocity
* @param traj
*/
virtual mkt_msgs::Trajectory2D calculator(
const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override;
/**
* @brief Reset all data
*/
virtual void reset() override;
/**
* @brief Stoping move navigation
*/
virtual void stop() override;
/**
* @brief resume move navigation after stopped
*/
virtual void resume() override;
/**
* @brief Create a new PredictiveTrajectory instance
* @return A pointer to the new PredictiveTrajectory instance
*/
static score_algorithm::ScoreAlgorithm::Ptr create();
protected:
inline double sign(double x)
{
return x < 0.0 ? -1.0 : 1.0;
}
/**
* @brief Initialize parameters
*/
virtual void getParams();
/**
* @brief Dynamically adjust look ahead distance based on the speed
* @param velocity
* @return look ahead distance
*/
double getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity);
/**
* @brief Get lookahead point on the global plan
* @param lookahead_dist
* @param global_plan
* @return lookahead point
*/
std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator
getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan);
/**
* @brief Prune global plan
* @param tf
* @param pose
* @param global_plan
* @param dist_behind_robot
* @return true if plan is pruned, false otherwise
*/
bool pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose,
robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot);
/**
* @brief Transforms the global plan of the robot from the planner frame to the local frame (modified).
*
* The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h
* such that the index of the current goal pose is returned as well as
* the transformation between the global plan and the planning frame.
* @param tf A reference to a tf buffer
* @param global_plan The plan to be transformed
* @param pose The pose of the robot
* @param costmap A reference to the costmap being used so the window size for transforming can be computed
* @param global_frame The frame to transform the plan to
* @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
* @param[out] transformed_plan Populated with the transformed plan
* @return \c true if the global plan is transformed, \c false otherwise
*/
bool transformGlobalPlan(
TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
robot_nav_2d_msgs::Path2D &transformed_plan);
robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose);
/**
* @brief Should rotate to path
* @param carrot_pose
* @param angle_to_path
* @return true if should rotate, false otherwise
*/
bool shouldRotateToPath(
const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x);
/**
* @brief Rotate to heading
* @param angle_to_path
* @param velocity The velocity of the robot
* @param cmd_vel The velocity commands to be filled
*/
void rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel);
/**
* @brief the robot is moving acceleration limits
* @param velocity The velocity of the robot
* @param cmd_vel The velocity commands
* @param cmd_vel_result The velocity commands result
*/
void moveWithAccLimits(
const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result);
/**
* @brief Stop the robot taking into account acceleration limits
* @param pose The pose of the robot in the global frame
* @param velocity The velocity of the robot
* @param cmd_vel The velocity commands to be filled
* @return True if a valid trajectory was found, false otherwise
*/
bool stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel);
/**
* @brief Apply constraints
* @param dist_error
* @param lookahead_dist
* @param curvature
* @param curr_speed
* @param pose_cost
* @param linear_vel
* @param sign
*/
void applyConstraints(
const double &dist_error, const double &lookahead_dist,
const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed,
const double &pose_cost, double &linear_vel, const double &sign_x);
void computePurePursuit(
const score_algorithm::TrajectoryGenerator::Ptr &traj,
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const robot_nav_2d_msgs::Twist2D &velocity,
const double &min_approach_linear_velocity,
const double &journey_plan,
const double &sign_x,
const double &lookahead_dist_min,
const double &lookahead_dist_max,
const double &lookahead_dist,
const double &lookahead_time,
const double &dt,
robot_nav_2d_msgs::Twist2D &drive_cmd
);
double adjustSpeedWithHermiteTrajectory(
const robot_nav_2d_msgs::Twist2D &velocity,
const robot_nav_2d_msgs::Path2D &trajectory,
double v_target,
const double &sign_x
);
std::vector<robot_geometry_msgs::Point> interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution);
/**
* @brief Cost at pose
* @param x
* @param y
* @return cost
*/
double costAtPose(const double &x, const double &y);
double computeDecelerationFactor(double remaining_distance, double decel_distance);
double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan);
double estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan);
std::vector<double> angle_history_;
size_t window_size_;
bool initialized_;
bool nav_stop_;
robot::NodeHandle nh_, nh_priv_;
std::string frame_id_path_;
std::string robot_base_frame_;
robot_nav_2d_msgs::Pose2DStamped goal_;
robot_nav_2d_msgs::Path2D global_plan_;
robot_nav_2d_msgs::Path2D compute_plan_;
robot_nav_2d_msgs::Path2D transform_plan_;
robot_nav_2d_msgs::Twist2D prevous_drive_cmd_;
double x_direction_, y_direction_, theta_direction_;
double max_robot_pose_search_dist_;
double global_plan_prune_distance_{1.0};
// Lookahead
bool use_velocity_scaled_lookahead_dist_;
double lookahead_time_;
double lookahead_dist_;
double min_lookahead_dist_;
double max_lookahead_dist_;
double max_lateral_accel_;
// journey
double min_journey_squared_{1e9};
double max_journey_squared_{1e9};
// Rotate to heading
bool use_rotate_to_heading_;
double rotate_to_heading_min_angle_;
double max_vel_theta_, min_vel_theta_, acc_lim_theta_, decel_lim_theta_;
double min_path_distance_, max_path_distance_;
// Regulated linear velocity scaling
bool use_regulated_linear_velocity_scaling_;
double max_vel_x_, min_vel_x_, acc_lim_x_, decel_lim_x_;
double max_vel_y_, min_vel_y_, acc_lim_y_, decel_lim_y_;
double rot_stopped_velocity_, trans_stopped_velocity_;
double min_approach_linear_velocity_;
double regulated_linear_scaling_min_radius_;
double regulated_linear_scaling_min_speed_;
bool use_cost_regulated_linear_velocity_scaling_;
double inflation_cost_scaling_factor_;
double cost_scaling_dist_, cost_scaling_gain_;
double cost_left_goal_, cost_right_goal_;
double cost_left_side_ , cost_right_side_;
double center_cost_;
// Control frequency
double control_duration_;
std::vector<robot_geometry_msgs::Point> footprint_spec_;
robot::Time last_actuator_update_;
boost::shared_ptr<KalmanFilter> kf_;
int m_, n_;
Eigen::MatrixXd A;
Eigen::MatrixXd C;
Eigen::MatrixXd Q;
Eigen::MatrixXd R;
Eigen::MatrixXd P;
}; // class PredictiveTrajectory
} // namespace diff
} // namespace mkt_algorithm
#endif //_NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__

6
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h
View File

@ -59,12 +59,6 @@ namespace mkt_algorithm
*/
static score_algorithm::ScoreAlgorithm::Ptr create();
protected:
/**
* @brief Initialize parameters
*/
virtual void getParams() override;
}; // class RotateToGoalDiff
} // namespace diff

52
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h
View File

@ -1,52 +0,0 @@
#ifndef _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__
#define _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__
#include <robot/robot.h>
#include <score_algorithm/score_algorithm.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PointStamped.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_core2/costmap.h>
#include <nav_grid/coordinate_conversion.h>
#include <angles/angles.h>
namespace mkt_algorithm
{
namespace diff
{
class PurePursuit
{
public:
void computePurePursuit(
const score_algorithm::TrajectoryGenerator::Ptr &traj,
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const robot_nav_2d_msgs::Twist2D &velocity,
const double &min_approach_linear_velocity,
const double &journey_plan,
const double &sign_x,
const double &lookahead_dist_min,
const double &lookahead_dist_max,
const double &lookahead_dist,
const double &lookahead_time,
const double &dt,
robot_nav_2d_msgs::Twist2D &drive_cmd
);
private:
void applyConstraints(const double &dist_error, const double &lookahead_dist,
const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
const double &pose_cost, double &linear_vel, const double &sign_x);
double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const double &journey_plan);
double computeDecelerationFactor(const double &effective_journey, const double &d_reduce);
// properties
double max_lateral_accel_;
};
}
}
#endif

5
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
View File

@ -7,15 +7,14 @@ void mkt_algorithm::diff::GoStraight::initialize(
{
if (!initialized_)
{
nh_ = robot::NodeHandle("~");
nh_ = nh;
nh_priv_ = robot::NodeHandle(nh, name);
name_ = name;
tf_ = tf;
traj_ = traj;
costmap_robot_ = costmap_robot;
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
this->getParams();
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
if (footprint.size() > 1)

668
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
View File

@ -1,6 +1,5 @@
#include <mkt_algorithm/diff/diff_predictive_trajectory.h>
#include <mkt_algorithm/diff/diff_predictive_trajectory_.h>
#include <boost/dll/alias.hpp>
#define LIMIT_VEL_THETA 0.33
mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory() {}
@ -9,14 +8,14 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
{
if (!initialized_)
{
nh_ = robot::NodeHandle("~/");
nh_ = nh;
nh_priv_ = robot::NodeHandle(nh, name);
name_ = name;
tf_ = tf;
traj_ = traj;
costmap_robot_ = costmap_robot;
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
this->getParams();
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
if (footprint.size() > 1)
@ -105,10 +104,12 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
nh_priv_.param<bool>("use_rotate_to_heading", use_rotate_to_heading_, false);
nh_priv_.param<double>("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785);
nh_priv_.param<double>("rot_stopped_velocity", rot_stopped_velocity_, 0.0);
nh_priv_.param<double>("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<double>("rot_stopped_velocity", rot_stopped_velocity_, 0.01);
nh_priv_.param<double>("trans_stopped_velocity", trans_stopped_velocity_, 0.01);
if (trans_stopped_velocity_ < 0.02)
trans_stopped_velocity_ = 0.5 * min_approach_linear_velocity_;
if (trans_stopped_velocity_ > min_approach_linear_velocity_)
min_approach_linear_velocity_ = trans_stopped_velocity_ * 2.0;
// Regulated linear velocity scaling
nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
@ -120,6 +121,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
nh_priv_.param<double>("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0);
nh_priv_.param<double>("cost_scaling_dist", cost_scaling_dist_, 0.6);
nh_priv_.param<double>("cost_scaling_gain", cost_scaling_gain_, 1.0);
nh_priv_.param<double>("max_lateral_accel", max_lateral_accel_, 1.0);
if (inflation_cost_scaling_factor_ <= 0.0)
{
robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
@ -127,7 +129,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
}
double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
control_duration_ = 1.0 / control_frequency;
window_size_ = (size_t)(control_frequency * 3.0);
if (traj_)
{
@ -145,6 +146,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
traj_.get()->getNodeHandle().param<double>("min_vel_theta", min_vel_theta_, 0.0);
traj_.get()->getNodeHandle().param<double>("acc_lim_theta", acc_lim_theta_, 0.0);
traj_.get()->getNodeHandle().param<double>("decel_lim_theta", decel_lim_theta_, 0.0);
traj_.get()->getNodeHandle().param<double>("min_speed_xy", min_speed_xy_, 0.0);
traj_.get()->getNodeHandle().param<double>("max_speed_xy", max_speed_xy_, 0.0);
}
}
@ -284,7 +287,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
return false;
}
const auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transform_plan_);
if(fabs(carrot_pose.pose.y) > 0.2)
{
lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_))
{
robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
return false;
}
}
x_direction = x_direction_;
y_direction = y_direction_ = 0;
@ -319,7 +331,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
for (auto it = carrot_pose_it - 1; it != transform_plan_.poses.begin(); --it)
{
double dx = it->pose.x - carrot_pose_it->pose.x;
double dy = it->pose.x - carrot_pose_it->pose.y;
double dy = it->pose.y - carrot_pose_it->pose.y;
distance_it += std::hypot(dx, dy);
if (distance_it > costmap_robot_->getCostmap()->getResolution())
{
@ -373,14 +385,15 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
robot_nav_2d_msgs::Twist2D drive_cmd;
double sign_x = sign(x_direction_);
robot_nav_2d_msgs::Twist2D twist;
traj_->startNewIteration(velocity);
while (robot::ok() && traj_->hasMoreTwists())
{
twist = traj_->nextTwist();
}
drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5);
double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
drive_cmd.x = std::min(sqrt(twist.x * twist.x), fabs(v_max));
robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
if (transformed_plan.poses.empty())
{
@ -390,35 +403,16 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
prevous_drive_cmd_ = drive_cmd;
return result;
}
else
{
result.poses.clear();
result.poses.reserve(transformed_plan.poses.size());
for (const auto &pose_stamped : transformed_plan.poses)
{
result.poses.push_back(pose_stamped.pose);
}
}
double lookahead_dist = getLookAheadDistance(velocity);
double tolerance = hypot(pose.pose.x - compute_plan_.poses.front().pose.x, pose.pose.y - compute_plan_.poses.front().pose.y);
if (transformed_plan.poses.empty())
{
robot::log_warning("[%s:%d]\n Transformed plan is empty after compute lookahead point", __FILE__, __LINE__);
return result;
}
auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
bool allow_rotate = false;
nh_priv_.param("allow_rotate", allow_rotate, false);
robot_geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose;
const double distance_allow_rotate = min_journey_squared_;
const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y);
const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
const double path_distance_to_rotate = hypot(pose.pose.x - transformed_plan.poses.back().pose.x, pose.pose.y - transformed_plan.poses.back().pose.y);
allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
double angle_to_heading;
if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
{
@ -434,18 +428,30 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
}
else
{
const double vel_x_reduce = std::min(fabs(v_max), 0.3);
double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
carrot_dist2 = std::max(carrot_dist2, 0.05);
double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
double v_target = this->adjustSpeedWithHermiteTrajectory(velocity, transformed_plan, drive_cmd.x, sign_x);
robot_nav_2d_msgs::Twist2D drive_target;
drive_target.x = v_target;
transformed_plan = this->generateHermiteTrajectory(transformed_plan.poses.back());
if(fabs(carrot_pose.pose.y) > 0.2)
{
lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
}
carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
const 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 journey_plan = transformed_plan.poses.empty() ? distance_allow_rotate : journey(transformed_plan.poses, 0, transformed_plan.poses.size() - 1);
this->computePurePursuit(
carrot_pose,
drive_target,
velocity,
transformed_plan,
min_approach_linear_velocity_,
sign_x,
dt,
drive_cmd);
const double vel_x_reduce = 0.3;
const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
const double min_S = min_lookahead_dist_ + max_path_distance_, max_S = max_lookahead_dist_ + max_path_distance_;
double d_reduce = std::clamp(journey_plan, min_S, max_S);
double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
@ -458,51 +464,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
double vel_reduce = sign_x > 0
? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
: std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce;
double v_theta = drive_cmd.x * curvature;
double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_))
{
carrot_dist2 *= 0.6;
curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
v_theta = drive_cmd.x * curvature;
}
if (fabs(v_theta) > LIMIT_VEL_THETA)
{
robot_nav_2d_msgs::Twist2D cmd_vel, cmd_result;
cmd_vel.x = sign_x > 0
? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1))
: std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1));
cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5);
this->moveWithAccLimits(velocity, cmd_vel, cmd_result);
drive_cmd.x = std::copysign(cmd_result.x, sign_x);
v_theta = drive_cmd.x * curvature;
}
if (journey_plan < min_journey_squared_)
{
if (transform_plan_.poses.size() > 2)
{
robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
double dx = end.pose.x - start.pose.x;
double dy = end.pose.y - start.pose.y;
v_theta = atan2(dy, dx);
if (v_theta > M_PI_2)
v_theta -= M_PI;
else if (v_theta < -M_PI_2)
v_theta += M_PI;
// v_theta *= 0.5;
v_theta = std::clamp(v_theta, -0.02, 0.02);
}
else
v_theta = 0.0;
}
double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8;
double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt;
double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt;
drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth);
drive_cmd.x = journey_plan > d_reduce ? drive_cmd.x : vel_reduce;
robot::log_info("journey_plan: %f, max_path_distance_: %f, d_reduce: %f, vel_reduce: %f", journey_plan, max_path_distance_, d_reduce, vel_reduce);
if (this->nav_stop_)
{
@ -517,31 +480,240 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
return result;
}
Eigen::VectorXd y(2);
y << drive_cmd.x, drive_cmd.theta;
// Eigen::VectorXd y(2);
// y << drive_cmd.x, drive_cmd.theta;
// Cập nhật lại A nếu dt thay đổi
for (int i = 0; i < n_; ++i)
for (int j = 0; j < n_; ++j)
A(i, j) = (i == j ? 1.0 : 0.0);
for (int i = 0; i < n_; i += 3)
// // 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) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
// drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
}
result.poses.clear();
result.poses.reserve(transformed_plan.poses.size());
for (const auto &pose_stamped : transformed_plan.poses)
{
A(i, i + 1) = dt;
A(i, i + 2) = 0.5 * dt * dt;
A(i + 1, i + 2) = dt;
if(fabs(pose_stamped.pose.x - carrot_pose.pose.x) < 1e-6 &&
fabs(pose_stamped.pose.y - carrot_pose.pose.y) < 1e-6 &&
fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6)
break;
result.poses.push_back(pose_stamped.pose);
}
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;
}
double mkt_algorithm::diff::PredictiveTrajectory::adjustSpeedWithHermiteTrajectory(
const robot_nav_2d_msgs::Twist2D &velocity,
const robot_nav_2d_msgs::Path2D &trajectory,
double v_target,
const double &sign_x)
{
if (journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
return min_speed_xy_ * sign_x;
// Use speed-scaled preview distance to evaluate curvature
double preview_dist = std::clamp(std::fabs(velocity.x) * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_);
double traveled = 0.0;
double max_kappa = 0.0;
for (size_t i = 1; i + 1 < trajectory.poses.size(); ++i)
{
const auto &p0 = trajectory.poses[i - 1].pose;
const auto &p1 = trajectory.poses[i].pose;
const auto &p2 = trajectory.poses[i + 1].pose;
const double a = std::hypot(p1.x - p0.x, p1.y - p0.y);
const double b = std::hypot(p2.x - p1.x, p2.y - p1.y);
const double c = std::hypot(p2.x - p0.x, p2.y - p0.y);
traveled += a;
if (a > 1e-6 && b > 1e-6 && c > 1e-6)
{
const double cross = (p1.x - p0.x) * (p2.y - p0.y) - (p1.y - p0.y) * (p2.x - p0.x);
const double area2 = std::fabs(cross); // 2 * area
const double kappa = 2.0 * area2 / (a * b * c);
max_kappa = std::max(max_kappa, std::fabs(kappa));
}
if (traveled >= preview_dist)
break;
}
double v_limit = std::fabs(v_target);
if (max_kappa > 1e-6 && max_lateral_accel_ > 1e-6)
{
const double v_curve = std::sqrt(max_lateral_accel_ / max_kappa);
v_limit = std::min(v_limit, v_curve);
}
if (decel_lim_x_ > 1e-6)
{
const double remaining = journey(trajectory.poses, 0, trajectory.poses.size() - 1);
const double v_stop = std::sqrt(2.0 * decel_lim_x_ * std::max(0.0, remaining));
v_limit = std::min(v_limit, v_stop);
}
return std::copysign(v_limit, sign_x);
}
void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const robot_nav_2d_msgs::Twist2D &drive_target,
const robot_nav_2d_msgs::Twist2D &velocity,
const robot_nav_2d_msgs::Path2D &trajectory,
const double &min_approach_linear_velocity,
const double &sign_x,
const double &dt,
robot_nav_2d_msgs::Twist2D &drive_cmd)
{
// 1) Curvature from pure pursuit
const double L2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
if (L2 < 1e-6)
return;
const double kappa = 2.0 * carrot_pose.pose.y / L2;
// 3) Adjust speed using Hermite trajectory curvature + remaining distance
double v_target = adjustSpeedWithHermiteTrajectory(velocity, trajectory, drive_target.x, sign_x);
const double y_abs = std::fabs(carrot_pose.pose.y);
const double y_soft = 0.1;
if (y_abs > y_soft)
{
double scale = y_soft / y_abs; // y càng lớn => scale càng nhỏ
scale = std::clamp(scale, 0.2, 1.0); // không giảm quá sâu
v_target *= scale;
robot_nav_2d_msgs::Twist2D cmd, result;
cmd.x = v_target;
this->moveWithAccLimits(velocity, cmd, result);
v_target = result.x;
}
// 4) Maintain minimum approach speed
if (std::fabs(v_target) < min_approach_linear_velocity)
v_target = std::copysign(min_approach_linear_velocity, sign_x);
// 5) Angular speed from curvature
double w_target = v_target * kappa;
w_target = std::clamp(w_target, -max_vel_theta_, max_vel_theta_);
// 6) Apply acceleration limits (linear + angular)
const double dv = std::clamp(v_target - velocity.x, -acc_lim_x_ * dt, acc_lim_x_ * dt);
const double dw = std::clamp(w_target - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
drive_cmd.x = velocity.x + dv;
drive_cmd.theta = velocity.theta + dw;
}
double mkt_algorithm::diff::PredictiveTrajectory::estimateGoalHeading(const robot_nav_2d_msgs::Path2D &plan) {
if (plan.poses.size() < 2) return 0.0;
const auto& p1 = plan.poses[plan.poses.size() - 2];
const auto& p2 = plan.poses[plan.poses.size() - 1];
return std::atan2(p2.pose.y - p1.pose.y, p2.pose.x - p1.pose.x);
}
void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
const double &dist_error, const double &lookahead_dist,
const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
const double &pose_cost, double &linear_vel, const double &sign_x)
{
double curvature_vel = linear_vel;
double cost_vel = linear_vel;
double approach_vel = linear_vel;
if (use_regulated_linear_velocity_scaling_)
{
const double &min_rad = regulated_linear_scaling_min_radius_;
const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad;
if (radius < min_rad)
{
curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad);
robot_nav_2d_msgs::Twist2D cmd, result;
cmd.x = curvature_vel;
this->moveWithAccLimits(velocity, cmd, result);
curvature_vel = result.x;
linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_);
}
}
if (use_cost_regulated_linear_velocity_scaling_ &&
pose_cost != static_cast<double>(robot_costmap_2d::NO_INFORMATION) &&
pose_cost != static_cast<double>(robot_costmap_2d::FREE_SPACE))
{
const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius();
const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) *
std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) +
inscribed_radius;
if (min_distance_to_obstacle < cost_scaling_dist_)
{
cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_;
}
robot_nav_2d_msgs::Twist2D cmd, result;
cmd.x = cost_vel;
this->moveWithAccLimits(velocity, cmd, result);
cost_vel = result.x;
linear_vel = std::min(cost_vel, curvature_vel);
}
// ss << linear_vel << " ";
// Use the lowest of the 2 constraint heuristics, but above the minimum translational speed
// if the actual lookahead distance is shorter than requested, that means we're at the
// end of the path. We'll scale linear velocity by error to slow to a smooth stop.
// This expression is eq. to
// (1) holding time to goal, t, constant using the theoretical
// lookahead distance and proposed velocity and
// (2) using t with the actual lookahead
// distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t).
double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr
? 2.0 * costmap_robot_->getCostmap()->getResolution()
: 0.1;
if (dist_error > dist_error_limit)
{
double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0;
double unbounded_vel = approach_vel * velocity_scaling;
if (unbounded_vel < min_approach_linear_velocity_)
{
approach_vel = min_approach_linear_velocity_;
}
else
{
approach_vel *= velocity_scaling;
}
// Use the lowest velocity between approach and other constraints, if all overlapping
robot_nav_2d_msgs::Twist2D cmd, result;
cmd.x = approach_vel;
this->moveWithAccLimits(velocity, cmd, result);
approach_vel = result.x;
linear_vel = std::min(linear_vel, approach_vel);
}
// Limit linear velocities to be valid
double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x);
double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x);
double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y);
double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y);
double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y);
double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y);
double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel);
linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel);
linear_vel = sign_x * linear_vel;
}
bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x)
{
@ -574,7 +746,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
}
}
}
// ROS_INFO_THROTTLE(0.1,"path_angle %.3f %.3f", path_angle, std::atan2(carrot_pose.pose.y, carrot_pose.pose.x));
// Whether we should rotate robot to rough path heading
angle_to_path = curvature ? path_angle : std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
angle_to_path = sign_x < 0
@ -590,22 +761,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
: std::clamp(heading_rotate + heading_linear, 0.3, M_PI_2);
bool result = use_rotate_to_heading_ && (is_stopped || sign(angle_to_path) * sign_x < 0) && fabs(angle_to_path) > heading_rotate;
// if (result)
// {
// ROS_WARN_THROTTLE(0.1, "x %.3f %.3f theta %.3f %.3f",
// velocity.x, trans_stopped_velocity_, velocity.theta, max_vel_theta_ + rot_stopped_velocity_);
// ROS_WARN_THROTTLE(0.1, "%x %.3f %.3f %.3f %.3f %.3f \n",
// is_stopped, path_angle, std::atan2(carrot_pose.pose.y, carrot_pose.pose.x), angle_to_path, heading_rotate, sign_x);
// }
// else
// {
// ROS_INFO_THROTTLE(0.1, "x %.3f %.3f theta %.3f %.3f",
// velocity.x, trans_stopped_velocity_, velocity.theta, max_vel_theta_ + rot_stopped_velocity_);
// ROS_INFO_THROTTLE(0.1, "%x %.3f %.3f %.3f %.3f %.3f \n",
// is_stopped, path_angle,std::atan2(carrot_pose.pose.y, carrot_pose.pose.x) ,angle_to_path, heading_rotate, sign_x);
// }
return result;
}
@ -631,7 +786,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an
double target_angular_speed = max_vel_theta;
if (fabs(ang_diff) < angular_decel_zone)
{
// ROS_WARN_THROTTLE(0.2,"%f %f %f %f", ang_diff, angular_decel_zone, zone_begin, max_vel_theta);
double cosine_factor = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(ang_diff) / angular_decel_zone)));
double cosine_speed = max_vel_theta * cosine_factor;
@ -641,9 +795,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an
// Ensure minimum speed to avoid stalling
target_angular_speed = std::max(target_angular_speed, reduce_vel_theta);
}
// else
// ROS_INFO_THROTTLE(1,"%f %f %f %f", ang_diff, angular_decel_zone, max_vel_theta, reduce_vel_theta);
// Apply direction
double v_theta_desired = std::copysign(target_angular_speed, ang_diff);
@ -694,40 +845,40 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_
auto goal_pose_it = std::prev(global_plan.poses.end());
return goal_pose_it;
}
unsigned int goal_index = (unsigned)global_plan.poses.size() - 1;
// double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x;
// double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y;
// double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x;
// double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y;
double start_direction_x = global_plan.poses[1].pose.x - global_plan.poses[0].pose.x;
double start_direction_y = global_plan.poses[1].pose.y - global_plan.poses[0].pose.y;
double end_direction_x = global_plan.poses[goal_index].pose.x - global_plan.poses[goal_index - 1].pose.x;
double end_direction_y = global_plan.poses[goal_index].pose.y - global_plan.poses[goal_index - 1].pose.y;
// // make sure that atan2 is defined
// double start_angle = atan2(start_direction_y, start_direction_x);
// double end_angle = atan2(end_direction_y, end_direction_x);
// double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2);
// make sure that atan2 is defined
double start_angle = atan2(start_direction_y, start_direction_x);
double end_angle = atan2(end_direction_y, end_direction_x);
double permition_threshold = std::clamp(fabs(end_angle - start_angle) / 2.0, 0.32, M_PI_2);
// for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++)
// {
// if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
// {
// const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x;
// const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y;
for (unsigned int i = 1; i < (int)global_plan.poses.size(); i++)
{
if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9)
{
const double current_direction_x = global_plan.poses[i].pose.x - global_plan.poses[i - 1].pose.x;
const double current_direction_y = global_plan.poses[i].pose.y - global_plan.poses[i - 1].pose.y;
// if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
// {
// double current_angle = atan2(current_direction_y, current_direction_x);
// goal_index = i;
// if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_))
// continue;
if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9)
{
double current_angle = atan2(current_direction_y, current_direction_x);
goal_index = i;
if (fabs(velocity.x) <= trans_stopped_velocity_ && fabs(velocity.theta) >= (0.8 * min_vel_theta_))
continue;
if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold)
{
goal_index = i;
break;
}
}
}
}
// if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) >= permition_threshold)
// {
// goal_index = i;
// break;
// }
// }
// }
// }
// Find the first pose which is at a distance greater than the lookahead distance
auto goal_pose_it = std::find_if(
@ -745,16 +896,6 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_
return goal_pose_it;
}
robot_geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose)
{
robot_geometry_msgs::PointStamped carrot_msg;
carrot_msg.header = carrot_pose.header;
carrot_msg.point.x = carrot_pose.pose.x;
carrot_msg.point.y = carrot_pose.pose.y;
carrot_msg.point.z = 0.5; // publish right over map to stand out
return carrot_msg;
}
bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot)
{
if (global_plan.poses.empty())
@ -801,6 +942,70 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
return true;
}
robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose)
{
robot_nav_2d_msgs::Path2D hermite_trajectory;
hermite_trajectory.poses.clear();
hermite_trajectory.header.stamp = pose.header.stamp;
hermite_trajectory.header.frame_id = pose.header.frame_id;
// Characteristic length (can be tuned)
const double x = pose.pose.x;
const double y = pose.pose.y;
const double theta = pose.pose.theta;
double L = std::sqrt(x * x + y * y);
if (L < 1e-6) {
// Degenerate: return single pose
robot_nav_2d_msgs::Pose2DStamped pose_stamped;
pose_stamped.pose.x = 0.0;
pose_stamped.pose.y = 0.0;
pose_stamped.pose.theta = 0.0;
pose_stamped.header.stamp = pose.header.stamp;
pose_stamped.header.frame_id = pose.header.frame_id;
hermite_trajectory.poses.push_back(pose_stamped);
return hermite_trajectory;
}
int samples = L/costmap_robot_->getCostmap()->getResolution();
if (samples < 1) {
samples = 1;
}
hermite_trajectory.poses.reserve(samples + 1);
for (int i = 0; i <= samples; ++i) {
double t = static_cast<double>(i) / samples;
double t2 = t * t;
double t3 = t2 * t;
double h00 = 2 * t3 - 3 * t2 + 1;
double h10 = t3 - 2 * t2 + t;
double h01 = -2 * t3 + 3 * t2;
double h11 = t3 - t2;
// Hermite interpolation
double px = h10 * L + h01 * x + h11 * (L * std::cos(theta));
double py = h01 * y + h11 * (L * std::sin(theta));
// First derivatives for heading
double dh00 = 6 * t2 - 6 * t;
double dh10 = 3 * t2 - 4 * t + 1;
double dh01 = -6 * t2 + 6 * t;
double dh11 = 3 * t2 - 2 * t;
double dx = dh10 * L + dh01 * x + dh11 * (L * std::cos(theta));
double dy = dh01 * y + dh11 * (L * std::sin(theta));
double heading = std::atan2(dy, dx);
robot_nav_2d_msgs::Pose2DStamped pose;
pose.pose.x = px;
pose.pose.y = py;
pose.pose.theta = heading;
pose.header.stamp = hermite_trajectory.header.stamp;
pose.header.frame_id = hermite_trajectory.header.frame_id;
hermite_trajectory.poses.push_back(pose);
}
return hermite_trajectory;
}
bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
@ -976,103 +1181,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_na
return true;
}
void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
const double &dist_error, const double &lookahead_dist,
const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
const double &pose_cost, double &linear_vel, double &sign_x)
{
double curvature_vel = linear_vel;
double cost_vel = linear_vel;
double approach_vel = linear_vel;
// std::stringstream ss;
// ss << linear_vel << " ";
// limit the linear velocity by curvature
if (use_regulated_linear_velocity_scaling_)
{
const double &min_rad = regulated_linear_scaling_min_radius_;
const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad;
if (radius < min_rad)
{
curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad);
robot_nav_2d_msgs::Twist2D cmd, result;
cmd.x = curvature_vel;
this->moveWithAccLimits(velocity, cmd, result);
curvature_vel = result.x;
linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_);
}
}
// ss << linear_vel << " ";
// limit the linear velocity by proximity to obstacles
if (use_cost_regulated_linear_velocity_scaling_ &&
pose_cost != static_cast<double>(robot_costmap_2d::NO_INFORMATION) &&
pose_cost != static_cast<double>(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<robot_geometry_msgs::Point>
mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution)
{
@ -1127,24 +1235,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, co
return static_cast<double>(cost);
}
void mkt_algorithm::diff::PredictiveTrajectory::updateCostAtOffset(
TFListenerPtr tf, const std::string &robot_base_frame, const robot_nav_2d_msgs::Pose2DStamped &base_pose,
double x_offset, double y_offset, double &cost_left, double &cost_right)
{
robot_nav_2d_msgs::Pose2DStamped stamped_pose, transformed_pose;
stamped_pose = base_pose;
stamped_pose.pose.x += x_offset;
stamped_pose.pose.y += y_offset;
if (robot_nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, transformed_pose))
{
double cost = this->costAtPose(stamped_pose.pose.x, stamped_pose.pose.y);
if (transformed_pose.pose.y < 0)
cost_right = std::max(cost_right, cost);
else if (transformed_pose.pose.y > 0)
cost_left = std::max(cost_left, cost);
}
}
double mkt_algorithm::diff::PredictiveTrajectory::computeDecelerationFactor(double remaining_distance, double decel_distance)
{
@ -1173,26 +1263,6 @@ double mkt_algorithm::diff::PredictiveTrajectory::getEffectiveDistance(const rob
return journey_plan * std::max(0.0, alignment); // Only positive projection
}
bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::vector<double> &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<mkt_algorithm::diff::PredictiveTrajectory>();

1329
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory_.cpp
View File

File diff suppressed because it is too large Load Diff

74
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp
View File

@ -14,6 +14,7 @@ void mkt_algorithm::diff::RotateToGoal::initialize(
tf_ = tf;
traj_ = traj;
costmap_robot_ = costmap_robot;
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
this->getParams();
x_direction_ = y_direction_ = theta_direction_ = 0;
@ -22,68 +23,13 @@ void mkt_algorithm::diff::RotateToGoal::initialize(
}
}
void mkt_algorithm::diff::RotateToGoal::getParams()
{
robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
nh_priv_.param<double>("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
nh_priv_.param<double>("angle_threshold", angle_threshold_, M_PI / 8);
nh_priv_.param<int>("index_samples", index_samples_, 0);
nh_priv_.param<bool>("follow_step_path", follow_step_path_, false);
nh_priv_.param<bool>("use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_, false);
nh_priv_.param<double>("lookahead_dist", lookahead_dist_, 0.25);
nh_priv_.param<double>("min_lookahead_dist", min_lookahead_dist_, 0.3);
nh_priv_.param<double>("max_lookahead_dist", max_lookahead_dist_, 0.9);
nh_priv_.param<double>("lookahead_time", lookahead_time_, 1.5);
nh_priv_.param<double>("min_journey_squared", min_journey_squared_, std::numeric_limits<double>::infinity());
nh_priv_.param<double>("max_journey_squared", max_journey_squared_, std::numeric_limits<double>::infinity());
nh_priv_.param<bool>("use_rotate_to_heading", use_rotate_to_heading_, false);
nh_priv_.param<double>("rotate_to_heading_min_angle", rotate_to_heading_min_angle_, 0.785);
nh_priv_.param<double>("rot_stopped_velocity", rot_stopped_velocity_, 0.0);
nh_priv_.param<double>("trans_stopped_velocity", trans_stopped_velocity_, 0.0);
nh_priv_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
// Regulated linear velocity scaling
nh_priv_.param<bool>("use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_, false);
nh_priv_.param<double>("regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_, 0.9);
nh_priv_.param<double>("regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_, 0.25);
// Inflation cost scaling (Limit velocity by proximity to obstacles)
nh_priv_.param<bool>("use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_, true);
nh_priv_.param<double>("inflation_cost_scaling_factor", inflation_cost_scaling_factor_, 3.0);
nh_priv_.param<double>("cost_scaling_dist", cost_scaling_dist_, 0.6);
nh_priv_.param<double>("cost_scaling_gain", cost_scaling_gain_, 1.0);
if (inflation_cost_scaling_factor_ <= 0.0)
{
robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
use_cost_regulated_linear_velocity_scaling_ = false;
}
double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
control_duration_ = 1.0 / control_frequency;
if (traj_)
{
traj_.get()->getNodeHandle().param<double>("max_vel_x", max_vel_x_, 0.0);
traj_.get()->getNodeHandle().param<double>("min_vel_x", min_vel_x_, 0.0);
traj_.get()->getNodeHandle().param<double>("acc_lim_x", acc_lim_x_, 0.0);
traj_.get()->getNodeHandle().param<double>("decel_lim_x", decel_lim_x_, 0.0);
traj_.get()->getNodeHandle().param<double>("max_vel_y", max_vel_y_, 0.0);
traj_.get()->getNodeHandle().param<double>("min_vel_y", min_vel_y_, 0.0);
traj_.get()->getNodeHandle().param<double>("acc_lim_y", acc_lim_y_, 0.0);
traj_.get()->getNodeHandle().param<double>("decel_lim_y", decel_lim_y_, 0.0);
traj_.get()->getNodeHandle().param<double>("max_vel_theta", max_vel_theta_, 0.0);
traj_.get()->getNodeHandle().param<double>("min_vel_theta", min_vel_theta_, 0.0);
traj_.get()->getNodeHandle().param<double>("acc_lim_theta", acc_lim_theta_, 0.0);
traj_.get()->getNodeHandle().param<double>("decel_lim_theta", decel_lim_theta_, 0.0);
}
}
void mkt_algorithm::diff::RotateToGoal::reset()
{
if (initialized_)
{
this->clear();
x_direction_ = y_direction_ = theta_direction_ = 0;
}
}
bool mkt_algorithm::diff::RotateToGoal::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity,
@ -96,13 +42,17 @@ bool mkt_algorithm::diff::RotateToGoal::prepare(const robot_nav_2d_msgs::Pose2DS
robot::log_error("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2)
{
robot::log_error("[%s:%d]\n The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size());
return false;
}
this->getParams();
global_plan_ = global_plan;
frame_id_path_ = global_plan.header.frame_id;
goal_ = goal;
double angle = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta);
x_direction = y_direction = theta_direction = sign(angle);
;
return true;
}

183
src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp
View File

@ -1,183 +0,0 @@
#include <mkt_algorithm/diff/pure_pursuit.h>
void mkt_algorithm::diff::PurePursuit::computePurePursuit(
const score_algorithm::TrajectoryGenerator::Ptr &traj,
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const robot_nav_2d_msgs::Twist2D &velocity,
const double &min_approach_linear_velocity,
const double &journey_plan,
const double &sign_x,
const double &lookahead_dist_min,
const double &lookahead_dist_max,
const double &lookahead_dist,
const double &lookahead_time,
const double &dt,
robot_nav_2d_msgs::Twist2D &drive_cmd)
{
if (!traj)
return;
const double velocity_max = sign_x > 0 ? traj->getTwistLinear(true).x : traj->getTwistLinear(false).x;
const double vel_x_reduce = std::min(fabs(velocity_max), 0.3);
double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
carrot_dist2 = std::max(carrot_dist2, 0.05);
double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
if (max_lateral_accel_ > 1e-6)
{
const double curvature_abs = std::max(fabs(curvature), 1e-6);
const double v_lateral_limit = sqrt(max_lateral_accel_ / curvature_abs);
drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit);
}
double post_cost = 0.0;
double distance_error = 0.0;
robot_nav_2d_msgs::Twist2D twist = traj->getTwist();
this->applyConstraints(distance_error, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
double d_reduce = std::clamp(journey_plan, lookahead_dist_min, lookahead_dist_max);
double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
double v_min =
journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity) * cosine_factor_begin_reduce + min_approach_linear_velocity;
v_min *= sign_x;
double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
double vel_reduce = sign_x > 0
? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
: std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
drive_cmd.x = (journey_plan) >= d_reduce ? drive_cmd.x : vel_reduce;
double v_theta = drive_cmd.x * curvature;
double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
carrot_dist2 *= 0.6;
curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
v_theta = drive_cmd.x * curvature;
double limit_acc_theta = fabs(v_theta) > 0.15 ? 1.0 : 1.8;
double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt;
double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt;
drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth);
}
void mkt_algorithm::diff::PurePursuit::applyConstraints(
const double &dist_error, const double &lookahead_dist,
const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
const double &pose_cost, double &linear_vel, const double &sign_x)
{
double curvature_vel = linear_vel;
double cost_vel = linear_vel;
double approach_vel = linear_vel;
if (use_regulated_linear_velocity_scaling_)
{
const double &min_rad = regulated_linear_scaling_min_radius_;
const double radius = curvature > 1e-9 ? fabs(1.0 / curvature) : min_rad;
if (radius < min_rad)
{
curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad);
robot_nav_2d_msgs::Twist2D cmd, result;
cmd.x = curvature_vel;
this->moveWithAccLimits(velocity, cmd, result);
curvature_vel = result.x;
linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_);
}
}
if (use_cost_regulated_linear_velocity_scaling_ &&
pose_cost != static_cast<double>(robot_costmap_2d::NO_INFORMATION) &&
pose_cost != static_cast<double>(robot_costmap_2d::FREE_SPACE))
{
const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius();
const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) *
std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) +
inscribed_radius;
if (min_distance_to_obstacle < cost_scaling_dist_)
{
cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_;
}
robot_nav_2d_msgs::Twist2D cmd, result;
cmd.x = cost_vel;
this->moveWithAccLimits(velocity, cmd, result);
cost_vel = result.x;
linear_vel = std::min(cost_vel, curvature_vel);
}
// ss << linear_vel << " ";
// Use the lowest of the 2 constraint heuristics, but above the minimum translational speed
// if the actual lookahead distance is shorter than requested, that means we're at the
// end of the path. We'll scale linear velocity by error to slow to a smooth stop.
// This expression is eq. to
// (1) holding time to goal, t, constant using the theoretical
// lookahead distance and proposed velocity and
// (2) using t with the actual lookahead
// distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t).
double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr
? 2.0 * costmap_robot_->getCostmap()->getResolution()
: 0.1;
if (dist_error > dist_error_limit)
{
double velocity_scaling = lookahead_dist > 1e-9 ? 1.0 - (dist_error / lookahead_dist) : 1.0;
double unbounded_vel = approach_vel * velocity_scaling;
if (unbounded_vel < min_approach_linear_velocity_)
{
approach_vel = min_approach_linear_velocity_;
}
else
{
approach_vel *= velocity_scaling;
}
// Use the lowest velocity between approach and other constraints, if all overlapping
robot_nav_2d_msgs::Twist2D cmd, result;
cmd.x = approach_vel;
this->moveWithAccLimits(velocity, cmd, result);
approach_vel = result.x;
linear_vel = std::min(linear_vel, approach_vel);
}
// Limit linear velocities to be valid
double min_vel_x = traj_ ? min_vel_x_ : fabs(traj_->getTwistLinear(false).x);
double max_vel_x = traj_ ? max_vel_x_ : fabs(traj_->getTwistLinear(true).x);
double min_vel_y = traj_ ? min_vel_y_ : fabs(traj_->getTwistLinear(false).y);
double max_vel_y = traj_ ? max_vel_y_ : fabs(traj_->getTwistLinear(true).y);
double max_linear_vel = sqrt(max_vel_x * max_vel_x + max_vel_y * max_vel_y);
double min_linear_vel = sqrt(min_vel_x * min_vel_x + min_vel_y * min_vel_y);
double desired_linear_vel = sign_x > 0 ? fabs(max_linear_vel) : fabs(min_linear_vel);
linear_vel = std::clamp(fabs(linear_vel), min_approach_linear_velocity_, desired_linear_vel);
linear_vel = sign_x * linear_vel;
}
double mkt_algorithm::diff::PurePursuit::getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
double journey_plan)
{
double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x +
carrot_pose.pose.y * carrot_pose.pose.y);
// Avoid division by zero and handle backward motion
if (carrot_distance < 1e-3)
return journey_plan;
// Project remaining path onto carrot direction
double alignment = fabs(carrot_pose.pose.x / carrot_distance); // cos(angle)
return journey_plan * std::max(0.0, alignment); // Only positive projection
}
double mkt_algorithm::diff::PurePursuit::computeDecelerationFactor(double remaining_distance, double decel_distance)
{
if (remaining_distance >= decel_distance)
{
return 1.0; // Full speed
}
// Smooth transition using cosine function
double ratio = fabs(remaining_distance / decel_distance);
return 0.5 * (1.0 + cos(M_PI * (1.0 - ratio))); // Smooth from 1 to 0
}

26
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
View File

@ -120,7 +120,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
if (!rotate_algorithm_)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create rotate algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
// exit(1);
exit(1);
}
rotate_algorithm_->initialize(parent_, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
robot::log_info_at(__FILE__, __LINE__, "Successfully initialized rotate algorithm \"%s\"", algorithm_rotate_name.c_str());
@ -128,11 +128,12 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
catch (const std::exception &ex)
{
robot::log_error_at(__FILE__, __LINE__, "Failed to create the %s rotate algorithm, are you sure it is properly registered and that the containing library is built? Exception: %s", algorithm_rotate_name.c_str(), ex.what());
exit(1);
// exit(1);
}
std::string goal_checker_name;
planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
robot::log_info_at(__FILE__, __LINE__, "goal_checker_name: %s", goal_checker_name.c_str());
try
{
robot::PluginLoaderHelper loader;
@ -153,6 +154,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
robot::log_error_at(__FILE__, __LINE__, "Unexpected exception while creating goal checker \"%s\": %s", goal_checker_name.c_str(), ex.what());
exit(1);
}
this->initializeOthers();
robot::log_info("[%s:%d]\n %s is sucessed", __FILE__, __LINE__, name.c_str());
initialized_ = true;
@ -181,10 +183,10 @@ void pnkx_local_planner::PNKXLocalPlanner::reset()
{
robot::log_info_at(__FILE__, __LINE__, "New Goal Received.");
this->unlock();
traj_generator_->reset();
goal_checker_->reset();
nav_algorithm_->reset();
rotate_algorithm_->reset();
if(traj_generator_) traj_generator_->reset();
if(goal_checker_) goal_checker_->reset();
if(nav_algorithm_) nav_algorithm_->reset();
if(rotate_algorithm_) rotate_algorithm_->reset();
ret_nav_ = ret_angle_ = false;
}
@ -214,8 +216,6 @@ void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &pa
{
return;
}
// robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(local_plan_.poses.front());
// pnkx_local_planner::transformGlobalPlan(tf_, local_plan_, local_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, path);
path = local_plan_;
}
@ -250,7 +250,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
double x_direction, y_direction, theta_direction;
if (!ret_nav_)
{
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
if (nav_algorithm_ && !nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@ -258,7 +258,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
}
else
{
if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
if (rotate_algorithm_ && !rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@ -269,7 +269,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
xytheta_direct[0] = x_direction != 0 ? fabs(x_direction) / x_direction : 0;
xytheta_direct[1] = y_direction != 0 ? fabs(y_direction) / y_direction : 0;
xytheta_direct[2] = theta_direction != 0 ? fabs(theta_direction) / theta_direction : 0;
traj_generator_->setDirect(xytheta_direct);
if(traj_generator_) traj_generator_->setDirect(xytheta_direct);
}
robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose,
@ -307,14 +307,14 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg
}
else if (!ret_nav_)
{
traj = nav_algorithm_->calculator(pose, velocity);
if(nav_algorithm_) traj = nav_algorithm_->calculator(pose, velocity);
local_plan_.header.stamp = robot::Time::now();
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
local_plan_ = robot_nav_2d_utils::pathToPath(path);
}
else
{
traj = rotate_algorithm_->calculator(pose, velocity);
if(rotate_algorithm_) traj = rotate_algorithm_->calculator(pose, velocity);
local_plan_.header.stamp = robot::Time::now();
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getBaseFrameID(), robot::Time::now());
local_plan_ = robot_nav_2d_utils::pathToPath(path);