diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index 581f117..f430f63 100644 --- a/config/pnkx_local_planner_params.yaml +++ b/config/pnkx_local_planner_params.yaml @@ -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 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 ddf4b30..9ba1f05 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 @@ -543,13 +543,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( // } // else // { - // if(fabs(carrot_pose.pose.y) > 0.2) - // { - // lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist); - // } - robot_nav_2d_msgs::Twist2D drive_target; + 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); + // Normal Pure Pursuit this->computePurePursuit( carrot_pose, @@ -633,28 +630,32 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( if (std::fabs(v_target) < min_approach_linear_velocity) v_target = std::copysign(min_approach_linear_velocity, sign_x); + std::stringstream ss; + // 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_) { + 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; - 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_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); - } + 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); + ss << "error: " << error << " "; double w_heading = 0.0; pid(error, near_goal_heading_integral_, @@ -667,6 +668,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( // 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 { @@ -678,6 +680,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( { near_goal_heading_was_active_ = false; } + w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta)); // 6) Apply acceleration limits (linear + angular) @@ -687,7 +690,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( drive_cmd.x = velocity.x + dv; drive_cmd.theta = velocity.theta + dw; - + ss << drive_cmd.theta << " "; Eigen::VectorXd y(2); y << drive_cmd.x, drive_cmd.theta; @@ -707,6 +710,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( 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_); + ROS_WARN_THROTTLE(0.1, "%s", ss.str().c_str()); } void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( @@ -792,16 +796,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 +887,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,7 +1241,7 @@ 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_ + max_path_distance_)) { return generateParallelPath(path, sign_x); } @@ -1279,15 +1281,16 @@ 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 = 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; } @@ -1312,10 +1315,8 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer 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); + 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 = x; @@ -1356,6 +1357,8 @@ 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_out; @@ -1426,6 +1429,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; diff --git a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp index ae8b0c1..2548090 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp @@ -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; } }