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

update

Browse Source
This commit is contained in:
HiepLM 2026-01-16 17:48:57 +07:00
parent ebda1f81a1
commit 3f93370462
18 changed files with 142 additions and 1769 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -153,11 +153,6 @@ namespace score_algorithm
double old_xy_goal_tolerance_;
double angle_threshold_;
bool enable_publish_;
// robot::Publisher reached_intermediate_goals_pub_;
// robot::Publisher current_goal_pub_;
// robot::Publisher closet_robot_goal_pub_;
// robot::Publisher transformed_plan_pub_, compute_plan_pub_;
std::vector<robot_nav_2d_msgs::Pose2DStamped> reached_intermediate_goals_;
std::vector<unsigned int> start_index_saved_vt_;

6
src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h
View File

@ -98,6 +98,12 @@ namespace score_algorithm
*/
virtual robot_nav_2d_msgs::Twist2D nextTwist() = 0;
/**
* @brief Get the current twist
* @return The current twist
*/
virtual robot_nav_2d_msgs::Twist2D getTwist() = 0;
/**
* @brief Get all the twists for an iteration.
*

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

@ -81,7 +81,7 @@ endif()
# Libraries
# ========================================================
add_library(${PROJECT_NAME}_diff SHARED
src/diff/diff_predictive_trajectory.cpp
src/diff/diff_predictive_trajectory_copy.cpp
src/diff/diff_rotate_to_goal.cpp
src/diff/diff_go_straight.cpp
# src/diff/pure_pursuit.cpp

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

@ -210,8 +210,6 @@ namespace mkt_algorithm
const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed,
const double &pose_cost, double &linear_vel, double &sign_x);
std::vector<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
@ -231,13 +229,8 @@ namespace mkt_algorithm
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_;
bool initialized_;
bool nav_stop_, avoid_obstacles_;
bool nav_stop_;
robot::NodeHandle nh_, nh_priv_;
std::string frame_id_path_;
std::string robot_base_frame_;
@ -285,9 +278,6 @@ namespace mkt_algorithm
double inflation_cost_scaling_factor_;
double cost_scaling_dist_, cost_scaling_gain_;
double cost_left_goal_, cost_right_goal_;
double cost_left_side_ , cost_right_side_;
double center_cost_;
// Control frequency
double control_duration_;

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

@ -23,26 +23,32 @@ namespace mkt_algorithm
PurePursuit();
~PurePursuit();
void computePurePursuit(
const double &velocity_min,
const double &velocity_max,
const double &linear_vel,
const score_algorithm::TrajectoryGenerator::Ptr &traj,
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const double &min_dist2,
const double &curvature_dist2_floor,
const double &dist2_override);
const robot_nav_2d_msgs::Twist2D &velocity,
const double &min_approach_linear_velocity,
const double &journey_plan,
const double &sign_x,
const double &lookahead_dist_min,
const double &lookahead_dist_max,
const double &lookahead_dist,
const double &lookahead_time,
robot_nav_2d_msgs::Twist2D &drive_cmd
);
private:
void applyConstraints(const double &dist_error, const double &lookahead_dist,
const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
const double &pose_cost, double &linear_vel, double &sign_x);
const double &pose_cost, double &linear_vel, const double &sign_x);
double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const double &journey_plan);
double computeDecelerationFactor(const double &effective_journey, const double &d_reduce);
bool detectWobbleByCarrotAngle(std::vector<double> &angle_history, const double &carrot_angle,
const double &amplitude_threshold, const size_t &window_size);
// properties
double max_lateral_accel_;
};
}
}

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

@ -15,7 +15,6 @@ void mkt_algorithm::diff::GoStraight::initialize(
costmap_robot_ = costmap_robot;
this->getParams();
nh_.param("publish_topic", enable_publish_, false);
nh_.param<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>();

1327
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy 2.cpp
View File

File diff suppressed because it is too large Load Diff

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

@ -1,9 +1,11 @@
#include <mkt_algorithm/diff/diff_predictive_trajectory.h>
#include <mkt_algorithm/diff/pure_pursuit.h>
#include <boost/dll/import.hpp>
#include <robot/robot.h>
#define LIMIT_VEL_THETA 0.33
void mkt_algorithm::diff::PredictiveTrajectory::initialize(
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
{
@ -100,7 +102,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter()
void mkt_algorithm::diff::PredictiveTrajectory::getParams()
{
robot_base_frame_ = nh_priv_.param<std::string>("robot_base_frame", std::string("base_link"));
nh_priv_.param<bool>("avoid_obstacles", avoid_obstacles_, false);
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);
@ -140,7 +141,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
}
double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
control_duration_ = 1.0 / control_frequency;
window_size_ = (size_t)(control_frequency * 3.0);
if (traj_)
{
@ -297,10 +297,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
robot::log_warning("[%s:%d]\n Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
return false;
}
// else
// {
// robot::log_info("[%s:%d]\n Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_);
// }
x_direction = x_direction_;
y_direction = y_direction_ = 0;
@ -350,11 +346,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
robot_geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
// teb_local_planner::PoseSE2 start_pose(front);
// teb_local_planner::PoseSE2 goal_pose(back);
// const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
const double dir_path = 0.0;
if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
x_direction = dir_path > 0 ? FORWARD : BACKWARD;
}
@ -391,15 +383,12 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
robot_nav_2d_msgs::Twist2D drive_cmd;
double sign_x = sign(x_direction_);
robot_nav_2d_msgs::Twist2D twist;
robot::log_info("[%s:%d] velocity in predictive trajectory: %f %f %f", __FILE__, __LINE__, velocity.x, velocity.y, velocity.theta);
traj_->startNewIteration(velocity);
while (robot::ok() && traj_->hasMoreTwists())
{
twist = traj_->nextTwist();
}
drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5);
robot::log_info("[%s:%d] twist: %f %f %f", __FILE__, __LINE__, twist.x, twist.y, twist.theta);
robot::log_info("--------------------------------");
double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
@ -433,12 +422,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
const double distance_allow_rotate = min_journey_squared_;
const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y);
const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
if (avoid_obstacles_)
allow_rotate = journey_plan >= distance_allow_rotate &&
fabs(front.y) <= 0.2 &&
(path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution());
else
allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
double angle_to_heading;
if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
@ -455,88 +440,24 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
}
else
{
const double vel_x_reduce = std::min(fabs(v_max), 0.3);
double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
carrot_dist2 = std::max(carrot_dist2, 0.05);
double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
const auto &plan_back_pose = compute_plan_.poses.back();
double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y);
post_cost = std::max(post_cost, center_cost_);
this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
double d_reduce = std::clamp(journey_plan, min_S, max_S);
double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
double v_min =
journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
v_min *= sign_x;
// std::shared_ptr<PurePursuit> pure_pursuit = std::make_shared<PurePursuit>();
// pure_pursuit->computePurePursuit(traj_, carrot_pose, velocity, min_approach_linear_velocity_, journey_plan, sign_x, lookahead_dist, lookahead_time_, drive_cmd);
double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
double vel_reduce = sign_x > 0
? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
: std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce;
double v_theta = drive_cmd.x * curvature;
double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_))
if (this->nav_stop_)
{
carrot_dist2 *= 0.6;
curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
v_theta = drive_cmd.x * curvature;
}
if (fabs(v_theta) > LIMIT_VEL_THETA)
{
robot_nav_2d_msgs::Twist2D cmd_vel, cmd_result;
cmd_vel.x = sign_x > 0
? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1))
: std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1));
cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5);
this->moveWithAccLimits(velocity, cmd_vel, cmd_result);
drive_cmd.x = std::copysign(cmd_result.x, sign_x);
v_theta = drive_cmd.x * curvature;
}
if (journey_plan < min_journey_squared_)
{
if (transform_plan_.poses.size() > 2)
if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
{
robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
double dx = end.pose.x - start.pose.x;
double dy = end.pose.y - start.pose.y;
v_theta = atan2(dy, dx);
if (v_theta > M_PI_2)
v_theta -= M_PI;
else if (v_theta < -M_PI_2)
v_theta += M_PI;
// v_theta *= 0.5;
v_theta = std::clamp(v_theta, -0.02, 0.02);
if (!stopWithAccLimits(pose, velocity, drive_cmd))
return result;
}
else
v_theta = 0.0;
drive_cmd = {};
result.velocity = drive_cmd;
return result;
}
double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8;
double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt;
double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt;
drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth);
if (this->nav_stop_)
{
if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
{
if (!stopWithAccLimits(pose, velocity, drive_cmd))
return result;
}
else
drive_cmd = {};
result.velocity = drive_cmd;
return result;
}
Eigen::VectorXd y(2);
y << drive_cmd.x, drive_cmd.theta;
@ -553,19 +474,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
}
kf_->update(y, dt, A);
// Check if Kalman filter's estimated velocity exceeds v_max
if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE))
{
drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0];
}
else
{
drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
}
drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x);
drive_cmd.theta = std::clamp(kf_->state()[3], -LIMIT_VEL_THETA, LIMIT_VEL_THETA);
}
@ -1107,49 +1016,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
// ROS_INFO_STREAM_THROTTLE(0.1, ss.str());
}
std::vector<robot_geometry_msgs::Point>
mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution)
{
// Dịch sang tọa độ tuyệt đối
std::vector<robot_geometry_msgs::Point> abs_footprint;
double cos_th = std::cos(pose.theta);
double sin_th = std::sin(pose.theta);
for (auto pt : footprint)
{
pt.x *= 1.2;
pt.y *= 1.2;
robot_geometry_msgs::Point abs_pt;
abs_pt.x = pose.x + pt.x * cos_th - pt.y * sin_th;
abs_pt.y = pose.y + pt.x * sin_th + pt.y * cos_th;
abs_footprint.push_back(abs_pt);
}
std::vector<robot_geometry_msgs::Point> points;
for (size_t i = 0; i < abs_footprint.size(); ++i)
{
const robot_geometry_msgs::Point &start = abs_footprint[i];
const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
double dx = end.x - start.x;
double dy = end.y - start.y;
double distance = std::hypot(dx, dy);
int steps = std::max(1, static_cast<int>(std::floor(distance / resolution)));
for (int j = 0; j <= steps; ++j)
{
if (j == steps && i != abs_footprint.size() - 1)
continue; // tránh lặp
double t = static_cast<double>(j) / steps;
robot_geometry_msgs::Point pt;
pt.x = start.x + t * dx;
pt.y = start.y + t * dy;
points.push_back(pt);
}
}
return points;
}
double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, const double &y)
{
unsigned int mx, my;
@ -1227,89 +1093,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v
return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0;
}
void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose)
{
const auto &plan_back_pose = compute_plan_.poses.back();
// const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0;
// const double offset_min = this->min_path_distance_;
auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
for (const auto &point : points_rb)
{
double cost_goal = costAtPose(point.x, point.y);
double dx = point.x - pose.pose.x;
double dy = point.y - pose.pose.y;
double cos_yaw = cos(-pose.pose.theta);
double sin_yaw = sin(-pose.pose.theta);
double y_rel = dx * sin_yaw + dy * cos_yaw;
const double epsilon = min_path_distance_;
if (y_rel > epsilon)
{
cost_left_side_ = std::max(cost_left_side_, cost_goal);
}
else if (y_rel < -epsilon)
{
cost_right_side_ = std::max(cost_right_side_, cost_goal);
}
}
unsigned int step_footprint = 10;
if ((unsigned int)(compute_plan_.poses.size() - 1) < 10)
{
auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
for (const auto &point : points)
{
double cost_goal = costAtPose(point.x, point.y);
double dx = point.x - plan_back_pose.pose.x;
double dy = point.y - plan_back_pose.pose.y;
double cos_yaw = cos(-plan_back_pose.pose.theta);
double sin_yaw = sin(-plan_back_pose.pose.theta);
double y_rel = dx * sin_yaw + dy * cos_yaw;
const double epsilon = min_path_distance_;
if (y_rel > epsilon)
{
cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
}
else if (y_rel < -epsilon)
{
cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
}
else
center_cost_ = std::max(center_cost_, cost_goal);
}
}
else
{
for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint)
{
auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
for (const auto &point : points)
{
double cost_goal = costAtPose(point.x, point.y);
double dx = point.x - compute_plan_.poses[i].pose.x;
double dy = point.y - compute_plan_.poses[i].pose.y;
double cos_yaw = cos(-compute_plan_.poses[i].pose.theta);
double sin_yaw = sin(-compute_plan_.poses[i].pose.theta);
double y_rel = dx * sin_yaw + dy * cos_yaw;
const double epsilon = min_path_distance_;
if (y_rel > epsilon)
{
cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
}
else if (y_rel < -epsilon)
{
cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
}
else
center_cost_ = std::max(center_cost_, cost_goal);
}
double dx = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.x - compute_plan_.poses[i].pose.x;
double dy = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.y - compute_plan_.poses[i].pose.y;
if (hypot(dx, dy) > 1.0)
break;
}
}
}
score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create()
{

163
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy.cpp → src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory_copy.cpp
View File

@ -16,7 +16,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
traj_ = traj;
costmap_robot_ = costmap_robot;
this->getParams();
nh_.param("publish_topic", enable_publish_, false);
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
footprint_spec_ = costmap_robot_->getRobotFootprint();
@ -42,6 +41,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
this->min_path_distance_ = min_length > 0.1 ? min_length : 0.1;
this->max_path_distance_ = max_length > 0.1 ? max_length : 0.1;
}
this->initKalmanFilter();
x_direction_ = y_direction_ = theta_direction_ = 0;
this->initialized_ = true;
@ -100,7 +100,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::initKalmanFilter()
void mkt_algorithm::diff::PredictiveTrajectory::getParams()
{
robot_base_frame_ = nh_priv_.param<std::string>("robot_base_frame", std::string("base_link"));
nh_priv_.param<bool>("avoid_obstacles", avoid_obstacles_, false);
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);
@ -140,7 +139,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
}
double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
control_duration_ = 1.0 / control_frequency;
window_size_ = (size_t)(control_frequency * 3.0);
if (traj_)
{
@ -433,13 +431,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
const double distance_allow_rotate = min_journey_squared_;
const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y);
const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
if (avoid_obstacles_)
allow_rotate = journey_plan >= distance_allow_rotate &&
fabs(front.y) <= 0.2 &&
(path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution());
else
allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
double angle_to_heading;
if (allow_rotate && shouldRotateToPath(transformed_plan, carrot_pose, velocity, angle_to_heading, sign_x))
{
@ -461,8 +454,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
const auto &plan_back_pose = compute_plan_.poses.back();
double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y);
post_cost = std::max(post_cost, center_cost_);
double post_cost = 0.0;
this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
@ -483,12 +475,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
double v_theta = drive_cmd.x * curvature;
double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_))
{
carrot_dist2 *= 0.6;
curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
v_theta = drive_cmd.x * curvature;
}
if (fabs(v_theta) > LIMIT_VEL_THETA)
{
robot_nav_2d_msgs::Twist2D cmd_vel, cmd_result;
@ -552,19 +539,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
A(i + 1, i + 2) = dt;
}
kf_->update(y, dt, A);
// Check if Kalman filter's estimated velocity exceeds v_max
if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE))
{
drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0];
}
else
{
drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
}
drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_max), fabs(v_max));
drive_cmd.x = fabs(drive_cmd.x) >= min_approach_linear_velocity_ ? drive_cmd.x : std::copysign(min_approach_linear_velocity_, sign_x);
drive_cmd.theta = std::clamp(kf_->state()[3], -LIMIT_VEL_THETA, LIMIT_VEL_THETA);
@ -1107,49 +1083,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
// ROS_INFO_STREAM_THROTTLE(0.1, ss.str());
}
std::vector<robot_geometry_msgs::Point>
mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution)
{
// Dịch sang tọa độ tuyệt đối
std::vector<robot_geometry_msgs::Point> abs_footprint;
double cos_th = std::cos(pose.theta);
double sin_th = std::sin(pose.theta);
for (auto pt : footprint)
{
pt.x *= 1.2;
pt.y *= 1.2;
robot_geometry_msgs::Point abs_pt;
abs_pt.x = pose.x + pt.x * cos_th - pt.y * sin_th;
abs_pt.y = pose.y + pt.x * sin_th + pt.y * cos_th;
abs_footprint.push_back(abs_pt);
}
std::vector<robot_geometry_msgs::Point> points;
for (size_t i = 0; i < abs_footprint.size(); ++i)
{
const robot_geometry_msgs::Point &start = abs_footprint[i];
const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
double dx = end.x - start.x;
double dy = end.y - start.y;
double distance = std::hypot(dx, dy);
int steps = std::max(1, static_cast<int>(std::floor(distance / resolution)));
for (int j = 0; j <= steps; ++j)
{
if (j == steps && i != abs_footprint.size() - 1)
continue; // tránh lặp
double t = static_cast<double>(j) / steps;
robot_geometry_msgs::Point pt;
pt.x = start.x + t * dx;
pt.y = start.y + t * dy;
points.push_back(pt);
}
}
return points;
}
double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, const double &y)
{
unsigned int mx, my;
@ -1227,90 +1160,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v
return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0;
}
void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose)
{
const auto &plan_back_pose = compute_plan_.poses.back();
// const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0;
// const double offset_min = this->min_path_distance_;
auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
for (const auto &point : points_rb)
{
double cost_goal = costAtPose(point.x, point.y);
double dx = point.x - pose.pose.x;
double dy = point.y - pose.pose.y;
double cos_yaw = cos(-pose.pose.theta);
double sin_yaw = sin(-pose.pose.theta);
double y_rel = dx * sin_yaw + dy * cos_yaw;
const double epsilon = min_path_distance_;
if (y_rel > epsilon)
{
cost_left_side_ = std::max(cost_left_side_, cost_goal);
}
else if (y_rel < -epsilon)
{
cost_right_side_ = std::max(cost_right_side_, cost_goal);
}
}
unsigned int step_footprint = 10;
if ((unsigned int)(compute_plan_.poses.size() - 1) < 10)
{
auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
for (const auto &point : points)
{
double cost_goal = costAtPose(point.x, point.y);
double dx = point.x - plan_back_pose.pose.x;
double dy = point.y - plan_back_pose.pose.y;
double cos_yaw = cos(-plan_back_pose.pose.theta);
double sin_yaw = sin(-plan_back_pose.pose.theta);
double y_rel = dx * sin_yaw + dy * cos_yaw;
const double epsilon = min_path_distance_;
if (y_rel > epsilon)
{
cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
}
else if (y_rel < -epsilon)
{
cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
}
else
center_cost_ = std::max(center_cost_, cost_goal);
}
}
else
{
for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint)
{
auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
for (const auto &point : points)
{
double cost_goal = costAtPose(point.x, point.y);
double dx = point.x - compute_plan_.poses[i].pose.x;
double dy = point.y - compute_plan_.poses[i].pose.y;
double cos_yaw = cos(-compute_plan_.poses[i].pose.theta);
double sin_yaw = sin(-compute_plan_.poses[i].pose.theta);
double y_rel = dx * sin_yaw + dy * cos_yaw;
const double epsilon = min_path_distance_;
if (y_rel > epsilon)
{
cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
}
else if (y_rel < -epsilon)
{
cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
}
else
center_cost_ = std::max(center_cost_, cost_goal);
}
double dx = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.x - compute_plan_.poses[i].pose.x;
double dy = compute_plan_.poses[(unsigned int)(compute_plan_.poses.size() - 1)].pose.y - compute_plan_.poses[i].pose.y;
if (hypot(dx, dy) > 1.0)
break;
}
}
}
score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create()
{
return std::make_shared<mkt_algorithm::diff::PredictiveTrajectory>();

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

@ -25,7 +25,6 @@ void mkt_algorithm::diff::RotateToGoal::initialize(
void mkt_algorithm::diff::RotateToGoal::getParams()
{
robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
nh_priv_.param<bool>("avoid_obstacles", avoid_obstacles_, false);
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);

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

@ -1,13 +1,23 @@
#include <mkt_algorithm/diff/pure_pursuit.h>
#define LIMIT_VEL_THETA 0.33
void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity_min,
const double &velocity_max,
const double &linear_vel,
void mkt_algorithm::diff::PurePursuit::computePurePursuit(
const score_algorithm::TrajectoryGenerator::Ptr &traj,
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const double &min_dist2,
const double &curvature_dist2_floor,
const double &dist2_override)
const robot_nav_2d_msgs::Twist2D &velocity,
const double &min_approach_linear_velocity,
const double &journey_plan,
const double &sign_x,
const double &lookahead_dist_min,
const double &lookahead_dist_max,
const double &lookahead_dist,
const double &lookahead_time,
robot_nav_2d_msgs::Twist2D &drive_cmd)
{
if (!traj)
return;
const double velocity_max = sign_x > 0 ? traj->getTwistLinear(true).x : traj->getTwistLinear(false).x;
const double vel_x_reduce = std::min(fabs(velocity_max), 0.3);
double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
carrot_dist2 = std::max(carrot_dist2, 0.05);
@ -20,18 +30,19 @@ void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity
drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit);
}
const auto &plan_back_pose = compute_plan_.poses.back();
double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y);
post_cost = std::max(post_cost, center_cost_);
this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
double d_reduce = std::clamp(journey_plan, min_S, max_S);
double post_cost = 0.0;
double distance_error = 0.0;
robot_nav_2d_msgs::Twist2D twist = traj->getTwist();
this->applyConstraints(distance_error, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
double d_reduce = std::clamp(journey_plan, lookahead_dist_min, lookahead_dist_max);
double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
double v_min =
journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity) * cosine_factor_begin_reduce + min_approach_linear_velocity;
v_min *= sign_x;
double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
@ -39,14 +50,49 @@ void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity
double vel_reduce = sign_x > 0
? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
: std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce;
drive_cmd.x = (journey_plan) >= d_reduce ? drive_cmd.x : vel_reduce;
double v_theta = drive_cmd.x * curvature;
double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_))
carrot_dist2 *= 0.6;
curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
v_theta = drive_cmd.x * curvature;
if (fabs(v_theta) > LIMIT_VEL_THETA)
{
carrot_dist2 *= 0.6;
curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
robot_nav_2d_msgs::Twist2D cmd_vel, cmd_result;
cmd_vel.x = sign_x > 0
? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1))
: std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1));
cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5);
// this->moveWithAccLimits(velocity, cmd_vel, cmd_result);
drive_cmd.x = std::copysign(cmd_result.x, sign_x);
v_theta = drive_cmd.x * curvature;
}
if (journey_plan < min_journey_squared_)
{
if (transform_plan_.poses.size() > 2)
{
robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
double dx = end.pose.x - start.pose.x;
double dy = end.pose.y - start.pose.y;
v_theta = atan2(dy, dx);
if (v_theta > M_PI_2)
v_theta -= M_PI;
else if (v_theta < -M_PI_2)
v_theta += M_PI;
// v_theta *= 0.5;
v_theta = std::clamp(v_theta, -0.02, 0.02);
}
else
v_theta = 0.0;
}
double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8;
double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt;
double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt;
drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth);
}

6
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h
View File

@ -81,6 +81,12 @@ namespace mkt_plugins
* @return Next valid velocity combination
*/
robot_nav_2d_msgs::Twist2D nextTwist() override;
/**
* @brief Get the current velocity
* @return The current velocity
*/
robot_nav_2d_msgs::Twist2D getTwist() override;
/**
* @brief Generate a trajectory from start pose to goal

6
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h
View File

@ -54,6 +54,12 @@ public:
* Should only be called when hasMoreTwists() returns true.
*/
virtual robot_nav_2d_msgs::Twist2D nextTwist() = 0;
/**
* @brief Get the current velocity
* @return The current velocity
*/
virtual robot_nav_2d_msgs::Twist2D getTwist() = 0;
}; // class VelocityIterator
} // namespace mkt_plugins

