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 6283028..a873f8c 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 @@ -630,49 +630,53 @@ 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; 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; - // 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()) - // { - // if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6) - // continue; - // heading_ref = angles::normalize_angle(std::atan2(dy, dx)); - // break; - // } - // } + 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; + const auto& dx = p1.x - p.x ; + const auto& dy = p1.y - p.y ; + if(std::hypot(dx, dy) >= costmap_robot_->getCostmap()->getResolution()) + { + if(fabs(dx) < 1e-6 && fabs(dy) < 1e-6) + continue; + heading_ref = std::atan2(dy, dx); + ss << "error " << heading_ref << " "; + if(sign_x < 0.0) + heading_ref += std::copysign(M_PI, heading_ref) * (-1.0); + break; + } + } - // const double error = angles::normalize_angle(heading_ref); - // ss << "error: " << error << " "; - // double w_heading = 0.0; - // pid(error, - // near_goal_heading_integral_, - // near_goal_heading_last_error_, - // dt, - // final_heading_kp_angular_, - // final_heading_ki_angular_, - // final_heading_kd_angular_, - // w_heading); - // // 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 - // { - w_target = 0.0; + const double error = heading_ref; + ss << error << " "; + double w_heading = 0.0; + pid(error, + near_goal_heading_integral_, + near_goal_heading_last_error_, + dt, + final_heading_kp_angular_, + final_heading_ki_angular_, + final_heading_kd_angular_, + w_heading); + // Apply acceleration limits + 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; + } + else + { + w_target = std::clamp(w_target, -0.001, 0.001); near_goal_heading_was_active_ = false; - // } + } } else { @@ -687,6 +691,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( drive_cmd.x = velocity.x + dv; drive_cmd.theta = velocity.theta + dw; + Eigen::VectorXd y(2); y << drive_cmd.x, drive_cmd.theta; @@ -706,6 +711,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_); + // robot::log_info("%s", ss.str().c_str()); } void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( @@ -769,16 +775,22 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( for(int i = 2; i < global_plan.poses.size(); i++) { const auto& p = global_plan.poses[i]; + const auto& dx = p.pose.x - p1.pose.x; + const auto& dy = p.pose.y - p1.pose.y; if(std::hypot(p.pose.x, p.pose.y) > costmap_robot_->getCostmap()->getResolution()) { - path_angle = std::atan2(p.pose.y - p1.pose.y, p.pose.x - p1.pose.x); + if(fabs(dx) < 1e-9 && fabs(dy) < 1e-9) + continue; + path_angle = std::atan2(dy, dx); break; } } } // Whether we should rotate robot to rough path heading - angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : path_angle; + if(sign_x < 0.0) + path_angle += std::copysign(M_PI, path_angle) * (-1.0); + angle_to_path = path_angle; double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y); // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) double heading_rotate = rotate_to_heading_min_angle_; @@ -1236,7 +1248,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.back().pose.x) < min_lookahead_dist_) + if(fabs(path.poses.front().pose.y) <= 0.03 && fabs(path.poses.back().pose.x) < min_lookahead_dist_ ) { return generateParallelPath(path, sign_x); } diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp index 7f61930..84f7c56 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp +++ b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp @@ -190,13 +190,14 @@ namespace two_points_planner } else { - robot_geometry_msgs::PoseStamped pose = start; - pose.pose.position.x += resolution * cos(theta); - pose.pose.position.y += resolution * sin(theta); + auto goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal); // hoặc start.theta + robot_geometry_msgs::PoseStamped pose = goal; + pose.pose.position.x += resolution * std::cos(goal_2d.pose.theta); + pose.pose.position.y += resolution * std::sin(goal_2d.pose.theta); plan.push_back(pose); - pose = start; - pose.pose.position.x -= resolution * cos(theta); - pose.pose.position.y -= resolution * sin(theta); + pose = goal; + pose.pose.position.x -= resolution * std::cos(goal_2d.pose.theta); + pose.pose.position.y -= resolution * std::sin(goal_2d.pose.theta); plan.push_back(pose); plan.push_back(goal); return true;