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

fix gentrajectory

Browse Source
This commit is contained in:
HiepLM
2026-03-25 15:35:15 +07:00
parent 69823442f9
commit ea41848a4a
2 changed files with 62 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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 * @brief Generate Hermite trajectory
* @param pose * @param path
* @param sign_x * @param sign_x
* @return trajectory * @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 * @brief Generate Hermite quadratic trajectory
@@ -206,7 +206,7 @@ namespace mkt_algorithm
* @param sign_x * @param sign_x
* @return trajectory * @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 * @brief Should rotate to path

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

@@ -529,19 +529,19 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
} }
else else
{ {
// === Final Heading Alignment Check === // // === Final Heading Alignment Check ===
double xy_error = 0.0, heading_error = 0.0; // double xy_error = 0.0, heading_error = 0.0;
if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x)) // if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x))
{ // {
// Use Arc Motion controller for final heading alignment // // Use Arc Motion controller for final heading alignment
alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd); // alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd);
#ifdef BUILD_WITH_ROS // #ifdef BUILD_WITH_ROS
ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f", // 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); // heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta);
#endif // #endif
} // }
else // else
{ // {
// if(fabs(carrot_pose.pose.y) > 0.2) // if(fabs(carrot_pose.pose.y) > 0.2)
// { // {
// lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist); // lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist);
@@ -559,7 +559,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
sign_x, sign_x,
dt, dt,
drive_cmd); drive_cmd);
} // }
applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt); applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt);
if (this->nav_stop_) if (this->nav_stop_)
{ {
@@ -1242,13 +1242,13 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
{ {
return generateParallelPath(path, sign_x); return generateParallelPath(path, sign_x);
} }
return generateHermiteTrajectory(path.poses.back(), sign_x); return generateHermiteTrajectory(path, sign_x);
} }
else // nếu đường cong else // nếu đường cong
{ {
if(fabs(drive_cmd.x) < min_speed_xy_) if(fabs(drive_cmd.x) < min_speed_xy_)
drive_cmd.x = std::copysign(min_speed_xy_, sign_x); drive_cmd.x = std::copysign(min_speed_xy_, sign_x);
return generateHermiteQuadraticTrajectory(path.poses.back(), sign_x); return generateHermiteQuadraticTrajectory(path, sign_x);
} }
} }
@@ -1285,33 +1285,43 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generatePar
parallel_path.poses[i].pose.x = x_off; parallel_path.poses[i].pose.x = x_off;
parallel_path.poses[i].pose.y = y_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].pose.theta = p.theta;
parallel_path.poses[i].header = path.poses[i].header; parallel_path.poses[i].header = path.poses[i].header;
} }
return parallel_path; return parallel_path;
} }
robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory( 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; robot_nav_2d_msgs::Path2D hermite_trajectory;
hermite_trajectory.poses.clear(); hermite_trajectory.poses.clear();
hermite_trajectory.header.stamp = pose.header.stamp; hermite_trajectory.header = path.header;
hermite_trajectory.header.frame_id = pose.header.frame_id;
const double x = pose.pose.x; if (path.poses.empty())
const double y = pose.pose.y; return hermite_trajectory;
const double theta = pose.pose.theta;
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;
const double theta = goal.pose.theta;
robot::log_debug("x: %f, y: %f, theta: %f", x, y, theta);
const double L = std::hypot(x, y); const double L = std::hypot(x, y);
if (L < 1e-6) { if (L < 1e-6) {
robot_nav_2d_msgs::Pose2DStamped pose_stamped; robot_nav_2d_msgs::Pose2DStamped pose_stamped;
pose_stamped.pose.x = 0.0; pose_stamped.pose.x = x;
pose_stamped.pose.y = 0.0; pose_stamped.pose.y = y;
pose_stamped.pose.theta = 0.0; pose_stamped.pose.theta = theta;
pose_stamped.header.stamp = pose.header.stamp; pose_stamped.header.stamp = hermite_trajectory.header.stamp;
pose_stamped.header.frame_id = pose.header.frame_id; pose_stamped.header.frame_id = hermite_trajectory.header.frame_id;
hermite_trajectory.poses.push_back(pose_stamped); hermite_trajectory.poses.push_back(pose_stamped);
return hermite_trajectory; return hermite_trajectory;
} }
@@ -1347,28 +1357,35 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer
double heading = std::atan2(dy, dx); double heading = std::atan2(dy, dx);
robot_nav_2d_msgs::Pose2DStamped pose; robot_nav_2d_msgs::Pose2DStamped pose_out;
pose.pose.x = px; pose_out.pose.x = px;
pose.pose.y = py; pose_out.pose.y = py;
pose.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading; pose_out.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading;
pose.header.stamp = hermite_trajectory.header.stamp; pose_out.header.stamp = hermite_trajectory.header.stamp;
pose.header.frame_id = hermite_trajectory.header.frame_id; pose_out.header.frame_id = hermite_trajectory.header.frame_id;
hermite_trajectory.poses.push_back(pose); hermite_trajectory.poses.push_back(pose_out);
} }
return hermite_trajectory; return hermite_trajectory;
} }
robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteQuadraticTrajectory( 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; robot_nav_2d_msgs::Path2D trajectory;
trajectory.poses.clear(); trajectory.poses.clear();
trajectory.header.stamp = pose.header.stamp; trajectory.header = path.header;
trajectory.header.frame_id = pose.header.frame_id; if (path.poses.empty())
return trajectory;
const double x = pose.pose.x; const auto &goal = path.poses.back();
const double y = pose.pose.y; if (trajectory.header.frame_id.empty())
const double theta = sign_x < 0 ? angles::normalize_angle(pose.pose.theta + M_PI) : pose.pose.theta; 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); const double L = std::hypot(x, y);
if (L < 1e-6) if (L < 1e-6)
{ {