From cac2343d4754bb9353774f8f87345c60adcdaad3 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Sat, 18 Apr 2026 08:31:50 +0200 Subject: [PATCH] Thuat toan --- src/APIs/c_api/src/nav_c_api.cpp | 11 +++++++--- .../src/diff/diff_predictive_trajectory.cpp | 21 ++++++++++++------- 2 files changed, 22 insertions(+), 10 deletions(-) diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index fede502..0292a09 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -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 (...) { diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index cc91fa1..1b66616 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -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; }