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

Thuat toan

Browse Source
This commit is contained in:
HiepLM
2026-04-18 08:31:50 +02:00
parent 9f0bd9f485
commit cac2343d47
2 changed files with 22 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/APIs/c_api/src/nav_c_api.cpp
View File

@@ -569,11 +569,16 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
return false;
robot_geometry_msgs::Vector3 linear;
linear.x = linear_x;
linear.x = 0.1;
linear.y = linear_y;
linear.z = linear_z;
robot::log_info("setTwistLinear %f", linear.x);
return nav_ptr->setTwistLinear(linear);
bool result = nav_ptr->setTwistLinear(linear);
robot::log_info("setTwistLinear Forward %f", linear.x);
linear.x = -0.1;
result &= result && nav_ptr->setTwistLinear(linear);
robot::log_info("setTwistLinear Backward %f", linear.x);
return result;
}
catch (...)
{

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

@@ -440,8 +440,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
double dir_path = cos(fabs(angle_path - goal_pose.theta));
if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
x_direction = dir_path > 0 ? FORWARD : BACKWARD;
else
x_direction = 0.0;
}
catch (std::exception &e)
{
@@ -483,8 +481,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 = sqrt(twist.x * twist.x);
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())
@@ -591,7 +589,11 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
}
if(fabs(v_max == 0.0))
{
drive_cmd.x = 0.0;
robot::log_warning_throttle(0.2, "[%s:%d]\n v_max is 0.0", __FILE__, __LINE__);
return result;
}
result.velocity = drive_cmd;
prevous_drive_cmd_ = drive_cmd;
return result;
@@ -678,6 +680,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit(
double dw_heading = std::clamp(w_heading - velocity.theta, -acc_lim_theta_ * dt, acc_lim_theta_ * dt);
ss << "dw_heading " << dw_heading << " ";
w_target = velocity.theta + dw_heading;
// w_target = std::clamp(w_target, -0.03, 0.03);
}
else
{
@@ -776,10 +779,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
// 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)
if(is_stopped && global_plan.poses.size() >= 4 &&
journey(global_plan.poses, 0, global_plan.poses.size() - 1) >= 0.7 * min_lookahead_dist_)
{
const auto& p1 = global_plan.poses[1];
for(int i = 2; i < global_plan.poses.size(); i++)
const auto& p1 = global_plan.poses[2];
for(int i = 3; i < global_plan.poses.size(); i++)
{
const auto& p = global_plan.poses[i];
const auto& dx = p.pose.x - p1.pose.x;
@@ -820,6 +824,9 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
// 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);
// }
#else
if (result)
robot::log_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
return result;
}