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

Merge remote-tracking branch 'origin/3.0' into awc-devel

Browse Source
This commit is contained in:
hoangson02
2026-03-26 11:56:53 +00:00
parent 3944447b4d 58d925f2be
commit ed8e364ab4
6 changed files with 137 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
config/pnkx_local_planner_params.yaml
View File

@@ -106,8 +106,8 @@ MKTAlgorithmDiffPredictiveTrajectory:
angular_decel_zone: 0.1
# stoped
rot_stopped_velocity: 0.05
trans_stopped_velocity: 0.06
rot_stopped_velocity: 0.03
trans_stopped_velocity: 0.03
use_final_heading_alignment: true
final_heading_xy_tolerance: 0.1
@@ -143,8 +143,8 @@ MKTAlgorithmDiffGoStraight:
angular_decel_zone: 0.1
# stoped
rot_stopped_velocity: 0.05
trans_stopped_velocity: 0.06
rot_stopped_velocity: 0.03
trans_stopped_velocity: 0.03
use_final_heading_alignment: true
final_heading_xy_tolerance: 0.1
@@ -180,8 +180,8 @@ MKTAlgorithmDiffRotateToGoal:
angular_decel_zone: 0.1
# stoped
rot_stopped_velocity: 0.05
trans_stopped_velocity: 0.06
rot_stopped_velocity: 0.03
trans_stopped_velocity: 0.03
use_final_heading_alignment: true
final_heading_xy_tolerance: 0.1

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

