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

update

Browse Source
This commit is contained in:
HiepLM
2026-04-23 20:10:57 +07:00
parent 5812542eaf
commit d681201698
3 changed files with 9 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -443,8 +443,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
}
catch (std::exception &e)
{
robot::log_error("[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
return false;
robot::log_warning_throttle(0.2, "[%s:%d]\n getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
x_direction = x_direction_;
}
}
@@ -1262,11 +1262,12 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra
const double straight_threshold = std::max(0.05, 2.0 * (costmap_robot_ ? costmap_robot_->getCostmap()->getResolution() : 0.05));
drive_cmd.x = this->adjustSpeedWithHermiteTrajectory(velocity, path, drive_target.x, sign_x);
drive_cmd.theta = max_vel_theta_;
// nếu đường thẳng
if (max_kappa <= straight_threshold)
{
if(fabs(path.poses.back().pose.x) * 0.9 < min_lookahead_dist_)
if(hypot(path.poses.back().pose.x, path.poses.back().pose.y) * 0.9 < min_lookahead_dist_)
{
if(fabs(path.poses.back().pose.x) < min_journey_squared_)
drive_cmd.theta = 0.01;
@@ -1326,8 +1327,6 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generatePar
return parallel_path;
}
robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory(
const robot_nav_2d_msgs::Path2D &path, const double &sign_x)
{