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

update 4/2

Browse Source
This commit is contained in:
HiepLM 2026-02-04 09:31:56 +07:00
parent 15f842d703
commit 7b3c5b8d5f
9 changed files with 233 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
config/pnkx_local_planner_params.yaml
View File

@ -51,7 +51,7 @@ LimitedAccelGenerator:
min_speed_xy: 0.25 # min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity
max_vel_theta: 0.7 # max_rot_vel: 1.0 # choose slightly less than the base's capability
min_vel_theta: 0.07 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity
min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity
acc_lim_x: 1.0
acc_lim_y: 0.0 # diff drive robot
@ -102,17 +102,11 @@ MKTAlgorithmDiffPredictiveTrajectory:
rot_stopped_velocity: 0.05
trans_stopped_velocity: 0.06
# Regulated linear velocity scaling
use_regulated_linear_velocity_scaling: false # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
# only when true:
regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
regulated_linear_scaling_min_speed: 0.15 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
# Inflation cost scaling (Limit velocity by proximity to obstacles)
use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0
cost_scaling_dist: 0.2 # (default: 0.6)
cost_scaling_gain: 2.0 # (default: 1.0)
use_final_heading_alignment: true
final_heading_xy_tolerance: 0.1
final_heading_angle_tolerance: 0.05
final_heading_min_velocity: 0.05
final_heading_kp_angular: 2.0
MKTAlgorithmDiffGoStraight:
library_path: libmkt_algorithm_diff

4
src/Algorithms/Cores/score_algorithm/CMakeLists.txt
View File