6
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h
View File

@ -54,6 +54,12 @@ namespace mkt_plugins
* Automatically iterates to the next valid velocity if current is invalid.
*/
robot_nav_2d_msgs::Twist2D nextTwist() override;
/**
* @brief Get the current velocity
* @return The current velocity (x, y, theta)
*/
robot_nav_2d_msgs::Twist2D getTwist() override;
protected:
/**

5
src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp
View File

@ -166,6 +166,11 @@ namespace mkt_plugins
return velocity_iterator_->nextTwist();
}
robot_nav_2d_msgs::Twist2D StandardTrajectoryGenerator::getTwist()
{
return velocity_iterator_->getTwist();
}
std::vector<double> StandardTrajectoryGenerator::getTimeSteps(const robot_nav_2d_msgs::Twist2D &cmd_vel)
{
std::vector<double> steps;

9
src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp
View File

@ -40,6 +40,15 @@ robot_nav_2d_msgs::Twist2D XYThetaIterator::nextTwist()
return velocity;
}
robot_nav_2d_msgs::Twist2D XYThetaIterator::getTwist()
{
robot_nav_2d_msgs::Twist2D velocity;
velocity.x = x_it_->getVelocity();
velocity.y = y_it_->getVelocity();
velocity.theta = th_it_->getVelocity();
return velocity;
}
bool XYThetaIterator::isValidVelocity()
{
return kinematics_->isValidSpeed(x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity());

4
src/Navigations/Libraries/robot_actionlib_msgs/CMakeLists.txt
View File

@ -30,7 +30,6 @@ if (NOT BUILDING_WITH_CATKIN)
set(PACKAGES_DIR
robot_std_msgs
utils
robot_time
)
@ -41,7 +40,6 @@ else()
# ========================================================
find_package(catkin REQUIRED COMPONENTS
robot_std_msgs
utils
robot_time
)
@ -50,7 +48,7 @@ else()
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS robot_std_msgs utils robot_time
CATKIN_DEPENDS robot_std_msgs robot_time
DEPENDS Boost
)

3
src/Navigations/Libraries/robot_actionlib_msgs/package.xml
View File

@ -22,9 +22,6 @@
<build_depend>robot_std_msgs</build_depend>
<run_depend>robot_std_msgs</run_depend>
<build_depend>utils</build_depend>
<run_depend>utils</run_depend>
<build_depend>robot_time</build_depend>
<run_depend>robot_time</run_depend>
</package>