@@ -194,11 +194,11 @@ namespace mkt_algorithm
/**
* @brief Generate Hermite trajectory
* @param pose
* @param path
* @param sign_x
* @return trajectory
*/
robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);
robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x);
/**
* @brief Generate Hermite quadratic trajectory
@@ -206,7 +206,7 @@ namespace mkt_algorithm
* @param sign_x
* @return trajectory
*/
robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x);
robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x);
/**
* @brief Should rotate to path

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

@@ -476,7 +476,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
twist = traj_->nextTwist();
}
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));
// drive_cmd.x = std::min(sqrt(twist.x * twist.x), fabs(v_max));
drive_cmd.x = sqrt(twist.x * twist.x);
robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
if (transformed_plan.poses.empty())
@@ -529,26 +530,23 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
}
else
{
// === Final Heading Alignment Check ===
double xy_error = 0.0, heading_error = 0.0;
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);
#ifdef BUILD_WITH_ROS
ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
#endif
}
else
{
// if(fabs(carrot_pose.pose.y) > 0.2)
// // === Final Heading Alignment Check ===
// double xy_error = 0.0, heading_error = 0.0;
// if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
// {
// lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
// // Use Arc Motion controller for final heading alignment
// alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
// #ifdef BUILD_WITH_ROS
// ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f",
// heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
// #endif
// }
// else
// {
robot_nav_2d_msgs::Twist2D drive_target = drive_cmd;
// transformed_plan = this->generateTrajectory(transformed_plan, drive_cmd, velocity, sign_x, drive_target);
// carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
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,
@@ -559,7 +557,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
sign_x,
dt,
drive_cmd);
}
// }
applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
if (this->nav_stop_)
{
@@ -633,52 +631,55 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
v_target = std::copysign(min_approach_linear_velocity, sign_x);
// 5) Angular speed from curvature
double w_target = v_target * kappa + std::copysign(carrot_pose.pose.theta * dt, kappa);
double w_target = v_target * kappa;
if(journey(trajectory.poses, 0, trajectory.poses.size() - 1) <= min_journey_squared_)
{
if (trajectory.poses.size() >= 2) {
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;
}
}
// ss << w_target << " ";
// if (trajectory.poses.size() >= 2) {
// 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;
// const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x;
// const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y;
// if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution())
// {
// if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
// continue;
// heading_ref = angles::normalize_angle(std::atan2(dy, dx));
// break;
// }
// }
const double error = angles::normalize_angle(heading_ref);
double w_heading = 0.0;
pid(error,
near_goal_heading_integral_,
near_goal_heading_last_error_,
dt,
final_heading_kp_angular_,
final_heading_ki_angular_,
final_heading_kd_angular_,
w_heading);
// Apply acceleration limits
double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
w_target = velocity.theta + dw_heading;
w_target = std::clamp(w_target, -0.05, 0.05);
}
else
{
// const double error = angles::normalize_angle(heading_ref);
// ss << "error: " << error << " ";
// double w_heading = 0.0;
// pid(error,
// near_goal_heading_integral_,
// near_goal_heading_last_error_,
// dt,
// final_heading_kp_angular_,
// final_heading_ki_angular_,
// final_heading_kd_angular_,
// w_heading);
// // Apply acceleration limits
// double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
// w_target = velocity.theta + dw_heading;
// ss << w_target << " ";
// }
// else
// {
w_target = 0.0;
near_goal_heading_was_active_ = false;
}
// }
}
else
{
near_goal_heading_was_active_ = false;
}
// w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta));
// 6) Apply acceleration limits (linear + angular)
const double dv = std::clamp(v_target - velocity.x, -acc_lim_x_ * dt, acc_lim_x_ * dt);
@@ -686,8 +687,6 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
drive_cmd.x = velocity.x + dv;
drive_cmd.theta = velocity.theta + dw;
Eigen::VectorXd y(2);
y << drive_cmd.x, drive_cmd.theta;
@@ -705,8 +704,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
double v_min = min_approach_linear_velocity_;
drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target));
drive_cmd.x = fabs(drive_cmd.x) >= v_min ? drive_cmd.x : std::copysign(v_min, sign_x);
// if (kf_filter_angular_)
// drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
if (kf_filter_angular_)
drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_);
}
void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling(
@@ -792,16 +791,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
// (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;
// #ifdef BUILD_WITH_ROS
// 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);
#ifdef BUILD_WITH_ROS
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);
// else if(fabs(velocity.x) < min_speed_xy_)
// {
// ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta);
// ROS_INFO_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);
// }
// #endif
#endif
return result;
}
@@ -883,13 +882,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldAlignToFinalHeading(
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())
const auto& dx = sign_x < 0.0 ? p1.x - p.x : p.x - p1.x;
const auto& dy = sign_x < 0.0 ? p1.y - p.y : p.y - p1.y;
if(std::hypot(dx, dy) >= 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);
}
heading_error = angles::normalize_angle(std::atan2(dy, dx));
break;
}
}
@@ -1239,17 +1236,17 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
if (max_kappa <= straight_threshold && fabs(path.poses.back().pose.x) < min_lookahead_dist_) // nếu đường thẳng
{
if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.front().pose.x) < (min_lookahead_dist_ + max_path_distance_))
if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.back().pose.x) < min_lookahead_dist_)
{
return generateParallelPath(path, sign_x);
}
return generateHermiteTrajectory(path.poses.back(), sign_x);
return generateHermiteTrajectory(path, sign_x);
}
else // nếu đường cong
{
if(fabs(drive_cmd.x) < min_speed_xy_)
drive_cmd.x = std::copysign(min_speed_xy_, sign_x);
return generateHermiteQuadraticTrajectory(path.poses.back(), sign_x);
return generateHermiteQuadraticTrajectory(path, sign_x);
}
}
@@ -1279,40 +1276,49 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generatePar
dx = path.poses[i+1].pose.x - path.poses[i-1].pose.x;
dy = path.poses[i+1].pose.y - path.poses[i-1].pose.y;
}
if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
continue;
double theta = atan2(dy, dx);
double x_off = p.x - offset_y * sin(theta)*sign_x;
double y_off = p.y - offset_y * cos(theta)*sign_x;
parallel_path.poses[i].header = path.poses[i].header;
parallel_path.poses[i].pose.x = x_off;
parallel_path.poses[i].pose.y = y_off;
parallel_path.poses[i].pose.theta = theta; // hoặc giữ nguyên p.theta
parallel_path.poses[i].header = path.poses[i].header;
parallel_path.poses[i].pose.theta = sign_x < 0 ? angles::normalize_angle(theta + M_PI) : theta;
}
return parallel_path;
}
robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
{
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;
hermite_trajectory.header = path.header;
const double x = pose.pose.x;
const double y = pose.pose.y;
const double theta = pose.pose.theta;
if (path.poses.empty())
return hermite_trajectory;
const auto &goal = path.poses.back();
if (hermite_trajectory.header.frame_id.empty())
hermite_trajectory.header.frame_id = goal.header.frame_id;
if (hermite_trajectory.header.stamp.isZero())
hermite_trajectory.header.stamp = goal.header.stamp;
const double x = goal.pose.x;
const double y = goal.pose.y;
double theta = goal.pose.theta;
const double L = std::hypot(x, y);
if (L < 1e-6) {
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;
pose_stamped.pose.x = x;
pose_stamped.pose.y = y;
pose_stamped.pose.theta = theta;
pose_stamped.header.stamp = hermite_trajectory.header.stamp;
pose_stamped.header.frame_id = hermite_trajectory.header.frame_id;
hermite_trajectory.poses.push_back(pose_stamped);
return hermite_trajectory;
}
@@ -1346,30 +1352,39 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
double dx = dh10 * Lnegative + dh01 * x + dh11 * Lnegative * std::cos(theta);
double dy = dh01 * y + dh11 * Lnegative * std::sin(theta);
if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
continue;
double heading = std::atan2(dy, dx);
robot_nav_2d_msgs::Pose2DStamped pose;
pose.pose.x = px;
pose.pose.y = py;
pose.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
pose.header.stamp = hermite_trajectory.header.stamp;
pose.header.frame_id = hermite_trajectory.header.frame_id;
hermite_trajectory.poses.push_back(pose);
robot_nav_2d_msgs::Pose2DStamped pose_out;
pose_out.pose.x = px;
pose_out.pose.y = py;
pose_out.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
pose_out.header.stamp = hermite_trajectory.header.stamp;
pose_out.header.frame_id = hermite_trajectory.header.frame_id;
hermite_trajectory.poses.push_back(pose_out);
}
return hermite_trajectory;
}
robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteQuadraticTrajectory(
const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x)
const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
{
robot_nav_2d_msgs::Path2D trajectory;
trajectory.poses.clear();
trajectory.header.stamp = pose.header.stamp;
trajectory.header.frame_id = pose.header.frame_id;
trajectory.header = path.header;
if (path.poses.empty())
return trajectory;
const double x = pose.pose.x;
const double y = pose.pose.y;
const double theta = sign_x < 0 ? angles::normalize_angle(pose.pose.theta + M_PI) : pose.pose.theta;
const auto &goal = path.poses.back();
if (trajectory.header.frame_id.empty())
trajectory.header.frame_id = goal.header.frame_id;
if (trajectory.header.stamp.isZero())
trajectory.header.stamp = goal.header.stamp;
const double x = goal.pose.x;
const double y = goal.pose.y;
const double theta = sign_x < 0 ? angles::normalize_angle(goal.pose.theta + M_PI) : goal.pose.theta;
const double L = std::hypot(x, y);
if (L < 1e-6)
{
@@ -1409,6 +1424,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
double dx = 2.0 * ax * t + bx;
double dy = 2.0 * ay * t + by;
if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6)
continue;
double heading = std::atan2(dy, dx);
robot_nav_2d_msgs::Pose2DStamped pose_out;

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

@@ -70,9 +70,8 @@ bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStam
double tolerance = fabs(cos(theta)) >= fabs(sin(theta)) ? x : y;
if(fabs(tolerance) <= xy_goal_tolerance_)
{
robot::log_info_at(__FILE__, __LINE__, "%x %x", fabs(tolerance) <= xy_goal_tolerance_, tolerance * old_xy_goal_tolerance_ < 0);
robot::log_info_at(__FILE__, __LINE__, "%f %f %f %f", fabs(cos(theta)), fabs(sin(theta)), xy_goal_tolerance_, yaw_goal_tolerance_);
robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %f %f %f %f %f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
robot::log_info_at(__FILE__, __LINE__, "%.3f %.3f %.3f %.3f %.3f", fabs(cos(theta)), fabs(sin(theta)),xy_tolerance, xy_goal_tolerance_, yaw_goal_tolerance_);
robot::log_info_at(__FILE__, __LINE__, "Goal checker 1 ok %.3f %.3f %.3f %.3f %.3f ", tolerance, old_xy_goal_tolerance_, x, y, theta);
return true;
}
}

4
src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
View File

@@ -194,6 +194,10 @@ namespace two_points_planner
pose.pose.position.x += resolution * cos(theta);
pose.pose.position.y += resolution * sin(theta);
plan.push_back(pose);
pose = start;
pose.pose.position.x -= resolution * cos(theta);
pose.pose.position.y -= resolution * sin(theta);
plan.push_back(pose);
plan.push_back(goal);
return true;
}

4
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -976,6 +976,7 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
as_->processGoal(action_goal);
robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
}
catch (const std::exception &e)
@@ -1116,7 +1117,6 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
lock.unlock();
return false;
}
as_->processGoal(action_goal);
}
catch (const std::exception &e)
@@ -1278,7 +1278,6 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
}
robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
as_->processGoal(action_goal);
robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
}
catch (const std::exception &e)
@@ -1424,7 +1423,6 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
lock.unlock();
return false;
}
as_->processGoal(action_goal);
}
catch (const std::exception &e)