From 6b4d630d09900e2eea82da53e8634babe9ea43fa Mon Sep 17 00:00:00 2001 From: QUYVN Date: Tue, 24 Mar 2026 08:16:56 +0000 Subject: [PATCH 1/5] fix speed --- src/APIs/c_api/src/nav_c_api.cpp | 1 + .../Packages/move_base/src/move_base.cpp | 20 ++++++++++++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index ab7ac21..fede502 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -572,6 +572,7 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle, linear.x = linear_x; linear.y = linear_y; linear.z = linear_z; + robot::log_info("setTwistLinear %f", linear.x); return nav_ptr->setTwistLinear(linear); } catch (...) diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 5073b7a..3730106 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -976,6 +976,12 @@ 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); + + setTwistLinear(old_linear_fwd_); + setTwistLinear(old_linear_bwd_); + setTwistAngular(old_angular_fwd_); + setTwistAngular(old_angular_rev_); + robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server"); } catch (const std::exception &e) @@ -1118,6 +1124,11 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg, } as_->processGoal(action_goal); + + setTwistLinear(old_linear_fwd_); + setTwistLinear(old_linear_bwd_); + setTwistAngular(old_angular_fwd_); + setTwistAngular(old_angular_rev_); } catch (const std::exception &e) { @@ -1279,6 +1290,10 @@ 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); + setTwistLinear(old_linear_fwd_); + setTwistLinear(old_linear_bwd_); + setTwistAngular(old_angular_fwd_); + setTwistAngular(old_angular_rev_); robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server"); } catch (const std::exception &e) @@ -1424,8 +1439,11 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg, lock.unlock(); return false; } - as_->processGoal(action_goal); + setTwistLinear(old_linear_fwd_); + setTwistLinear(old_linear_bwd_); + setTwistAngular(old_angular_fwd_); + setTwistAngular(old_angular_rev_); } catch (const std::exception &e) { From 69823442f98f9d6a4e2d1947f0314b557e01ce54 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Tue, 24 Mar 2026 15:26:00 +0700 Subject: [PATCH 2/5] update --- src/Algorithms/Packages/global_planners/custom_planner | 2 +- src/Libraries/xmlrpcpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Algorithms/Packages/global_planners/custom_planner b/src/Algorithms/Packages/global_planners/custom_planner index 43810ce..7c84705 160000 --- a/src/Algorithms/Packages/global_planners/custom_planner +++ b/src/Algorithms/Packages/global_planners/custom_planner @@ -1 +1 @@ -Subproject commit 43810ce14095c3aabc4b351b9dbb025f03419a52 +Subproject commit 7c84705ce737736d53829916eb36c9860c10c418 diff --git a/src/Libraries/xmlrpcpp b/src/Libraries/xmlrpcpp index 4071815..1f8d5cc 160000 --- a/src/Libraries/xmlrpcpp +++ b/src/Libraries/xmlrpcpp @@ -1 +1 @@ -Subproject commit 40718158aec71537bdc06d0d4fcab249f43edb3a +Subproject commit 1f8d5cc30098001ea7587b86348677d86bc4d831 From ea41848a4a8faecfc5b00430c92cba5dba7372f1 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Wed, 25 Mar 2026 15:35:15 +0700 Subject: [PATCH 3/5] fix gentrajectory --- .../diff/diff_predictive_trajectory.h | 6 +- .../src/diff/diff_predictive_trajectory.cpp | 101 ++++++++++-------- 2 files changed, 62 insertions(+), 45 deletions(-) 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 1843f90..404f31d 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 @@ -529,19 +529,19 @@ 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 - { +// // === 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); @@ -559,7 +559,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_) { @@ -1242,13 +1242,13 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateTra { 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); } } @@ -1285,33 +1285,43 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generatePar 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].pose.theta = p.theta; parallel_path.poses[i].header = path.poses[i].header; } - 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; + const double theta = goal.pose.theta; + robot::log_debug("x: %f, y: %f, theta: %f", x, y, 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; } @@ -1347,28 +1357,35 @@ robot_nav_2d_msgs::Path2D mkt_algorithm::diff::PredictiveTrajectory::generateHer 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) { From ba503eca855a7025b1be370a0102255630e22acc Mon Sep 17 00:00:00 2001 From: QUYVN Date: Wed, 25 Mar 2026 10:33:01 +0000 Subject: [PATCH 4/5] fix giam toc --- .../mkt_algorithm/src/diff/diff_predictive_trajectory.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 404f31d..ddf4b30 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()) { From 58d925f2be8cd7949d022db4e5a99ab7262ba092 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Thu, 26 Mar 2026 14:42:26 +0700 Subject: [PATCH 5/5] fix --- config/pnkx_local_planner_params.yaml | 12 ++-- .../src/diff/diff_predictive_trajectory.cpp | 65 ++++++++++--------- .../mkt_plugins/src/goal_checker.cpp | 5 +- 3 files changed, 43 insertions(+), 39 deletions(-) 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; } }