diff --git a/config/pnkx_local_planner_params.yaml b/config/pnkx_local_planner_params.yaml index de01e63..7f87beb 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/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index 6a21147..936d3be 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -194,11 +194,11 @@ namespace mkt_algorithm /** * @brief Generate Hermite trajectory - * @param pose + * @param path * @param sign_x * @return trajectory */ - robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x); + robot_nav_2d_msgs::Path2D generateHermiteTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x); /** * @brief Generate Hermite quadratic trajectory @@ -206,7 +206,7 @@ namespace mkt_algorithm * @param sign_x * @return trajectory */ - robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x); + robot_nav_2d_msgs::Path2D generateHermiteQuadraticTrajectory(const robot_nav_2d_msgs::Path2D &path, const double &sign_x); /** * @brief Should rotate to path 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 04a0f96..6283028 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 @@ -476,8 +476,9 @@ 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 = 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()) { @@ -529,26 +530,23 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( } else { - // === Final Heading Alignment Check === - double xy_error = 0.0, heading_error = 0.0; - if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x)) - { - // Use Arc Motion controller for final heading alignment - alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd); -#ifdef BUILD_WITH_ROS - ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f", - heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta); -#endif - } - else - { - // if(fabs(carrot_pose.pose.y) > 0.2) - // { - // lookahead_dist = sqrt(carrot_pose.pose.y *carrot_pose.pose.y + lookahead_dist * lookahead_dist); - // } +// // === Final Heading Alignment Check === +// double xy_error = 0.0, heading_error = 0.0; +// if (shouldAlignToFinalHeading(transformed_plan, carrot_pose, velocity, xy_error, heading_error, sign_x)) +// { +// // Use Arc Motion controller for final heading alignment +// alignToFinalHeading(xy_error, heading_error, velocity, sign_x, dt, drive_cmd); +// #ifdef BUILD_WITH_ROS +// ROS_INFO("heading_err=%.3f deg, v=%.3f, w_current=%.3f, w_target=%.3f", +// heading_error * 180.0 / M_PI, drive_cmd.x, velocity.theta, drive_cmd.theta); +// #endif +// } +// else +// { 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); + 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, @@ -559,7 +557,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( sign_x, dt, drive_cmd); - } + // } applyDistanceSpeedScaling(compute_plan_, velocity, drive_cmd, sign_x, dt); if (this->nav_stop_) { @@ -633,52 +631,55 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( v_target = std::copysign(min_approach_linear_velocity, sign_x); // 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_) { - 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()) - { - 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); - } - break; - } - } + // 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; + // } + // } - const double error = angles::normalize_angle(heading_ref); - 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; - w_target = std::clamp(w_target, -0.05, 0.05); - } - else - { + // 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; near_goal_heading_was_active_ = false; - } + // } } else { near_goal_heading_was_active_ = false; } - // w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta)); + + w_target = std::clamp(w_target, -fabs(drive_target.theta), fabs(drive_target.theta)); // 6) Apply acceleration limits (linear + angular) const double dv = std::clamp(v_target - velocity.x, -acc_lim_x_ * dt, acc_lim_x_ * dt); @@ -686,8 +687,6 @@ 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; @@ -705,8 +704,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::computePurePursuit( double v_min = min_approach_linear_velocity_; drive_cmd.x = std::clamp(kf_->state()[0], -fabs(v_target), fabs(v_target)); 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_); + if (kf_filter_angular_) + drive_cmd.theta = std::clamp(kf_->state()[3], -max_vel_theta_, max_vel_theta_); } void mkt_algorithm::diff::PredictiveTrajectory::applyDistanceSpeedScaling( @@ -792,16 +791,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 +882,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,17 +1236,17 @@ 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_) { return generateParallelPath(path, sign_x); } - return generateHermiteTrajectory(path.poses.back(), sign_x); + return generateHermiteTrajectory(path, sign_x); } else // nếu đường cong { if(fabs(drive_cmd.x) < min_speed_xy_) drive_cmd.x = std::copysign(min_speed_xy_, sign_x); - return generateHermiteQuadraticTrajectory(path.poses.back(), sign_x); + return generateHermiteQuadraticTrajectory(path, sign_x); } } @@ -1279,40 +1276,49 @@ 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 = theta; // hoặc giữ nguyên 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; } + + robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteTrajectory( - const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x) + const robot_nav_2d_msgs::Path2D &path, const double &sign_x) { robot_nav_2d_msgs::Path2D hermite_trajectory; hermite_trajectory.poses.clear(); - hermite_trajectory.header.stamp = pose.header.stamp; - hermite_trajectory.header.frame_id = pose.header.frame_id; + hermite_trajectory.header = path.header; - const double x = pose.pose.x; - const double y = pose.pose.y; - const double theta = pose.pose.theta; + if (path.poses.empty()) + return hermite_trajectory; + + const auto &goal = path.poses.back(); + if (hermite_trajectory.header.frame_id.empty()) + hermite_trajectory.header.frame_id = goal.header.frame_id; + if (hermite_trajectory.header.stamp.isZero()) + hermite_trajectory.header.stamp = goal.header.stamp; + + const double x = goal.pose.x; + const double y = goal.pose.y; + 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 = 0.0; - pose_stamped.pose.y = 0.0; - pose_stamped.pose.theta = 0.0; - pose_stamped.header.stamp = pose.header.stamp; - pose_stamped.header.frame_id = pose.header.frame_id; + pose_stamped.pose.x = x; + pose_stamped.pose.y = y; + pose_stamped.pose.theta = theta; + pose_stamped.header.stamp = hermite_trajectory.header.stamp; + pose_stamped.header.frame_id = hermite_trajectory.header.frame_id; hermite_trajectory.poses.push_back(pose_stamped); return hermite_trajectory; } @@ -1346,30 +1352,39 @@ 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; - pose.pose.x = px; - pose.pose.y = py; - pose.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading; - pose.header.stamp = hermite_trajectory.header.stamp; - pose.header.frame_id = hermite_trajectory.header.frame_id; - hermite_trajectory.poses.push_back(pose); + robot_nav_2d_msgs::Pose2DStamped pose_out; + pose_out.pose.x = px; + pose_out.pose.y = py; + pose_out.pose.theta = sign_x < 0 ? angles::normalize_angle(heading + M_PI) : heading; + pose_out.header.stamp = hermite_trajectory.header.stamp; + pose_out.header.frame_id = hermite_trajectory.header.frame_id; + hermite_trajectory.poses.push_back(pose_out); } return hermite_trajectory; } robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHermiteQuadraticTrajectory( - const robot_nav_2d_msgs::Pose2DStamped &pose, const double &sign_x) + const robot_nav_2d_msgs::Path2D &path, const double &sign_x) { robot_nav_2d_msgs::Path2D trajectory; trajectory.poses.clear(); - trajectory.header.stamp = pose.header.stamp; - trajectory.header.frame_id = pose.header.frame_id; + trajectory.header = path.header; + if (path.poses.empty()) + return trajectory; - const double x = pose.pose.x; - const double y = pose.pose.y; - const double theta = sign_x < 0 ? angles::normalize_angle(pose.pose.theta + M_PI) : pose.pose.theta; + const auto &goal = path.poses.back(); + if (trajectory.header.frame_id.empty()) + trajectory.header.frame_id = goal.header.frame_id; + if (trajectory.header.stamp.isZero()) + trajectory.header.stamp = goal.header.stamp; + + const double x = goal.pose.x; + const double y = goal.pose.y; + const double theta = sign_x < 0 ? angles::normalize_angle(goal.pose.theta + M_PI) : goal.pose.theta; const double L = std::hypot(x, y); if (L < 1e-6) { @@ -1409,6 +1424,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; } } 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 d3064b3..7f61930 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 @@ -194,6 +194,10 @@ namespace two_points_planner pose.pose.position.x += resolution * cos(theta); pose.pose.position.y += resolution * sin(theta); plan.push_back(pose); + pose = start; + pose.pose.position.x -= resolution * cos(theta); + pose.pose.position.y -= resolution * sin(theta); + plan.push_back(pose); plan.push_back(goal); return true; } diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 5073b7a..3a8fdc1 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -976,6 +976,7 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal, robot::log_info("[MoveBase::moveTo] Processing goal through action server..."); as_->processGoal(action_goal); + robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server"); } catch (const std::exception &e) @@ -1116,7 +1117,6 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg, lock.unlock(); return false; } - as_->processGoal(action_goal); } catch (const std::exception &e) @@ -1278,7 +1278,6 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry } robot::log_info("[MoveBase::moveTo] Processing goal through action server..."); - as_->processGoal(action_goal); robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server"); } catch (const std::exception &e) @@ -1424,7 +1423,6 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg, lock.unlock(); return false; } - as_->processGoal(action_goal); } catch (const std::exception &e)