@ -75,6 +75,10 @@ add_library(${PROJECT_NAME} SHARED
if(BUILDING_WITH_CATKIN)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_compile_definitions(${PROJECT_NAME}
PUBLIC BUILD_WITH_ROS
)
target_include_directories(${PROJECT_NAME}
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

4
src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h
View File

@ -1,8 +1,10 @@
#ifndef _SCORE_ALGORITHM_H_INCLUDED__
#define _SCORE_ALGORITHM_H_INCLUDED__
#ifdef BUILD_WITH_ROS
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#endif
#include <robot/robot.h>
#include <iostream>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
@ -155,8 +157,10 @@ namespace score_algorithm
double old_xy_goal_tolerance_;
double angle_threshold_;
#ifdef BUILD_WITH_ROS
ros::Publisher current_goal_pub_;
ros::Publisher closet_robot_goal_pub_;
#endif
std::vector<robot_nav_2d_msgs::Pose2DStamped> reached_intermediate_goals_;
std::vector<unsigned int> start_index_saved_vt_;

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

@ -415,6 +415,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
robot_nav_2d_msgs::Pose2DStamped sub_pose;
sub_pose = global_plan.poses[closet_index];
#ifdef SCORE_ALGORITHM_WITH_ROS
{
robot_geometry_msgs::PoseStamped sub_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_pose);
geometry_msgs::PoseStamped sub_pose_stamped_ros;
@ -429,9 +430,11 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
sub_pose_stamped_ros.pose.orientation.w = sub_pose_stamped.pose.orientation.w;
closet_robot_goal_pub_.publish(sub_pose_stamped_ros);
}
#endif
robot_nav_2d_msgs::Pose2DStamped sub_goal;
sub_goal = global_plan.poses[goal_index];
#ifdef SCORE_ALGORITHM_WITH_ROS
{
robot_geometry_msgs::PoseStamped sub_goal_stamped = robot_nav_2d_utils::pose2DToPoseStamped(sub_goal);
geometry_msgs::PoseStamped sub_goal_stamped_ros;
@ -446,6 +449,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
sub_goal_stamped_ros.pose.orientation.w = sub_goal_stamped.pose.orientation.w;
current_goal_pub_.publish(sub_goal_stamped_ros);
}
#endif
result.header = global_plan.header;
for (int i = closet_index; i <= goal_index; i++)

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

@ -210,7 +210,7 @@ namespace mkt_algorithm
* @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);
void rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel, const double &dt);
/**
* @brief Check if robot should align to final heading (near goal position)
@ -226,7 +226,8 @@ namespace mkt_algorithm
const robot_nav_2d_msgs::Pose2DStamped &goal,
const robot_nav_2d_msgs::Twist2D &velocity,
double &xy_error,
double &heading_error);
double &heading_error,
const double &sign_x);
/**
* @brief Arc Motion controller for final heading alignment
@ -251,18 +252,20 @@ namespace mkt_algorithm
* @param velocity The velocity of the robot
* @param cmd_vel The velocity commands
* @param cmd_vel_result The velocity commands result
* @param dt The time step
*/
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);
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);
/**
* @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
* @param dt The time step
* @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);
bool stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel, const double &dt);
/**
* @brief Compute pure pursuit
@ -285,6 +288,21 @@ namespace mkt_algorithm
const double &dt,
robot_nav_2d_msgs::Twist2D &drive_cmd);
/**
* @brief Apply distance-based speed scaling and acceleration limits
* @param plan The plan to be scaled
* @param velocity Current velocity
* @param drive_cmd Input/output velocity command
* @param sign_x Direction of motion
* @param dt Time step
*/
void applyDistanceSpeedScaling(
const robot_nav_2d_msgs::Path2D &plan,
const robot_nav_2d_msgs::Twist2D &velocity,
robot_nav_2d_msgs::Twist2D &drive_cmd,
const double &sign_x,
const double &dt);
/**
* @brief Interpolate footprint
* @param pose
@ -365,8 +383,9 @@ namespace mkt_algorithm
Eigen::MatrixXd Q;
Eigen::MatrixXd R;
Eigen::MatrixXd P;
#ifdef BUILD_WITH_ROS
ros::Publisher lookahead_point_pub_;
#endif
}; // class PredictiveTrajectory

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

@ -96,7 +96,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
}
robot::Time current_time = robot::Time::now();
double dt = (current_time - last_actuator_update_).toSec();
const double dt = (current_time - last_actuator_update_).toSec();
last_actuator_update_ = current_time;
robot_nav_2d_msgs::Twist2D drive_cmd;

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

@ -77,10 +77,12 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
x0 << 0, 0, 0, 0, 0, 0;
kf_->init(robot::Time::now().toSec(), x0);
#ifdef BUILD_WITH_ROS
ros::NodeHandle nh_ros;
current_goal_pub_ = nh_ros.advertise<geometry_msgs::PoseStamped>("current_goal", 10);
closet_robot_goal_pub_ = nh_ros.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 10);
lookahead_point_pub_ = nh_ros.advertise<geometry_msgs::PoseStamped>("lookahead_point", 10);
#endif
x_direction_ = y_direction_ = theta_direction_ = 0;
this->initialized_ = true;
@ -217,6 +219,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
robot::log_error("[%s:%d]\n This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__);
return false;
}
if(robot::Time::now() - last_actuator_update_ > robot::Duration(0.5))
{
last_actuator_update_ = robot::Time::now();
}
std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
if (footprint.size() > 1)
@ -392,7 +398,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
}
robot::Time current_time = robot::Time::now();
double dt = (current_time - last_actuator_update_).toSec();
const double dt = (current_time - last_actuator_update_).toSec();
last_actuator_update_ = current_time;
robot_nav_2d_msgs::Twist2D drive_cmd;
@ -410,7 +416,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
if (transformed_plan.poses.empty())
{
robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);
stopWithAccLimits(pose, velocity, drive_cmd);
stopWithAccLimits(pose, velocity, drive_cmd, dt);
result.velocity = drive_cmd;
prevous_drive_cmd_ = drive_cmd;
return result;
@ -418,6 +424,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
double lookahead_dist = getLookAheadDistance(velocity);
auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
robot_geometry_msgs::PoseStamped carrot_pose_stamped = robot_nav_2d_utils::pose2DToPoseStamped(carrot_pose);
#ifdef BUILD_WITH_ROS
{
geometry_msgs::PoseStamped lookahead_point;
lookahead_point.header.stamp = ros::Time::now();
@ -431,6 +439,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
lookahead_point.pose.orientation.w = carrot_pose_stamped.pose.orientation.w;
lookahead_point_pub_.publish(lookahead_point);
}
#endif
bool allow_rotate = false;
nh_priv_.param("allow_rotate", allow_rotate, false);
@ -444,35 +453,37 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
{
if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_))
{
if (!stopWithAccLimits(pose, velocity, drive_cmd))
if (!stopWithAccLimits(pose, velocity, drive_cmd, dt))
return result;
}
else
{
rotateToHeading(angle_to_heading, velocity, drive_cmd);
rotateToHeading(angle_to_heading, velocity, drive_cmd, dt);
}
}
else
{
if(fabs(carrot_pose.pose.y) > 0.2)
{
lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
}
robot_nav_2d_msgs::Twist2D drive_target;
transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
// === Final Heading Alignment Check ===
double xy_error = 0.0, heading_error = 0.0;
if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error))
if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
{
// Use Arc Motion controller for final heading alignment
alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
ROS_INFO_THROTTLE(0.5, "[FinalHeadingAlign] xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w=%.3f",
xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, drive_cmd.theta);
#ifdef BUILD_WITH_ROS
ROS_INFO("xy_err=%.3f, heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
xy_error, heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
#endif
}
else
{
if(fabs(carrot_pose.pose.y) > 0.2)
{
lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
}
robot_nav_2d_msgs::Twist2D drive_target;
transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
// Normal Pure Pursuit
this->computePurePursuit(
carrot_pose,
@ -484,48 +495,13 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
dt,
drive_cmd);
}
double s = compute_plan_.poses.empty()
? min_journey_squared_
: journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
double S_th = 0.1 + max_path_distance_ + fabs(velocity.x) * lookahead_time_;
double S_final = std::min(S_th * 0.2, 0.1 + max_path_distance_);
double max_speed = fabs(drive_cmd.x);
double target_speed = max_speed;
if (s < S_th)
{
double r = std::clamp(s / S_th, 0.0, 1.0);
double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
target_speed = max_speed * cosine_factor;
}
double reduce_speed = std::min(max_speed, min_speed_xy_);
if (s < S_final)
{
double r = std::clamp(s / S_final, 0.0, 1.0);
double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
reduce_speed = (reduce_speed - min_approach_linear_velocity_) * cosine_factor + min_approach_linear_velocity_;
reduce_speed = std::max(reduce_speed, min_approach_linear_velocity_);
}
target_speed = std::max(target_speed, reduce_speed);
double v_desired = std::copysign(target_speed, sign_x);
double max_acc_vel = velocity.x + fabs(acc_lim_x_) * dt;
double min_acc_vel = velocity.x - fabs(acc_lim_x_) * dt;
double v_samp = std::clamp(v_desired, min_acc_vel, max_acc_vel);
double max_speed_to_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, s));
v_samp = std::copysign(std::min(max_speed_to_stop, fabs(v_samp)), sign_x);
if (sign_x > 0.0)
drive_cmd.x = std::clamp(v_samp, min_approach_linear_velocity_, max_speed);
else
drive_cmd.x = std::clamp(v_samp, -max_speed, -min_approach_linear_velocity_);
applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
if (this->nav_stop_)
{
if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
{
if (!stopWithAccLimits(pose, velocity, drive_cmd))
if (!stopWithAccLimits(pose, velocity, drive_cmd, dt))
return result;
}
else
@ -534,24 +510,24 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
return result;
}
// Eigen::VectorXd y(2);
// y << drive_cmd.x, drive_cmd.theta;
Eigen::VectorXd y(2);
y << drive_cmd.x, drive_cmd.theta;
// // Cập nhật lại A nếu dt thay đổi
// for (int i = 0; i < n_; ++i)
// for (int j = 0; j < n_; ++j)
// A(i, j) = (i == j ? 1.0 : 0.0);
// for (int i = 0; i < n_; i += 3)
// {
// A(i, i + 1) = dt;
// A(i, i + 2) = 0.5 * dt * dt;
// A(i + 1, i + 2) = dt;
// }
// kf_->update(y, dt, A);
// 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_);
// 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);
double v_min = min_approach_linear_velocity_;
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();
@ -564,7 +540,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
fabs(pose_stamped.pose.theta - carrot_pose.pose.theta) < 1e-6)
break;
}
result.velocity = drive_cmd;
prevous_drive_cmd_ = drive_cmd;
return result;
@ -603,7 +579,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
v_target *= scale;
robot_nav_2d_msgs::Twist2D cmd, result;
cmd.x = v_target;
this->moveWithAccLimits(velocity, cmd, result);
this->moveWithAccLimits(velocity, cmd, result, dt);
v_target = result.x;
}
@ -617,9 +593,21 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
{
const double k_heading = 0.3;
if (trajectory.poses.size() >= 2) {
const auto& p1 = trajectory.poses[trajectory.poses.size() - 2].pose;
const auto& p2 = trajectory.poses[trajectory.poses.size() - 1].pose;
double heading_ref = std::atan2(p2.y - p1.y, p2.x - p1.x);
const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
double heading_ref = 0.0;
for(int i = trajectory.poses.size() - 2; i >= 0; i--)
{
const auto& p = trajectory.poses[i].pose;
if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution())
{
heading_ref = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x));
if(sign_x < 0.0)
{
heading_ref = angles::normalize_angle(M_PI + heading_ref);
}
break;
}
}
// robot heading in base_link = 0, so error = heading_ref
double w_heading = k_heading * angles::normalize_angle(heading_ref);
@ -639,24 +627,80 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
drive_cmd.x = velocity.x + dv;
drive_cmd.theta = velocity.theta + dw;
robot::log_info("v_target: %f, w_target: %f, dv: %f, dw: %f, drive_cmd.x: %f, drive_cmd.theta: %f", v_target, w_target, dv, dw, drive_cmd.x, drive_cmd.theta);
}
void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
const robot_nav_2d_msgs::Path2D &plan,
const robot_nav_2d_msgs::Twist2D &velocity,
robot_nav_2d_msgs::Twist2D &drive_cmd,
const double &sign_x,
const double &dt)
{
double s = plan.poses.empty()
? min_journey_squared_
: journey(plan.poses, 0, plan.poses.size() - 1);
double S_th = 0.1 + max_path_distance_ + fabs(velocity.x) * lookahead_time_;
double S_final = std::min(S_th * 0.2, 0.1 + max_path_distance_);
double max_speed = fabs(drive_cmd.x);
double target_speed = max_speed;
if (s < S_th)
{
double r = std::clamp(s / S_th, 0.0, 1.0);
double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
target_speed = max_speed * cosine_factor;
}
double reduce_speed = std::min(max_speed, min_speed_xy_);
if (s < S_final)
{
double r = std::clamp(s / S_final, 0.0, 1.0);
double cosine_factor = 0.5 * (1.0 + std::cos(M_PI * (1.0 - r)));
reduce_speed = (reduce_speed - min_approach_linear_velocity_) * cosine_factor + min_approach_linear_velocity_;
reduce_speed = std::max(reduce_speed, min_approach_linear_velocity_);
}
target_speed = std::max(target_speed, reduce_speed);
double v_desired = std::copysign(target_speed, sign_x);
double max_acc_vel = velocity.x + fabs(acc_lim_x_) * dt;
double min_acc_vel = velocity.x - fabs(acc_lim_x_) * dt;
double v_samp = std::clamp(v_desired, min_acc_vel, max_acc_vel);
double max_speed_to_stop = std::sqrt(2.0 * fabs(decel_lim_x_) * std::max(0.0, s));
v_samp = std::copysign(std::min(max_speed_to_stop, fabs(v_samp)), sign_x);
if (sign_x > 0.0)
drive_cmd.x = std::clamp(v_samp, min_approach_linear_velocity_, max_speed);
else
drive_cmd.x = std::clamp(v_samp, -max_speed, -min_approach_linear_velocity_);
}
bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
robot_nav_2d_msgs::Twist2D velocity, double &angle_to_path, const double &sign_x)
{
bool curvature = false;
bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_);
double max_kappa = calculateMaxKappa(global_plan);
const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
curvature = max_kappa > straight_threshold;
const bool is_stopped = stopped(velocity, max_vel_theta_ + rot_stopped_velocity_ + 0.01, trans_stopped_velocity_ + 0.01);
// const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
// const double max_kappa = calculateMaxKappa(global_plan);
// const bool curvature = max_kappa > straight_threshold;
double path_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
if(is_stopped && global_plan.poses.size() >= 2)
{
const auto& p1 = global_plan.poses[1];
for(int i = 2; i < global_plan.poses.size(); i++)
{
const auto& p = global_plan.poses[i];
if(std::hypot(p.pose.x, p.pose.y) > costmap_robot_->getCostmap()->getResolution())
{
path_angle = std::atan2(p.pose.y - p1.pose.y, p.pose.x - p1.pose.x);
break;
}
}
}
// Whether we should rotate robot to rough path heading
double blend = 0.85; // Tỉ lệ blend
angle_to_path = curvature ? blend * path_angle : path_angle;
angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + angle_to_path) : angle_to_path;
angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : path_angle;
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_;
@ -665,10 +709,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
? rotate_to_heading_min_angle_ + heading_linear
: std::clamp(heading_rotate + heading_linear, angle_threshold_ + rotate_to_heading_min_angle_, M_PI_2);
bool result = use_rotate_to_heading_ &&
(is_stopped || sign(angle_to_path) * sign_x < 0 ) &&
fabs(angle_to_path) > heading_rotate;
// bool result = use_rotate_to_heading_ &&
// (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate;
bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate;
// if (result)
// ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x);
@ -680,11 +724,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
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)
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)
{
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 min_vel_theta = min_vel_theta_;
@ -715,24 +758,21 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an
// 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);
double v_theta_samp = std::copysign(
std::min(max_speed_to_stop, fabs(v_theta_desired)), ang_diff);
// === ACCELERATION LIMITS ===
double max_acc_vel = fabs(acc_lim_theta_) * dt;
double domega = std::clamp(v_theta_samp - velocity.theta, -max_acc_vel, max_acc_vel);
// === 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;
cmd_vel.theta = velocity.theta + domega;
// robot::log_info("cmd_vel.theta: %f, velocity.theta: %f, domega: %f, max_acc_vel: %f, v_theta_samp: %f", cmd_vel.theta, velocity.theta, domega, max_acc_vel, v_theta_samp);
}
bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading(
@ -740,7 +780,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading(
const robot_nav_2d_msgs::Pose2DStamped &goal,
const robot_nav_2d_msgs::Twist2D &velocity,
double &xy_error,
double &heading_error)
double &heading_error,
const double &sign_x)
{
if (!use_final_heading_alignment_)
return false;
@ -754,15 +795,23 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading(
// Transform goal heading to robot frame
// In robot frame, robot heading is 0, so heading_error = goal_heading_in_robot_frame
robot_nav_2d_msgs::Pose2DStamped goal_robot_frame;
if (tf_ && robot_nav_2d_utils::transformPose(tf_, robot_base_frame_, goal, goal_robot_frame))
heading_error = 0.0;
if(trajectory.poses.size() >= 2)
{
heading_error = angles::normalize_angle(goal_robot_frame.pose.theta);
}
else
{
// Fallback: use last pose heading in trajectory
heading_error = angles::normalize_angle(last_pose.theta);
const auto& p1 = trajectory.poses[trajectory.poses.size() - 1].pose;
for(int i = trajectory.poses.size() - 2; i >= 0; i--)
{
const auto& p = trajectory.poses[i].pose;
if(std::hypot(p1.x - p.x, p1.y - p.y) >= costmap_robot_->getCostmap()->getResolution())
{
heading_error = angles::normalize_angle(std::atan2(p1.y - p.y, p1.x - p.x));
if(sign_x < 0.0)
{
heading_error = angles::normalize_angle(M_PI + heading_error);
}
break;
}
}
}
// Check if we're close enough to goal position to start final heading alignment
@ -836,7 +885,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
{
// Ideal omega to complete rotation while traveling remaining distance
// omega = v * heading_error / xy_error
omega_arc = std::fabs(v_target) * heading_error / xy_error;
omega_arc = std::fabs(v_target - velocity.x) * heading_error / xy_error;
}
// Blend proportional and arc-based control
@ -844,13 +893,22 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
double blend_factor = std::clamp(xy_error / final_heading_xy_tolerance_, 0.0, 1.0);
omega_target = blend_factor * omega_arc + (1.0 - blend_factor) * omega_p;
// Reduce angular speed as heading error approaches zero
double heading_scale = std::clamp(
abs_heading_error / std::max(final_heading_angle_tolerance_, 1e-3),
0.0, 1.0);
heading_scale = heading_scale * heading_scale; // smoother near zero
omega_target *= heading_scale;
// Ensure minimum angular velocity for responsiveness
double omega_min = min_vel_theta_;
if (std::fabs(omega_target) < omega_min && abs_heading_error > final_heading_angle_tolerance_)
if (std::fabs(omega_target) < omega_min &&
abs_heading_error > final_heading_angle_tolerance_ * 3.0)
{
omega_target = heading_sign * omega_min;
}
}
// --- Apply acceleration limits ---
// Linear
@ -861,7 +919,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
// Angular
double omega_current = velocity.theta;
double domega_max = max_vel_theta_;
double domega_max = acc_lim_theta_ * dt;
double domega = std::clamp(omega_target - omega_current, -domega_max, domega_max);
cmd_vel.theta = omega_current + domega;
@ -878,8 +936,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::alignToFinalHeading(
cmd_vel.y = 0.0;
#ifdef BUILD_WITH_ROS
ROS_DEBUG_THROTTLE(0.2, "[FinalHeading] xy=%.3f, heading=%.3f, v=%.3f, omega=%.3f",
xy_error, heading_error, cmd_vel.x, cmd_vel.theta);
#endif
}
double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity)
@ -1400,10 +1460,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
}
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 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)
{
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);
@ -1428,11 +1486,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits(
// 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)
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, const double &dt)
{
// 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));

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

@ -17,6 +17,7 @@ void mkt_algorithm::diff::RotateToGoal::initialize(
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
this->getParams();
last_actuator_update_ = robot::Time::now();
x_direction_ = y_direction_ = theta_direction_ = 0;
this->initialized_ = true;
robot::log_info("[%s:%d]\n RotateToGoal Initialized successfully", __FILE__, __LINE__);
@ -25,10 +26,12 @@ void mkt_algorithm::diff::RotateToGoal::initialize(
void mkt_algorithm::diff::RotateToGoal::reset()
{
robot::log_info("[%s:%d]\n RotateToGoal is reset", __FILE__, __LINE__);
if (initialized_)
{
this->clear();
this->clear();
x_direction_ = y_direction_ = theta_direction_ = 0;
last_actuator_update_ = robot::Time::now();
}
}
@ -47,6 +50,10 @@ bool mkt_algorithm::diff::RotateToGoal::prepare(const robot_nav_2d_msgs::Pose2DS
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;
}
if(robot::Time::now() - last_actuator_update_ > robot::Duration(0.5))
{
last_actuator_update_ = robot::Time::now();
}
this->getParams();
frame_id_path_ = global_plan.header.frame_id;
goal_ = goal;
@ -65,10 +72,13 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::RotateToGoal::calculator(
if (!traj_)
return result;
const double dt = control_duration_;
robot::Time current_time = robot::Time::now();
const double dt = (current_time - last_actuator_update_).toSec();
last_actuator_update_ = current_time;
double vel_yaw = velocity.theta;
double ang_diff = angles::shortest_angular_distance(pose.pose.theta, goal_.pose.theta);
rotateToHeading(ang_diff, velocity, drive_cmd);
rotateToHeading(ang_diff, velocity, drive_cmd, dt);
drive_cmd.x = 0.0;
drive_cmd.y = 0.0;
result.velocity = drive_cmd;

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

@ -181,7 +181,6 @@ void pnkx_local_planner::PNKXLocalPlanner::getParams(robot::NodeHandle &nh)
void pnkx_local_planner::PNKXLocalPlanner::reset()
{
robot::log_info_at(__FILE__, __LINE__, "New Goal Received.");
this->unlock();
if(traj_generator_) traj_generator_->reset();
if(goal_checker_) goal_checker_->reset();