From d9735d5787a67ca096281dd6e225aebb8e5de9a8 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Fri, 10 Oct 2025 15:29:27 +0200 Subject: [PATCH 01/45] Use final pose orientation at terminal intermediate goal --- .../include/mir_dwb_critics/path_progress.h | 20 +- mir_dwb_critics/src/path_progress.cpp | 325 +++++++++++++++--- 2 files changed, 290 insertions(+), 55 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index bb78175b..91df808a 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -35,8 +35,14 @@ #define MIR_DWB_CRITICS_PATH_PROGRESS_H_ #include +#include #include +namespace geometry_msgs +{ +struct PoseStamped; +} + namespace mir_dwb_critics { /** @@ -63,14 +69,26 @@ class PathProgressCritic : public dwb_critics::MapGridCritic unsigned int& y, double& desired_angle); unsigned int getGoalIndex(const std::vector& plan, unsigned int start_index, - unsigned int last_valid_index) const; + unsigned int last_valid_index, double& desired_angle, bool& has_forward_direction) const; + + bool computeOutgoingAngle(const std::vector& plan, unsigned int index, + double& angle) const; + + bool isGoalReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, + double goal_yaw) const; double xy_local_goal_tolerance_; + double yaw_local_goal_tolerance_; double angle_threshold_; + double articulation_angle_threshold_; double heading_scale_; + bool enforce_forward_dot_; + + unsigned int last_progress_index_; std::vector reached_intermediate_goals_; double desired_angle_; + ros::Publisher intermediate_goal_pub_; }; } // namespace mir_dwb_critics diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index fa296929..2ac299f0 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -32,9 +32,16 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include +#include +#include #include #include #include +#include +#include +#include +#include +#include #include namespace mir_dwb_critics @@ -63,22 +70,32 @@ void PathProgressCritic::onInit() { dwb_critics::MapGridCritic::onInit(); critic_nh_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.20); + critic_nh_.param("yaw_local_goal_tolerance", yaw_local_goal_tolerance_, 0.15); critic_nh_.param("angle_threshold", angle_threshold_, M_PI_4); + critic_nh_.param("articulation_angle_threshold", articulation_angle_threshold_, 1.3089969389957472); critic_nh_.param("heading_scale", heading_scale_, 1.0); + critic_nh_.param("enforce_forward_dot", enforce_forward_dot_, true); + + intermediate_goal_pub_ = critic_nh_.advertise("intermediate_goal", 1); + + articulation_angle_threshold_ = std::max(articulation_angle_threshold_, angle_threshold_); // divide heading scale by position scale because the sum will be multiplied by scale again heading_scale_ /= getScale(); + last_progress_index_ = 0; + reached_intermediate_goals_.clear(); } void PathProgressCritic::reset() { reached_intermediate_goals_.clear(); + last_progress_index_ = 0; } double PathProgressCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj) { double position_score = MapGridCritic::scoreTrajectory(traj); - double heading_diff = fabs(remainder(traj.poses.back().theta - desired_angle_, 2 * M_PI)); + double heading_diff = fabs(angles::shortest_angular_distance(traj.poses.back().theta, desired_angle_)); double heading_score = heading_diff * heading_diff; return position_score + heading_scale_ * heading_score; @@ -98,6 +115,12 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co std::vector plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses; + if (plan.empty()) + { + ROS_ERROR_NAMED("PathProgressCritic", "The adjusted global plan was empty."); + return false; + } + // find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map unsigned int start_index = 0; double distance_to_start = std::numeric_limits::infinity(); @@ -157,96 +180,290 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } } - // find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point, - // i.e. is within angle_threshold_ of the starting direction. - unsigned int goal_index = start_index; + // Constrain the search range to enforce monotonic progress. + last_progress_index_ = std::min(last_progress_index_, static_cast(plan.size() - 1)); - while (goal_index < last_valid_index) + unsigned int search_start_index = std::max(start_index, last_progress_index_); + search_start_index = std::min(search_start_index, last_valid_index); + + unsigned int goal_index = search_start_index; + double goal_yaw = plan[goal_index].theta; + bool has_forward_direction = false; + bool found_goal = false; + + unsigned int search_index = search_start_index; + while (search_index <= last_valid_index) { - goal_index = getGoalIndex(plan, start_index, last_valid_index); + double candidate_yaw = goal_yaw; + bool candidate_has_forward = false; + unsigned int candidate_index = + getGoalIndex(plan, search_index, last_valid_index, candidate_yaw, candidate_has_forward); + + candidate_index = std::max(candidate_index, search_index); - // check if goal already reached - bool goal_already_reached = false; - for (auto& reached_intermediate_goal : reached_intermediate_goals_) + if (isGoalReached(robot_pose, plan[candidate_index], candidate_yaw)) { - double distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]); - if (distance < xy_local_goal_tolerance_) + last_progress_index_ = std::max(last_progress_index_, candidate_index); + geometry_msgs::Pose2D reached_pose = plan[candidate_index]; + reached_pose.theta = candidate_yaw; + if (reached_intermediate_goals_.empty() || + nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || + fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > 1e-6) + { + reached_intermediate_goals_.push_back(reached_pose); + } + ROS_DEBUG_NAMED("PathProgressCritic", + "Reached intermediate goal index %u. last_progress_index_: %u", candidate_index, + last_progress_index_); + + if (candidate_index >= last_valid_index) { - goal_already_reached = true; - // choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again - // (start_index is now > goal_index) - for (start_index = goal_index; start_index <= last_valid_index; ++start_index) + goal_index = candidate_index; + goal_yaw = candidate_yaw; + has_forward_direction = candidate_has_forward; + found_goal = true; + break; + } + + search_index = candidate_index + 1; + continue; + } + + if (enforce_forward_dot_ && candidate_has_forward) + { + double to_goal_x = plan[candidate_index].x - robot_pose.x; + double to_goal_y = plan[candidate_index].y - robot_pose.y; + double dot = to_goal_x * std::cos(candidate_yaw) + to_goal_y * std::sin(candidate_yaw); + if (dot < 0.0) + { + ROS_DEBUG_NAMED("PathProgressCritic", + "Skipping goal index %u due to backward alignment (dot product %.3f)", candidate_index, dot); + if (candidate_index >= last_valid_index) { - distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]); - if (distance >= xy_local_goal_tolerance_) - { - break; - } + break; } - break; + search_index = candidate_index + 1; + continue; } } - if (!goal_already_reached) + + goal_index = candidate_index; + goal_yaw = candidate_yaw; + has_forward_direction = candidate_has_forward; + found_goal = true; + break; + } + + if (!found_goal) + { + unsigned int fallback_start = std::min(last_valid_index, search_start_index); + goal_index = fallback_start; + bool selected_fallback = false; + + for (; goal_index <= last_valid_index; ++goal_index) { - // new goal not in reached_intermediate_goals_ - double distance = nav_2d_utils::poseDistance(plan[goal_index], robot_pose); - if (distance < xy_local_goal_tolerance_) + has_forward_direction = computeOutgoingAngle(plan, goal_index, goal_yaw); + if (has_forward_direction) { - // the robot has currently reached the goal - reached_intermediate_goals_.push_back(plan[goal_index]); - ROS_DEBUG_NAMED("PathProgressCritic", "Number of reached_intermediate goals: %zu", - reached_intermediate_goals_.size()); + if (!enforce_forward_dot_) + { + selected_fallback = true; + break; + } + + double to_goal_x = plan[goal_index].x - robot_pose.x; + double to_goal_y = plan[goal_index].y - robot_pose.y; + double dot = to_goal_x * std::cos(goal_yaw) + to_goal_y * std::sin(goal_yaw); + if (dot >= 0.0) + { + selected_fallback = true; + break; + } + continue; } - else + + goal_yaw = plan[goal_index].theta; + if (!enforce_forward_dot_ || goal_index == last_valid_index) { - // we've found a new goal! + selected_fallback = true; break; } } + + if (!selected_fallback) + { + return false; + } } - if (start_index > goal_index) - start_index = goal_index; + ROS_ASSERT(goal_index <= last_valid_index); - // save goal in x, y worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y); - desired_angle = plan[start_index].theta; + desired_angle = goal_yaw; + + ROS_DEBUG_NAMED("PathProgressCritic", + "Selected goal index %u (x: %.3f, y: %.3f, yaw: %.3f rad). last_progress_index_: %u", + goal_index, plan[goal_index].x, plan[goal_index].y, goal_yaw, last_progress_index_); + + if (intermediate_goal_pub_) + { + geometry_msgs::PoseStamped goal_pose; + goal_pose.header = global_plan.header; + goal_pose.header.stamp = ros::Time::now(); + goal_pose.pose.position.x = plan[goal_index].x; + goal_pose.pose.position.y = plan[goal_index].y; + goal_pose.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, goal_yaw); + goal_pose.pose.orientation.x = q.x(); + goal_pose.pose.orientation.y = q.y(); + goal_pose.pose.orientation.z = q.z(); + goal_pose.pose.orientation.w = q.w(); + intermediate_goal_pub_.publish(goal_pose); + } return true; } unsigned int PathProgressCritic::getGoalIndex(const std::vector& plan, unsigned int start_index, - unsigned int last_valid_index) const + unsigned int last_valid_index, double& desired_angle, + bool& has_forward_direction) const { - if (start_index >= last_valid_index) - return last_valid_index; + if (plan.empty()) + { + desired_angle = 0.0; + has_forward_direction = false; + return 0; + } + + const double epsilon = 1e-9; + unsigned int clamped_start = std::min(start_index, static_cast(plan.size() - 1)); + unsigned int clamped_last = std::min(last_valid_index, static_cast(plan.size() - 1)); + + if (clamped_start >= clamped_last) + { + has_forward_direction = computeOutgoingAngle(plan, clamped_start, desired_angle); + if (!has_forward_direction) + { + desired_angle = plan[clamped_start].theta; + } + return clamped_start; + } + + unsigned int goal_index = clamped_start; + double base_angle = 0.0; + bool base_angle_set = false; + double previous_segment_angle = 0.0; + bool previous_segment_angle_set = false; + unsigned int previous_segment_end_index = clamped_start; + double last_valid_segment_angle = 0.0; + bool last_valid_segment_angle_set = false; - unsigned int goal_index = start_index; - const double start_direction_x = plan[start_index + 1].x - plan[start_index].x; - const double start_direction_y = plan[start_index + 1].y - plan[start_index].y; - if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) + for (unsigned int i = clamped_start + 1; i <= clamped_last; ++i) { - // make sure that atan2 is defined - double start_angle = atan2(start_direction_y, start_direction_x); + double direction_x = plan[i].x - plan[i - 1].x; + double direction_y = plan[i].y - plan[i - 1].y; + double length = hypot(direction_x, direction_y); + if (length < epsilon) + { + continue; + } + + double current_angle = atan2(direction_y, direction_x); + last_valid_segment_angle = current_angle; + last_valid_segment_angle_set = true; - for (unsigned int i = start_index + 1; i <= last_valid_index; ++i) + if (!base_angle_set) { - const double current_direction_x = plan[i].x - plan[i - 1].x; - const double current_direction_y = plan[i].y - plan[i - 1].y; - if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) + base_angle = current_angle; + base_angle_set = true; + } + + if (previous_segment_angle_set) + { + double articulation_angle = fabs(angles::shortest_angular_distance(previous_segment_angle, current_angle)); + if (articulation_angle >= articulation_angle_threshold_) { - double current_angle = atan2(current_direction_y, current_direction_x); + goal_index = previous_segment_end_index; + break; + } + } - // goal pose is found if plan doesn't point far enough away from the starting point - if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) > angle_threshold_) - break; + double deviation = fabs(angles::shortest_angular_distance(base_angle, current_angle)); + if (deviation > angle_threshold_) + { + break; + } - goal_index = i; - } + goal_index = i; + previous_segment_angle = current_angle; + previous_segment_end_index = i; + previous_segment_angle_set = true; + } + + has_forward_direction = computeOutgoingAngle(plan, goal_index, desired_angle); + if (!has_forward_direction) + { + if (goal_index == plan.size() - 1) + { + desired_angle = plan[goal_index].theta; } + else if (previous_segment_angle_set) + { + desired_angle = previous_segment_angle; + has_forward_direction = true; + } + else if (last_valid_segment_angle_set) + { + desired_angle = last_valid_segment_angle; + has_forward_direction = true; + } + else + { + desired_angle = plan[goal_index].theta; + } + } + + if (goal_index == plan.size() - 1) + { + has_forward_direction = false; } + return goal_index; } +bool PathProgressCritic::computeOutgoingAngle(const std::vector& plan, unsigned int index, + double& angle) const +{ + const double epsilon = 1e-9; + if (plan.empty() || index >= plan.size()) + { + return false; + } + + for (unsigned int i = index + 1; i < plan.size(); ++i) + { + double dx = plan[i].x - plan[index].x; + double dy = plan[i].y - plan[index].y; + double length = hypot(dx, dy); + if (length >= epsilon) + { + angle = atan2(dy, dx); + return true; + } + } + + return false; +} + +bool PathProgressCritic::isGoalReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, + double goal_yaw) const +{ + double distance = nav_2d_utils::poseDistance(goal_pose, robot_pose); + double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, goal_yaw)); + + return distance < xy_local_goal_tolerance_ && yaw_error < yaw_local_goal_tolerance_; +} + } // namespace mir_dwb_critics PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathProgressCritic, dwb_local_planner::TrajectoryCritic) From f8f79f16754428cde379973b7a8281acb37d3c95 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Fri, 10 Oct 2025 15:34:31 +0200 Subject: [PATCH 02/45] Include PoseStamped definition in header --- mir_dwb_critics/include/mir_dwb_critics/path_progress.h | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index 91df808a..e957216a 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -35,14 +35,10 @@ #define MIR_DWB_CRITICS_PATH_PROGRESS_H_ #include +#include #include #include -namespace geometry_msgs -{ -struct PoseStamped; -} - namespace mir_dwb_critics { /** From 522a67c505514f547c600da1cc937d62ff46c1a9 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sat, 11 Oct 2025 12:08:03 +0200 Subject: [PATCH 03/45] Hold intermediate goal until within yaw tolerance --- .../include/mir_dwb_critics/path_progress.h | 6 + mir_dwb_critics/src/path_progress.cpp | 115 +++++++++++++++--- 2 files changed, 105 insertions(+), 16 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index e957216a..57b1c18a 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -75,12 +75,18 @@ class PathProgressCritic : public dwb_critics::MapGridCritic double xy_local_goal_tolerance_; double yaw_local_goal_tolerance_; + double final_goal_yaw_tolerance_; double angle_threshold_; double articulation_angle_threshold_; double heading_scale_; bool enforce_forward_dot_; unsigned int last_progress_index_; + bool holding_goal_; + unsigned int held_goal_index_; + geometry_msgs::Pose2D held_goal_pose_; + double hold_position_epsilon_; + double hold_yaw_epsilon_; std::vector reached_intermediate_goals_; double desired_angle_; diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 2ac299f0..95b2037c 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -71,6 +72,11 @@ void PathProgressCritic::onInit() dwb_critics::MapGridCritic::onInit(); critic_nh_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.20); critic_nh_.param("yaw_local_goal_tolerance", yaw_local_goal_tolerance_, 0.15); + ros::NodeHandle private_nh("~"); + if (!private_nh.getParam("yaw_goal_tolerance", final_goal_yaw_tolerance_)) + { + critic_nh_.param("yaw_goal_tolerance", final_goal_yaw_tolerance_, yaw_local_goal_tolerance_); + } critic_nh_.param("angle_threshold", angle_threshold_, M_PI_4); critic_nh_.param("articulation_angle_threshold", articulation_angle_threshold_, 1.3089969389957472); critic_nh_.param("heading_scale", heading_scale_, 1.0); @@ -84,12 +90,25 @@ void PathProgressCritic::onInit() heading_scale_ /= getScale(); last_progress_index_ = 0; reached_intermediate_goals_.clear(); + holding_goal_ = false; + held_goal_index_ = 0; + held_goal_pose_.x = 0.0; + held_goal_pose_.y = 0.0; + held_goal_pose_.theta = 0.0; + hold_position_epsilon_ = 1e-6; + hold_yaw_epsilon_ = 1e-6; + final_goal_yaw_tolerance_ = std::max(final_goal_yaw_tolerance_, 1e-6); } void PathProgressCritic::reset() { reached_intermediate_goals_.clear(); last_progress_index_ = 0; + holding_goal_ = false; + held_goal_index_ = 0; + held_goal_pose_.x = 0.0; + held_goal_pose_.y = 0.0; + held_goal_pose_.theta = 0.0; } double PathProgressCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj) @@ -121,6 +140,14 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co return false; } + if (holding_goal_ && held_goal_index_ >= plan.size()) + { + ROS_DEBUG_NAMED("PathProgressCritic", "Held goal index %u is out of range for current plan of size %zu. Releasing hold.", + held_goal_index_, plan.size()); + holding_goal_ = false; + held_goal_index_ = 0; + } + // find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map unsigned int start_index = 0; double distance_to_start = std::numeric_limits::infinity(); @@ -186,6 +213,52 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co unsigned int search_start_index = std::max(start_index, last_progress_index_); search_start_index = std::min(search_start_index, last_valid_index); + auto publishIntermediateGoal = [&](const geometry_msgs::Pose2D& goal_pose, double goal_yaw) { + if (!intermediate_goal_pub_) + { + return; + } + geometry_msgs::PoseStamped goal_msg; + goal_msg.header = global_plan.header; + goal_msg.header.stamp = ros::Time::now(); + goal_msg.pose.position.x = goal_pose.x; + goal_msg.pose.position.y = goal_pose.y; + goal_msg.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, goal_yaw); + goal_msg.pose.orientation.x = q.x(); + goal_msg.pose.orientation.y = q.y(); + goal_msg.pose.orientation.z = q.z(); + goal_msg.pose.orientation.w = q.w(); + intermediate_goal_pub_.publish(goal_msg); + }; + + if (holding_goal_) + { + double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, held_goal_pose_.theta)); + if (yaw_error >= final_goal_yaw_tolerance_) + { + unsigned int held_x = 0; + unsigned int held_y = 0; + if (worldToGridBounded(info, held_goal_pose_.x, held_goal_pose_.y, held_x, held_y)) + { + x = held_x; + y = held_y; + desired_angle = held_goal_pose_.theta; + publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); + ROS_DEBUG_NAMED("PathProgressCritic", "Holding goal index %u due to yaw error %.3f rad (threshold %.3f)", + held_goal_index_, yaw_error, final_goal_yaw_tolerance_); + return true; + } + + ROS_WARN_NAMED("PathProgressCritic", + "Held goal (index %u) is outside the local costmap. Releasing hold to search for a new goal.", + held_goal_index_); + holding_goal_ = false; + held_goal_index_ = 0; + } + } + unsigned int goal_index = search_start_index; double goal_yaw = plan[goal_index].theta; bool has_forward_direction = false; @@ -301,26 +374,36 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y); desired_angle = goal_yaw; + bool same_as_held = false; + if (holding_goal_) + { + double position_diff_x = plan[goal_index].x - held_goal_pose_.x; + double position_diff_y = plan[goal_index].y - held_goal_pose_.y; + double yaw_diff = angles::shortest_angular_distance(goal_yaw, held_goal_pose_.theta); + same_as_held = (fabs(position_diff_x) <= hold_position_epsilon_) && + (fabs(position_diff_y) <= hold_position_epsilon_) && + (fabs(yaw_diff) <= hold_yaw_epsilon_); + } + + if (!same_as_held) + { + held_goal_pose_ = plan[goal_index]; + held_goal_pose_.theta = goal_yaw; + held_goal_index_ = goal_index; + } + else + { + held_goal_pose_.x = plan[goal_index].x; + held_goal_pose_.y = plan[goal_index].y; + held_goal_pose_.theta = goal_yaw; + } + holding_goal_ = true; + ROS_DEBUG_NAMED("PathProgressCritic", "Selected goal index %u (x: %.3f, y: %.3f, yaw: %.3f rad). last_progress_index_: %u", goal_index, plan[goal_index].x, plan[goal_index].y, goal_yaw, last_progress_index_); - if (intermediate_goal_pub_) - { - geometry_msgs::PoseStamped goal_pose; - goal_pose.header = global_plan.header; - goal_pose.header.stamp = ros::Time::now(); - goal_pose.pose.position.x = plan[goal_index].x; - goal_pose.pose.position.y = plan[goal_index].y; - goal_pose.pose.position.z = 0.0; - tf2::Quaternion q; - q.setRPY(0.0, 0.0, goal_yaw); - goal_pose.pose.orientation.x = q.x(); - goal_pose.pose.orientation.y = q.y(); - goal_pose.pose.orientation.z = q.z(); - goal_pose.pose.orientation.w = q.w(); - intermediate_goal_pub_.publish(goal_pose); - } + publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); return true; } From 5812cec513bf0a4adce4c38afcacc6eb06367ee8 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sat, 11 Oct 2025 23:45:42 +0200 Subject: [PATCH 04/45] Guard articulation vertices from jump-ahead selection --- .../include/mir_dwb_critics/path_progress.h | 4 + mir_dwb_critics/src/path_progress.cpp | 128 +++++++++++++++++- 2 files changed, 129 insertions(+), 3 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index 57b1c18a..73f19ccb 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -67,6 +67,10 @@ class PathProgressCritic : public dwb_critics::MapGridCritic unsigned int getGoalIndex(const std::vector& plan, unsigned int start_index, unsigned int last_valid_index, double& desired_angle, bool& has_forward_direction) const; + bool findNextArticulation(const std::vector& plan, unsigned int start_index, + unsigned int end_index, unsigned int last_valid_index, unsigned int& articulation_index, + double& articulation_yaw, bool& has_forward_direction) const; + bool computeOutgoingAngle(const std::vector& plan, unsigned int index, double& angle) const; diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 95b2037c..26f56668 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -208,11 +208,23 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } // Constrain the search range to enforce monotonic progress. - last_progress_index_ = std::min(last_progress_index_, static_cast(plan.size() - 1)); + unsigned int plan_last_index = static_cast(plan.size() - 1); + last_progress_index_ = std::min(last_progress_index_, plan_last_index); unsigned int search_start_index = std::max(start_index, last_progress_index_); search_start_index = std::min(search_start_index, last_valid_index); + unsigned int articulation_scan_start = 0u; + if (plan_last_index >= 1) + { + unsigned int next_index = last_progress_index_ < plan_last_index ? last_progress_index_ + 1 : plan_last_index; + articulation_scan_start = std::max(next_index, 1u); + } + else + { + articulation_scan_start = plan_last_index; + } + auto publishIntermediateGoal = [&](const geometry_msgs::Pose2D& goal_pose, double goal_yaw) { if (!intermediate_goal_pub_) { @@ -259,6 +271,11 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } } + if (holding_goal_ && held_goal_index_ >= last_progress_index_) + { + search_start_index = std::min(search_start_index, held_goal_index_); + } + unsigned int goal_index = search_start_index; double goal_yaw = plan[goal_index].theta; bool has_forward_direction = false; @@ -272,7 +289,26 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co unsigned int candidate_index = getGoalIndex(plan, search_index, last_valid_index, candidate_yaw, candidate_has_forward); - candidate_index = std::max(candidate_index, search_index); + bool forced_articulation = false; + if (candidate_index > last_progress_index_) + { + unsigned int articulation_index = 0; + double articulation_yaw = candidate_yaw; + bool articulation_has_forward = candidate_has_forward; + if (findNextArticulation(plan, articulation_scan_start, candidate_index, last_valid_index, + articulation_index, articulation_yaw, articulation_has_forward)) + { + candidate_index = articulation_index; + candidate_yaw = articulation_yaw; + candidate_has_forward = articulation_has_forward; + forced_articulation = true; + } + } + + if (!forced_articulation) + { + candidate_index = std::max(candidate_index, search_index); + } if (isGoalReached(robot_pose, plan[candidate_index], candidate_yaw)) { @@ -302,7 +338,7 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co continue; } - if (enforce_forward_dot_ && candidate_has_forward) + if (enforce_forward_dot_ && candidate_has_forward && !forced_articulation) { double to_goal_x = plan[candidate_index].x - robot_pose.x; double to_goal_y = plan[candidate_index].y - robot_pose.y; @@ -367,6 +403,20 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co { return false; } + + if (goal_index > last_progress_index_) + { + unsigned int articulation_index = 0; + double articulation_yaw = goal_yaw; + bool articulation_has_forward = has_forward_direction; + if (findNextArticulation(plan, articulation_scan_start, goal_index, last_valid_index, + articulation_index, articulation_yaw, articulation_has_forward)) + { + goal_index = articulation_index; + goal_yaw = articulation_yaw; + has_forward_direction = articulation_has_forward; + } + } } ROS_ASSERT(goal_index <= last_valid_index); @@ -514,6 +564,78 @@ unsigned int PathProgressCritic::getGoalIndex(const std::vector& plan, unsigned int start_index, + unsigned int end_index, unsigned int last_valid_index, + unsigned int& articulation_index, double& articulation_yaw, + bool& has_forward_direction) const +{ + const double epsilon = 1e-9; + articulation_index = 0; + articulation_yaw = 0.0; + has_forward_direction = false; + + if (plan.size() < 2) + { + return false; + } + + unsigned int clamped_end = std::min(end_index, static_cast(plan.size() - 1)); + clamped_end = std::min(clamped_end, last_valid_index); + if (clamped_end < 1) + { + return false; + } + + unsigned int scan_start = std::max(start_index, 1u); + if (scan_start > clamped_end) + { + return false; + } + + double previous_segment_angle = 0.0; + bool previous_segment_angle_set = false; + unsigned int previous_segment_end_index = 0; + + for (unsigned int i = scan_start; i <= clamped_end; ++i) + { + double direction_x = plan[i].x - plan[i - 1].x; + double direction_y = plan[i].y - plan[i - 1].y; + double length = hypot(direction_x, direction_y); + if (length < epsilon) + { + continue; + } + + double current_angle = atan2(direction_y, direction_x); + + if (previous_segment_angle_set) + { + double articulation_angle = fabs(angles::shortest_angular_distance(previous_segment_angle, current_angle)); + if (articulation_angle >= articulation_angle_threshold_) + { + articulation_index = previous_segment_end_index; + articulation_yaw = current_angle; + has_forward_direction = true; + if (!computeOutgoingAngle(plan, articulation_index, articulation_yaw)) + { + has_forward_direction = false; + if (articulation_index == plan.size() - 1) + { + articulation_yaw = plan[articulation_index].theta; + } + } + return true; + } + } + + previous_segment_angle = current_angle; + previous_segment_angle_set = true; + previous_segment_end_index = i; + } + + return false; +} + bool PathProgressCritic::computeOutgoingAngle(const std::vector& plan, unsigned int index, double& angle) const { From 77985e620206d4c2745050bdf5080968a52577b3 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 07:51:24 +0200 Subject: [PATCH 05/45] Prevent articulation forcing from regressing --- mir_dwb_critics/src/path_progress.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 26f56668..ae609242 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -225,6 +225,10 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co articulation_scan_start = plan_last_index; } + auto articulationSearchStart = [&](unsigned int candidate_start) { + return std::max({candidate_start, articulation_scan_start, 1u}); + }; + auto publishIntermediateGoal = [&](const geometry_msgs::Pose2D& goal_pose, double goal_yaw) { if (!intermediate_goal_pub_) { @@ -295,7 +299,9 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co unsigned int articulation_index = 0; double articulation_yaw = candidate_yaw; bool articulation_has_forward = candidate_has_forward; - if (findNextArticulation(plan, articulation_scan_start, candidate_index, last_valid_index, + unsigned int articulation_start_index = articulationSearchStart(search_index); + if (articulation_start_index <= candidate_index && + findNextArticulation(plan, articulation_start_index, candidate_index, last_valid_index, articulation_index, articulation_yaw, articulation_has_forward)) { candidate_index = articulation_index; @@ -409,7 +415,9 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co unsigned int articulation_index = 0; double articulation_yaw = goal_yaw; bool articulation_has_forward = has_forward_direction; - if (findNextArticulation(plan, articulation_scan_start, goal_index, last_valid_index, + unsigned int articulation_start_index = articulationSearchStart(goal_index); + if (articulation_start_index <= goal_index && + findNextArticulation(plan, articulation_start_index, goal_index, last_valid_index, articulation_index, articulation_yaw, articulation_has_forward)) { goal_index = articulation_index; From c8a32d2f0e546fa21e4f73dad1dcd49bc2f82044 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 08:03:40 +0200 Subject: [PATCH 06/45] Publish articulation debug point cloud --- .../include/mir_dwb_critics/path_progress.h | 2 + mir_dwb_critics/src/path_progress.cpp | 88 ++++++++++++++++++- 2 files changed, 88 insertions(+), 2 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index 73f19ccb..64a34f95 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -95,6 +96,7 @@ class PathProgressCritic : public dwb_critics::MapGridCritic std::vector reached_intermediate_goals_; double desired_angle_; ros::Publisher intermediate_goal_pub_; + ros::Publisher articulation_points_pub_; }; } // namespace mir_dwb_critics diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index ae609242..34606e66 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -33,10 +33,10 @@ */ #include #include -#include #include #include #include +#include #include #include #include @@ -83,6 +83,7 @@ void PathProgressCritic::onInit() critic_nh_.param("enforce_forward_dot", enforce_forward_dot_, true); intermediate_goal_pub_ = critic_nh_.advertise("intermediate_goal", 1); + articulation_points_pub_ = critic_nh_.advertise("articulation_points", 1); articulation_angle_threshold_ = std::max(articulation_angle_threshold_, angle_threshold_); @@ -140,6 +141,8 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co return false; } + const unsigned int plan_last_index = static_cast(plan.size() - 1); + if (holding_goal_ && held_goal_index_ >= plan.size()) { ROS_DEBUG_NAMED("PathProgressCritic", "Held goal index %u is out of range for current plan of size %zu. Releasing hold.", @@ -208,7 +211,6 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } // Constrain the search range to enforce monotonic progress. - unsigned int plan_last_index = static_cast(plan.size() - 1); last_progress_index_ = std::min(last_progress_index_, plan_last_index); unsigned int search_start_index = std::max(start_index, last_progress_index_); @@ -229,6 +231,85 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co return std::max({candidate_start, articulation_scan_start, 1u}); }; + auto collectArticulationIndices = [&](unsigned int scan_start, unsigned int scan_end) { + std::vector articulation_indices; + if (plan.size() < 2) + { + return articulation_indices; + } + + const double epsilon = 1e-9; + unsigned int clamped_start = std::max(scan_start, 1u); + unsigned int clamped_end = std::min(scan_end, plan_last_index); + if (clamped_start > clamped_end) + { + return articulation_indices; + } + + double previous_segment_angle = 0.0; + bool previous_segment_angle_set = false; + unsigned int previous_segment_end_index = 0u; + + for (unsigned int i = clamped_start; i <= clamped_end; ++i) + { + double direction_x = plan[i].x - plan[i - 1].x; + double direction_y = plan[i].y - plan[i - 1].y; + double length = hypot(direction_x, direction_y); + if (length < epsilon) + { + continue; + } + + double current_angle = atan2(direction_y, direction_x); + if (previous_segment_angle_set) + { + double articulation_angle = + fabs(angles::shortest_angular_distance(previous_segment_angle, current_angle)); + if (articulation_angle >= articulation_angle_threshold_) + { + if (articulation_indices.empty() || articulation_indices.back() != previous_segment_end_index) + { + articulation_indices.push_back(previous_segment_end_index); + } + } + } + + previous_segment_angle = current_angle; + previous_segment_angle_set = true; + previous_segment_end_index = i; + } + + return articulation_indices; + }; + + auto publishArticulationPointCloud = [&](const std::vector& articulation_indices) { + if (!articulation_points_pub_) + { + return; + } + + sensor_msgs::PointCloud cloud_msg; + cloud_msg.header = global_plan.header; + cloud_msg.header.stamp = ros::Time::now(); + cloud_msg.points.reserve(articulation_indices.size()); + + for (unsigned int index : articulation_indices) + { + if (index >= plan.size()) + { + continue; + } + + geometry_msgs::Point32 point; + point.x = plan[index].x; + point.y = plan[index].y; + point.z = 0.0; + cloud_msg.points.push_back(point); + } + + articulation_points_pub_.publish(cloud_msg); + }; + auto publishIntermediateGoal = [&](const geometry_msgs::Pose2D& goal_pose, double goal_yaw) { if (!intermediate_goal_pub_) { @@ -249,6 +330,9 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co intermediate_goal_pub_.publish(goal_msg); }; + std::vector articulation_indices = collectArticulationIndices(1u, plan_last_index); + publishArticulationPointCloud(articulation_indices); + if (holding_goal_) { double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, held_goal_pose_.theta)); From 15e0c7735c943326133f447b9239bfb8b57417fd Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 08:24:39 +0200 Subject: [PATCH 07/45] Hold intermediate goals until fully reached --- mir_dwb_critics/src/path_progress.cpp | 88 ++++++++++++++++++--------- 1 file changed, 58 insertions(+), 30 deletions(-) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 34606e66..ff233971 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -210,27 +210,6 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } } - // Constrain the search range to enforce monotonic progress. - last_progress_index_ = std::min(last_progress_index_, plan_last_index); - - unsigned int search_start_index = std::max(start_index, last_progress_index_); - search_start_index = std::min(search_start_index, last_valid_index); - - unsigned int articulation_scan_start = 0u; - if (plan_last_index >= 1) - { - unsigned int next_index = last_progress_index_ < plan_last_index ? last_progress_index_ + 1 : plan_last_index; - articulation_scan_start = std::max(next_index, 1u); - } - else - { - articulation_scan_start = plan_last_index; - } - - auto articulationSearchStart = [&](unsigned int candidate_start) { - return std::max({candidate_start, articulation_scan_start, 1u}); - }; - auto collectArticulationIndices = [&](unsigned int scan_start, unsigned int scan_end) { std::vector articulation_indices; if (plan.size() < 2) @@ -335,12 +314,22 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co if (holding_goal_) { - double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, held_goal_pose_.theta)); - if (yaw_error >= final_goal_yaw_tolerance_) + unsigned int held_x = 0; + unsigned int held_y = 0; + bool held_in_costmap = worldToGridBounded(info, held_goal_pose_.x, held_goal_pose_.y, held_x, held_y); + + if (!held_in_costmap) { - unsigned int held_x = 0; - unsigned int held_y = 0; - if (worldToGridBounded(info, held_goal_pose_.x, held_goal_pose_.y, held_x, held_y)) + ROS_WARN_NAMED("PathProgressCritic", + "Held goal (index %u) is outside the local costmap. Releasing hold to search for a new goal.", + held_goal_index_); + holding_goal_ = false; + held_goal_index_ = 0; + } + else + { + double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, held_goal_pose_.theta)); + if (yaw_error >= final_goal_yaw_tolerance_) { x = held_x; y = held_y; @@ -351,19 +340,58 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co return true; } - ROS_WARN_NAMED("PathProgressCritic", - "Held goal (index %u) is outside the local costmap. Releasing hold to search for a new goal.", - held_goal_index_); + if (!isGoalReached(robot_pose, held_goal_pose_, held_goal_pose_.theta)) + { + x = held_x; + y = held_y; + desired_angle = held_goal_pose_.theta; + publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); + ROS_DEBUG_NAMED("PathProgressCritic", + "Holding goal index %u until full pose tolerance satisfied (XY + yaw)", held_goal_index_); + return true; + } + + last_progress_index_ = std::max(last_progress_index_, held_goal_index_); + geometry_msgs::Pose2D reached_pose = held_goal_pose_; + if (reached_intermediate_goals_.empty() || + nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || + fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > 1e-6) + { + reached_intermediate_goals_.push_back(reached_pose); + } + ROS_DEBUG_NAMED("PathProgressCritic", + "Reached held intermediate goal index %u while respecting pose tolerances. last_progress_index_: %u", + held_goal_index_, last_progress_index_); holding_goal_ = false; - held_goal_index_ = 0; } } + // Constrain the search range to enforce monotonic progress with the updated bookkeeping. + last_progress_index_ = std::min(last_progress_index_, plan_last_index); + + unsigned int search_start_index = std::max(start_index, last_progress_index_); + search_start_index = std::min(search_start_index, last_valid_index); + if (holding_goal_ && held_goal_index_ >= last_progress_index_) { search_start_index = std::min(search_start_index, held_goal_index_); } + unsigned int articulation_scan_start = 0u; + if (plan_last_index >= 1) + { + unsigned int next_index = last_progress_index_ < plan_last_index ? last_progress_index_ + 1 : plan_last_index; + articulation_scan_start = std::max(next_index, 1u); + } + else + { + articulation_scan_start = plan_last_index; + } + + auto articulationSearchStart = [&](unsigned int candidate_start) { + return std::max({candidate_start, articulation_scan_start, 1u}); + }; + unsigned int goal_index = search_start_index; double goal_yaw = plan[goal_index].theta; bool has_forward_direction = false; From 4d26e39e29f3b4f7d5556e49ab05beb52b09da86 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 08:29:25 +0200 Subject: [PATCH 08/45] Align final goal selection with global tolerances --- .../include/mir_dwb_critics/path_progress.h | 1 + mir_dwb_critics/src/path_progress.cpp | 25 +++++++++++++++++++ 2 files changed, 26 insertions(+) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index 64a34f95..ddef6949 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -81,6 +81,7 @@ class PathProgressCritic : public dwb_critics::MapGridCritic double xy_local_goal_tolerance_; double yaw_local_goal_tolerance_; double final_goal_yaw_tolerance_; + double final_goal_xy_tolerance_; double angle_threshold_; double articulation_angle_threshold_; double heading_scale_; diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index ff233971..2adfae42 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -77,6 +77,10 @@ void PathProgressCritic::onInit() { critic_nh_.param("yaw_goal_tolerance", final_goal_yaw_tolerance_, yaw_local_goal_tolerance_); } + if (!private_nh.getParam("xy_goal_tolerance", final_goal_xy_tolerance_)) + { + critic_nh_.param("xy_goal_tolerance", final_goal_xy_tolerance_, xy_local_goal_tolerance_); + } critic_nh_.param("angle_threshold", angle_threshold_, M_PI_4); critic_nh_.param("articulation_angle_threshold", articulation_angle_threshold_, 1.3089969389957472); critic_nh_.param("heading_scale", heading_scale_, 1.0); @@ -99,6 +103,7 @@ void PathProgressCritic::onInit() hold_position_epsilon_ = 1e-6; hold_yaw_epsilon_ = 1e-6; final_goal_yaw_tolerance_ = std::max(final_goal_yaw_tolerance_, 1e-6); + final_goal_xy_tolerance_ = std::max(final_goal_xy_tolerance_, 0.0); } void PathProgressCritic::reset() @@ -539,6 +544,26 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } } + bool pending_articulation = false; + if (last_progress_index_ < goal_index) + { + pending_articulation = std::find(articulation_indices.begin(), articulation_indices.end(), goal_index) != + articulation_indices.end(); + } + + if (!pending_articulation && final_goal_xy_tolerance_ >= 0.0 && plan_last_index <= last_valid_index) + { + double final_dx = plan[plan_last_index].x - plan[goal_index].x; + double final_dy = plan[plan_last_index].y - plan[goal_index].y; + double final_distance = hypot(final_dx, final_dy); + if (final_distance <= final_goal_xy_tolerance_) + { + goal_index = plan_last_index; + goal_yaw = plan[plan_last_index].theta; + has_forward_direction = false; + } + } + ROS_ASSERT(goal_index <= last_valid_index); worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y); From a76330ccc171f14795cff3d4302d0e37e083e96e Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 09:26:04 +0200 Subject: [PATCH 09/45] Ensure skipped articulations are recovered --- mir_dwb_critics/src/path_progress.cpp | 46 ++++++++++++++++++++++++++- 1 file changed, 45 insertions(+), 1 deletion(-) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 2adfae42..55e84e91 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -401,9 +401,53 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co double goal_yaw = plan[goal_index].theta; bool has_forward_direction = false; bool found_goal = false; + bool forced_skipped_articulation = false; + + if (search_start_index > last_progress_index_ + 1 && !articulation_indices.empty()) + { + unsigned int articulation_lower_bound = std::max(last_progress_index_ + 1, 1u); + unsigned int articulation_upper_bound = std::min(search_start_index - 1, last_valid_index); + + if (articulation_lower_bound <= articulation_upper_bound) + { + auto articulation_it = std::find_if(articulation_indices.begin(), articulation_indices.end(), + [&](unsigned int index) { + return index >= articulation_lower_bound && index <= articulation_upper_bound; + }); + + if (articulation_it != articulation_indices.end()) + { + goal_index = *articulation_it; + has_forward_direction = computeOutgoingAngle(plan, goal_index, goal_yaw); + if (!has_forward_direction) + { + goal_yaw = plan[goal_index].theta; + } + + if (enforce_forward_dot_ && has_forward_direction) + { + double to_goal_x = plan[goal_index].x - robot_pose.x; + double to_goal_y = plan[goal_index].y - robot_pose.y; + double dot = to_goal_x * std::cos(goal_yaw) + to_goal_y * std::sin(goal_yaw); + if (dot < 0.0) + { + ROS_WARN_NAMED("PathProgressCritic", + "Forcing skipped articulation index %u despite backward dot product %.3f due to policy.", + goal_index, dot); + } + } + + forced_skipped_articulation = true; + found_goal = true; + ROS_DEBUG_NAMED("PathProgressCritic", + "Recovered skipped articulation index %u between progress %u and search start %u.", + goal_index, last_progress_index_, search_start_index); + } + } + } unsigned int search_index = search_start_index; - while (search_index <= last_valid_index) + while (!forced_skipped_articulation && search_index <= last_valid_index) { double candidate_yaw = goal_yaw; bool candidate_has_forward = false; From b9bcf7d584a8b5bee4ee1779e6b81404707d1b9f Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 09:44:35 +0200 Subject: [PATCH 10/45] Visualize intermediate goal tolerance --- .../include/mir_dwb_critics/path_progress.h | 2 ++ mir_dwb_critics/src/path_progress.cpp | 31 +++++++++++++++++++ 2 files changed, 33 insertions(+) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index ddef6949..d8bceda2 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -98,6 +99,7 @@ class PathProgressCritic : public dwb_critics::MapGridCritic double desired_angle_; ros::Publisher intermediate_goal_pub_; ros::Publisher articulation_points_pub_; + ros::Publisher intermediate_goal_tolerance_pub_; }; } // namespace mir_dwb_critics diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 55e84e91..7b2167b1 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -88,6 +89,8 @@ void PathProgressCritic::onInit() intermediate_goal_pub_ = critic_nh_.advertise("intermediate_goal", 1); articulation_points_pub_ = critic_nh_.advertise("articulation_points", 1); + intermediate_goal_tolerance_pub_ = + critic_nh_.advertise("intermediate_goal_tolerance", 1); articulation_angle_threshold_ = std::max(articulation_angle_threshold_, angle_threshold_); @@ -312,6 +315,34 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co goal_msg.pose.orientation.z = q.z(); goal_msg.pose.orientation.w = q.w(); intermediate_goal_pub_.publish(goal_msg); + + if (intermediate_goal_tolerance_pub_) + { + visualization_msgs::MarkerArray marker_array; + visualization_msgs::Marker marker; + marker.header = goal_msg.header; + marker.header.stamp = goal_msg.header.stamp; + marker.ns = "intermediate_goal_tolerance"; + marker.id = 0; + marker.type = visualization_msgs::Marker::SPHERE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position = goal_msg.pose.position; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + double diameter = std::max(2.0 * final_goal_xy_tolerance_, 1e-6); + marker.scale.x = diameter; + marker.scale.y = diameter; + marker.scale.z = diameter; + marker.color.r = 0.2f; + marker.color.g = 0.8f; + marker.color.b = 0.4f; + marker.color.a = 0.35f; + marker.lifetime = ros::Duration(0.0); + marker_array.markers.push_back(marker); + intermediate_goal_tolerance_pub_.publish(marker_array); + } }; std::vector articulation_indices = collectArticulationIndices(1u, plan_last_index); From 57b58a12d8afaabb22985f3cfe07f76340686d5e Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 16:05:58 +0200 Subject: [PATCH 11/45] Scale tolerance marker with local goal tolerance --- mir_dwb_critics/src/path_progress.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 7b2167b1..6bb60784 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -331,7 +331,7 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; marker.pose.orientation.w = 1.0; - double diameter = std::max(2.0 * final_goal_xy_tolerance_, 1e-6); + double diameter = std::max(2.0 * xy_local_goal_tolerance_, 1e-6); marker.scale.x = diameter; marker.scale.y = diameter; marker.scale.z = diameter; From 256949c90adbbc34d9fe3d41b0ebdf43aabc5f53 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 19:07:56 +0200 Subject: [PATCH 12/45] enforce intermediate goals in articulatiions --- .../include/mir_dwb_critics/path_progress.h | 1 + mir_dwb_critics/src/path_progress.cpp | 48 ++++++++++++++++++- 2 files changed, 47 insertions(+), 2 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index d8bceda2..d4904b27 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -87,6 +87,7 @@ class PathProgressCritic : public dwb_critics::MapGridCritic double articulation_angle_threshold_; double heading_scale_; bool enforce_forward_dot_; + bool always_target_articulations_; unsigned int last_progress_index_; bool holding_goal_; diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 6bb60784..f4f0166f 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -86,6 +86,7 @@ void PathProgressCritic::onInit() critic_nh_.param("articulation_angle_threshold", articulation_angle_threshold_, 1.3089969389957472); critic_nh_.param("heading_scale", heading_scale_, 1.0); critic_nh_.param("enforce_forward_dot", enforce_forward_dot_, true); + critic_nh_.param("always_target_articulations", always_target_articulations_, true); intermediate_goal_pub_ = critic_nh_.advertise("intermediate_goal", 1); articulation_points_pub_ = critic_nh_.advertise("articulation_points", 1); @@ -434,7 +435,50 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co bool found_goal = false; bool forced_skipped_articulation = false; - if (search_start_index > last_progress_index_ + 1 && !articulation_indices.empty()) + if (always_target_articulations_ && !articulation_indices.empty()) + { + unsigned int window_start = std::max(search_start_index, last_progress_index_ + 1); + unsigned int window_end = last_valid_index; + + for (unsigned int idx : articulation_indices) + { + if (idx < window_start || idx > window_end) + { + continue; + } + + double articulation_yaw = plan[idx].theta; + bool articulation_has_forward = computeOutgoingAngle(plan, idx, articulation_yaw); + double goal_candidate_yaw = articulation_has_forward ? articulation_yaw : plan[idx].theta; + + if (isGoalReached(robot_pose, plan[idx], goal_candidate_yaw)) + { + last_progress_index_ = std::max(last_progress_index_, idx); + geometry_msgs::Pose2D reached_pose = plan[idx]; + reached_pose.theta = goal_candidate_yaw; + if (reached_intermediate_goals_.empty() || + nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || + fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > + 1e-6) + { + reached_intermediate_goals_.push_back(reached_pose); + } + continue; + } + + goal_index = idx; + goal_yaw = goal_candidate_yaw; + has_forward_direction = articulation_has_forward; + found_goal = true; + forced_skipped_articulation = true; + ROS_DEBUG_NAMED("PathProgressCritic", + "Selecting articulation index %u as prioritized intermediate goal. last_progress_index_: %u", + goal_index, last_progress_index_); + break; + } + } + + if (!found_goal && search_start_index > last_progress_index_ + 1 && !articulation_indices.empty()) { unsigned int articulation_lower_bound = std::max(last_progress_index_ + 1, 1u); unsigned int articulation_upper_bound = std::min(search_start_index - 1, last_valid_index); @@ -607,7 +651,7 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co unsigned int articulation_index = 0; double articulation_yaw = goal_yaw; bool articulation_has_forward = has_forward_direction; - unsigned int articulation_start_index = articulationSearchStart(goal_index); + unsigned int articulation_start_index = articulationSearchStart(search_start_index); if (articulation_start_index <= goal_index && findNextArticulation(plan, articulation_start_index, goal_index, last_valid_index, articulation_index, articulation_yaw, articulation_has_forward)) From e8857842b35313fc0a04bd337ecf213a0197f77b Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 19:09:23 +0200 Subject: [PATCH 13/45] add initial base rotational path alignment before starting to move linearly --- .../include/mir_dwb_critics/path_progress.h | 1 + mir_dwb_critics/src/path_progress.cpp | 30 +++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index d4904b27..668a5e61 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -88,6 +88,7 @@ class PathProgressCritic : public dwb_critics::MapGridCritic double heading_scale_; bool enforce_forward_dot_; bool always_target_articulations_; + bool initial_alignment_done_; unsigned int last_progress_index_; bool holding_goal_; diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index f4f0166f..d32cf717 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -87,6 +87,7 @@ void PathProgressCritic::onInit() critic_nh_.param("heading_scale", heading_scale_, 1.0); critic_nh_.param("enforce_forward_dot", enforce_forward_dot_, true); critic_nh_.param("always_target_articulations", always_target_articulations_, true); + initial_alignment_done_ = false; intermediate_goal_pub_ = critic_nh_.advertise("intermediate_goal", 1); articulation_points_pub_ = critic_nh_.advertise("articulation_points", 1); @@ -119,6 +120,7 @@ void PathProgressCritic::reset() held_goal_pose_.x = 0.0; held_goal_pose_.y = 0.0; held_goal_pose_.theta = 0.0; + initial_alignment_done_ = false; } double PathProgressCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj) @@ -152,6 +154,34 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co const unsigned int plan_last_index = static_cast(plan.size() - 1); + if (!initial_alignment_done_) + { + double desired_initial_yaw = plan.front().theta; + bool reached_alignment = isGoalReached(robot_pose, plan.front(), desired_initial_yaw); + if (!reached_alignment) + { + unsigned int initial_x = 0; + unsigned int initial_y = 0; + if (worldToGridBounded(info, plan.front().x, plan.front().y, initial_x, initial_y)) + { + x = initial_x; + y = initial_y; + desired_angle = desired_initial_yaw; + held_goal_pose_ = plan.front(); + held_goal_pose_.theta = desired_initial_yaw; + held_goal_index_ = 0; + holding_goal_ = true; + publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); + ROS_DEBUG_NAMED("PathProgressCritic", + "Holding initial alignment goal at plan index 0 (x: %.3f, y: %.3f, yaw: %.3f rad)", + held_goal_pose_.x, held_goal_pose_.y, held_goal_pose_.theta); + return true; + } + } + + initial_alignment_done_ = true; + } + if (holding_goal_ && held_goal_index_ >= plan.size()) { ROS_DEBUG_NAMED("PathProgressCritic", "Held goal index %u is out of range for current plan of size %zu. Releasing hold.", From 3d3b54ea7d09ab0f7bbc07e3b44ea27a87800f05 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 19:12:18 +0200 Subject: [PATCH 14/45] add missing lines of code on path progress --- mir_dwb_critics/src/path_progress.cpp | 56 +++++++++++++-------------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index d32cf717..bbc197f5 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -154,34 +154,6 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co const unsigned int plan_last_index = static_cast(plan.size() - 1); - if (!initial_alignment_done_) - { - double desired_initial_yaw = plan.front().theta; - bool reached_alignment = isGoalReached(robot_pose, plan.front(), desired_initial_yaw); - if (!reached_alignment) - { - unsigned int initial_x = 0; - unsigned int initial_y = 0; - if (worldToGridBounded(info, plan.front().x, plan.front().y, initial_x, initial_y)) - { - x = initial_x; - y = initial_y; - desired_angle = desired_initial_yaw; - held_goal_pose_ = plan.front(); - held_goal_pose_.theta = desired_initial_yaw; - held_goal_index_ = 0; - holding_goal_ = true; - publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); - ROS_DEBUG_NAMED("PathProgressCritic", - "Holding initial alignment goal at plan index 0 (x: %.3f, y: %.3f, yaw: %.3f rad)", - held_goal_pose_.x, held_goal_pose_.y, held_goal_pose_.theta); - return true; - } - } - - initial_alignment_done_ = true; - } - if (holding_goal_ && held_goal_index_ >= plan.size()) { ROS_DEBUG_NAMED("PathProgressCritic", "Held goal index %u is out of range for current plan of size %zu. Releasing hold.", @@ -376,6 +348,34 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } }; + if (!initial_alignment_done_) + { + double desired_initial_yaw = plan.front().theta; + bool reached_alignment = isGoalReached(robot_pose, plan.front(), desired_initial_yaw); + if (!reached_alignment) + { + unsigned int initial_x = 0; + unsigned int initial_y = 0; + if (worldToGridBounded(info, plan.front().x, plan.front().y, initial_x, initial_y)) + { + x = initial_x; + y = initial_y; + desired_angle = desired_initial_yaw; + held_goal_pose_ = plan.front(); + held_goal_pose_.theta = desired_initial_yaw; + held_goal_index_ = 0; + holding_goal_ = true; + publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); + ROS_DEBUG_NAMED("PathProgressCritic", + "Holding initial alignment goal at plan index 0 (x: %.3f, y: %.3f, yaw: %.3f rad)", + held_goal_pose_.x, held_goal_pose_.y, held_goal_pose_.theta); + return true; + } + } + + initial_alignment_done_ = true; + } + std::vector articulation_indices = collectArticulationIndices(1u, plan_last_index); publishArticulationPointCloud(articulation_indices); From 800382f590e2c9642a3403160b5356d65deb57d9 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 19:26:59 +0200 Subject: [PATCH 15/45] merge intermediate goals and articulation goals --- mir_dwb_critics/src/path_progress.cpp | 60 ++++++++++++++++++++------- 1 file changed, 46 insertions(+), 14 deletions(-) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index bbc197f5..e16e9cb9 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -465,27 +465,32 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co bool found_goal = false; bool forced_skipped_articulation = false; + unsigned int next_pending_articulation = 0; + double next_pending_articulation_yaw = 0.0; + bool next_pending_articulation_has_forward = false; + bool has_pending_articulation = false; + if (always_target_articulations_ && !articulation_indices.empty()) { - unsigned int window_start = std::max(search_start_index, last_progress_index_ + 1); - unsigned int window_end = last_valid_index; + unsigned int articulation_window_start = std::max(search_start_index, last_progress_index_ + 1); + unsigned int articulation_window_end = last_valid_index; for (unsigned int idx : articulation_indices) { - if (idx < window_start || idx > window_end) + if (idx < articulation_window_start || idx > articulation_window_end) { continue; } double articulation_yaw = plan[idx].theta; bool articulation_has_forward = computeOutgoingAngle(plan, idx, articulation_yaw); - double goal_candidate_yaw = articulation_has_forward ? articulation_yaw : plan[idx].theta; + double articulation_goal_yaw = articulation_has_forward ? articulation_yaw : plan[idx].theta; - if (isGoalReached(robot_pose, plan[idx], goal_candidate_yaw)) + if (isGoalReached(robot_pose, plan[idx], articulation_goal_yaw)) { last_progress_index_ = std::max(last_progress_index_, idx); geometry_msgs::Pose2D reached_pose = plan[idx]; - reached_pose.theta = goal_candidate_yaw; + reached_pose.theta = articulation_goal_yaw; if (reached_intermediate_goals_.empty() || nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > @@ -496,14 +501,10 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co continue; } - goal_index = idx; - goal_yaw = goal_candidate_yaw; - has_forward_direction = articulation_has_forward; - found_goal = true; - forced_skipped_articulation = true; - ROS_DEBUG_NAMED("PathProgressCritic", - "Selecting articulation index %u as prioritized intermediate goal. last_progress_index_: %u", - goal_index, last_progress_index_); + next_pending_articulation = idx; + next_pending_articulation_yaw = articulation_goal_yaw; + next_pending_articulation_has_forward = articulation_has_forward; + has_pending_articulation = true; break; } } @@ -628,11 +629,42 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } } + if (has_pending_articulation && candidate_index > next_pending_articulation) + { + goal_index = next_pending_articulation; + goal_yaw = next_pending_articulation_yaw; + has_forward_direction = next_pending_articulation_has_forward; + found_goal = true; + forced_skipped_articulation = true; + ROS_DEBUG_NAMED("PathProgressCritic", + "Switching to pending articulation index %u ahead of candidate %u.", goal_index, + candidate_index); + break; + } + goal_index = candidate_index; goal_yaw = candidate_yaw; has_forward_direction = candidate_has_forward; found_goal = true; break; + if (has_pending_articulation && goal_index > next_pending_articulation) + { + goal_index = next_pending_articulation; + goal_yaw = next_pending_articulation_yaw; + has_forward_direction = next_pending_articulation_has_forward; + forced_skipped_articulation = true; + ROS_DEBUG_NAMED("PathProgressCritic", + "Selecting pending articulation index %u during fallback after candidate %u.", goal_index, + candidate_index); + } + else + { + goal_index = candidate_index; + goal_yaw = candidate_yaw; + has_forward_direction = candidate_has_forward; + } + found_goal = true; + break; } if (!found_goal) From 1059e6d3be58e32758c0281145f744430cc8295b Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 20:17:55 +0200 Subject: [PATCH 16/45] bring back initial rotation and prioritize articulation goals over intermediate goals --- mir_dwb_critics/src/path_progress.cpp | 75 ++++++++++++--------------- 1 file changed, 34 insertions(+), 41 deletions(-) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index e16e9cb9..a5ec16b1 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -464,13 +464,12 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co bool has_forward_direction = false; bool found_goal = false; bool forced_skipped_articulation = false; + unsigned int next_articulation_index = 0; + double next_articulation_yaw = 0.0; + bool next_articulation_has_forward = false; + bool has_next_articulation = false; - unsigned int next_pending_articulation = 0; - double next_pending_articulation_yaw = 0.0; - bool next_pending_articulation_has_forward = false; - bool has_pending_articulation = false; - - if (always_target_articulations_ && !articulation_indices.empty()) + if (initial_alignment_done_ && !articulation_indices.empty()) { unsigned int articulation_window_start = std::max(search_start_index, last_progress_index_ + 1); unsigned int articulation_window_end = last_valid_index; @@ -501,10 +500,10 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co continue; } - next_pending_articulation = idx; - next_pending_articulation_yaw = articulation_goal_yaw; - next_pending_articulation_has_forward = articulation_has_forward; - has_pending_articulation = true; + next_articulation_index = idx; + next_articulation_yaw = articulation_goal_yaw; + next_articulation_has_forward = articulation_has_forward; + has_next_articulation = true; break; } } @@ -583,6 +582,20 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co candidate_index = std::max(candidate_index, search_index); } + if (has_next_articulation && candidate_index >= next_articulation_index) + { + goal_index = next_articulation_index; + goal_yaw = next_articulation_yaw; + has_forward_direction = next_articulation_has_forward; + found_goal = true; + forced_skipped_articulation = true; + has_next_articulation = false; + ROS_DEBUG_NAMED("PathProgressCritic", + "Selecting pending articulation index %u reached at candidate %u.", goal_index, + candidate_index); + break; + } + if (isGoalReached(robot_pose, plan[candidate_index], candidate_yaw)) { last_progress_index_ = std::max(last_progress_index_, candidate_index); @@ -629,42 +642,11 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } } - if (has_pending_articulation && candidate_index > next_pending_articulation) - { - goal_index = next_pending_articulation; - goal_yaw = next_pending_articulation_yaw; - has_forward_direction = next_pending_articulation_has_forward; - found_goal = true; - forced_skipped_articulation = true; - ROS_DEBUG_NAMED("PathProgressCritic", - "Switching to pending articulation index %u ahead of candidate %u.", goal_index, - candidate_index); - break; - } - goal_index = candidate_index; goal_yaw = candidate_yaw; has_forward_direction = candidate_has_forward; found_goal = true; break; - if (has_pending_articulation && goal_index > next_pending_articulation) - { - goal_index = next_pending_articulation; - goal_yaw = next_pending_articulation_yaw; - has_forward_direction = next_pending_articulation_has_forward; - forced_skipped_articulation = true; - ROS_DEBUG_NAMED("PathProgressCritic", - "Selecting pending articulation index %u during fallback after candidate %u.", goal_index, - candidate_index); - } - else - { - goal_index = candidate_index; - goal_yaw = candidate_yaw; - has_forward_direction = candidate_has_forward; - } - found_goal = true; - break; } if (!found_goal) @@ -708,6 +690,17 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co return false; } + if (has_next_articulation && goal_index >= next_articulation_index) + { + goal_index = next_articulation_index; + goal_yaw = next_articulation_yaw; + has_forward_direction = next_articulation_has_forward; + has_next_articulation = false; + forced_skipped_articulation = true; + ROS_DEBUG_NAMED("PathProgressCritic", + "Fallback switching to pending articulation index %u.", goal_index); + } + if (goal_index > last_progress_index_) { unsigned int articulation_index = 0; From 5073d133f4ae953bb6df15518e476ffd46f3b90f Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 22:13:45 +0200 Subject: [PATCH 17/45] =?UTF-8?q?Adjusted=20the=20final-goal=20snapping=20?= =?UTF-8?q?logic=20so=20we=20only=20switch=20to=20the=20global=20plan?= =?UTF-8?q?=E2=80=99s=20last=20pose=20when=20that=20pose=20has=20already?= =?UTF-8?q?=20been=20selected=20as=20the=20current=20intermediate=20goal.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mir_dwb_critics/src/path_progress.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index a5ec16b1..134ef9ca 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -725,14 +725,14 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co articulation_indices.end(); } - if (!pending_articulation && final_goal_xy_tolerance_ >= 0.0 && plan_last_index <= last_valid_index) + if (!pending_articulation && final_goal_xy_tolerance_ >= 0.0 && plan_last_index <= last_valid_index && + goal_index == plan_last_index) { double final_dx = plan[plan_last_index].x - plan[goal_index].x; double final_dy = plan[plan_last_index].y - plan[goal_index].y; double final_distance = hypot(final_dx, final_dy); if (final_distance <= final_goal_xy_tolerance_) { - goal_index = plan_last_index; goal_yaw = plan[plan_last_index].theta; has_forward_direction = false; } From 6886d60bc4c38f92c870598added579ed5c28a29 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 22:27:25 +0200 Subject: [PATCH 18/45] prevent goal to steal the intermediate goal and follow the path instead, make sure first robot only rotates in place to align with path --- .../include/mir_dwb_critics/path_progress.h | 9 +++ mir_dwb_critics/src/path_progress.cpp | 71 ++++++++++++++++--- 2 files changed, 69 insertions(+), 11 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index 668a5e61..a7ad5a08 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -39,6 +39,8 @@ #include #include #include +#include +#include #include namespace mir_dwb_critics @@ -90,6 +92,13 @@ class PathProgressCritic : public dwb_critics::MapGridCritic bool always_target_articulations_; bool initial_alignment_done_; + // Plan change detection to reset internal state on new global plans + bool have_last_plan_; + size_t last_plan_size_; + geometry_msgs::Pose2D last_plan_end_pose_; + std::string last_plan_frame_id_; + ros::Time last_plan_stamp_; + unsigned int last_progress_index_; bool holding_goal_; unsigned int held_goal_index_; diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 134ef9ca..83213a10 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -109,6 +109,13 @@ void PathProgressCritic::onInit() hold_yaw_epsilon_ = 1e-6; final_goal_yaw_tolerance_ = std::max(final_goal_yaw_tolerance_, 1e-6); final_goal_xy_tolerance_ = std::max(final_goal_xy_tolerance_, 0.0); + have_last_plan_ = false; + last_plan_size_ = 0; + last_plan_end_pose_.x = 0.0; + last_plan_end_pose_.y = 0.0; + last_plan_end_pose_.theta = 0.0; + last_plan_frame_id_.clear(); + last_plan_stamp_ = ros::Time(0); } void PathProgressCritic::reset() @@ -145,6 +152,44 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } std::vector plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses; + // Reset state if a new global plan arrives (size, last pose, frame or stamp changes) + bool plan_changed = false; + if (!have_last_plan_) + { + plan_changed = true; + } + else + { + if (last_plan_size_ != plan.size()) plan_changed = true; + if (global_plan.header.frame_id != last_plan_frame_id_) plan_changed = true; + if (global_plan.header.stamp != last_plan_stamp_) plan_changed = true; + const geometry_msgs::Pose2D& end_pose = plan.back(); + if (fabs(end_pose.x - last_plan_end_pose_.x) > 1e-6 || fabs(end_pose.y - last_plan_end_pose_.y) > 1e-6 || + fabs(angles::shortest_angular_distance(end_pose.theta, last_plan_end_pose_.theta)) > 1e-6) + { + plan_changed = true; + } + } + + if (plan_changed) + { + // Abort everything and start over + reached_intermediate_goals_.clear(); + last_progress_index_ = 0; + holding_goal_ = false; + held_goal_index_ = 0; + held_goal_pose_.x = 0.0; + held_goal_pose_.y = 0.0; + held_goal_pose_.theta = 0.0; + initial_alignment_done_ = false; + + have_last_plan_ = true; + last_plan_size_ = plan.size(); + last_plan_end_pose_ = plan.back(); + last_plan_frame_id_ = global_plan.header.frame_id; + last_plan_stamp_ = global_plan.header.stamp; + } + if (plan.empty()) { @@ -351,24 +396,25 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co if (!initial_alignment_done_) { double desired_initial_yaw = plan.front().theta; - bool reached_alignment = isGoalReached(robot_pose, plan.front(), desired_initial_yaw); - if (!reached_alignment) + double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, desired_initial_yaw)); + if (yaw_error >= yaw_local_goal_tolerance_) { - unsigned int initial_x = 0; - unsigned int initial_y = 0; - if (worldToGridBounded(info, plan.front().x, plan.front().y, initial_x, initial_y)) + unsigned int rx = 0; + unsigned int ry = 0; + if (worldToGridBounded(info, robot_pose.x, robot_pose.y, rx, ry)) { - x = initial_x; - y = initial_y; + x = rx; + y = ry; desired_angle = desired_initial_yaw; - held_goal_pose_ = plan.front(); + held_goal_pose_.x = robot_pose.x; + held_goal_pose_.y = robot_pose.y; held_goal_pose_.theta = desired_initial_yaw; held_goal_index_ = 0; holding_goal_ = true; publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); ROS_DEBUG_NAMED("PathProgressCritic", - "Holding initial alignment goal at plan index 0 (x: %.3f, y: %.3f, yaw: %.3f rad)", - held_goal_pose_.x, held_goal_pose_.y, held_goal_pose_.theta); + "Initial alignment: rotate in place to yaw %.3f rad at robot pose (x: %.3f, y: %.3f)", + held_goal_pose_.theta, held_goal_pose_.x, held_goal_pose_.y); return true; } } @@ -508,7 +554,8 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } } - if (!found_goal && search_start_index > last_progress_index_ + 1 && !articulation_indices.empty()) + if (!found_goal && initial_alignment_done_ && search_start_index > last_progress_index_ + 1 && + !articulation_indices.empty()) { unsigned int articulation_lower_bound = std::max(last_progress_index_ + 1, 1u); unsigned int articulation_upper_bound = std::min(search_start_index - 1, last_valid_index); @@ -725,6 +772,8 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co articulation_indices.end(); } + // Only consider snapping to the final goal yaw when we have already selected the last path index as goal. + // Otherwise, keep following the path even if the final goal is within tolerance. if (!pending_articulation && final_goal_xy_tolerance_ >= 0.0 && plan_last_index <= last_valid_index && goal_index == plan_last_index) { From 8121278fc523e30c5d4f11aebbc3539423696caa Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 22:38:20 +0200 Subject: [PATCH 19/45] Added pruning of the global plan to prevent oscillations --- mir_dwb_critics/src/path_progress.cpp | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 83213a10..5e47e774 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -197,7 +197,31 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co return false; } - const unsigned int plan_last_index = static_cast(plan.size() - 1); + unsigned int plan_last_index = static_cast(plan.size() - 1); + + if (initial_alignment_done_ && plan.size() > 1 && last_progress_index_ > 0) + { + unsigned int prune_limit = std::min(last_progress_index_, plan_last_index); + if (prune_limit > 0) + { + plan.erase(plan.begin(), plan.begin() + prune_limit); + last_progress_index_ = last_progress_index_ >= prune_limit ? last_progress_index_ - prune_limit : 0; + plan_last_index = static_cast(plan.size() - 1); + + if (holding_goal_) + { + if (held_goal_index_ < prune_limit) + { + holding_goal_ = false; + held_goal_index_ = 0; + } + else + { + held_goal_index_ -= prune_limit; + } + } + } + } if (holding_goal_ && held_goal_index_ >= plan.size()) { From 8e8885cd3fca42e5f613aa042d5f8da8e3640613 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 22:57:27 +0200 Subject: [PATCH 20/45] Avoid resetting path progress on timestamp-only plan updates --- .../include/mir_dwb_critics/path_progress.h | 4 ++ mir_dwb_critics/src/path_progress.cpp | 47 ++++++++++++++----- 2 files changed, 40 insertions(+), 11 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index a7ad5a08..f7e4327d 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -98,6 +98,10 @@ class PathProgressCritic : public dwb_critics::MapGridCritic geometry_msgs::Pose2D last_plan_end_pose_; std::string last_plan_frame_id_; ros::Time last_plan_stamp_; + std::vector last_plan_; + + double plan_position_epsilon_; + double plan_yaw_epsilon_; unsigned int last_progress_index_; bool holding_goal_; diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 5e47e774..2ce3c921 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -116,6 +116,9 @@ void PathProgressCritic::onInit() last_plan_end_pose_.theta = 0.0; last_plan_frame_id_.clear(); last_plan_stamp_ = ros::Time(0); + last_plan_.clear(); + plan_position_epsilon_ = 1e-4; + plan_yaw_epsilon_ = 1e-4; } void PathProgressCritic::reset() @@ -152,7 +155,28 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } std::vector plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses; - // Reset state if a new global plan arrives (size, last pose, frame or stamp changes) + + auto plansEqual = [&](const std::vector& a, + const std::vector& b) { + if (a.size() != b.size()) + { + return false; + } + + for (size_t idx = 0; idx < a.size(); ++idx) + { + if (fabs(a[idx].x - b[idx].x) > plan_position_epsilon_ || + fabs(a[idx].y - b[idx].y) > plan_position_epsilon_ || + fabs(angles::shortest_angular_distance(a[idx].theta, b[idx].theta)) > plan_yaw_epsilon_) + { + return false; + } + } + + return true; + }; + + // Reset state if a new global plan arrives (size, geometry, or frame changes) bool plan_changed = false; if (!have_last_plan_) { @@ -160,12 +184,11 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } else { - if (last_plan_size_ != plan.size()) plan_changed = true; - if (global_plan.header.frame_id != last_plan_frame_id_) plan_changed = true; - if (global_plan.header.stamp != last_plan_stamp_) plan_changed = true; - const geometry_msgs::Pose2D& end_pose = plan.back(); - if (fabs(end_pose.x - last_plan_end_pose_.x) > 1e-6 || fabs(end_pose.y - last_plan_end_pose_.y) > 1e-6 || - fabs(angles::shortest_angular_distance(end_pose.theta, last_plan_end_pose_.theta)) > 1e-6) + if (global_plan.header.frame_id != last_plan_frame_id_) + { + plan_changed = true; + } + else if (!plansEqual(plan, last_plan_)) { plan_changed = true; } @@ -184,12 +207,14 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co initial_alignment_done_ = false; have_last_plan_ = true; - last_plan_size_ = plan.size(); - last_plan_end_pose_ = plan.back(); - last_plan_frame_id_ = global_plan.header.frame_id; - last_plan_stamp_ = global_plan.header.stamp; } + last_plan_size_ = plan.size(); + last_plan_end_pose_ = plan.back(); + last_plan_frame_id_ = global_plan.header.frame_id; + last_plan_stamp_ = global_plan.header.stamp; + last_plan_ = plan; + if (plan.empty()) { From acac85485aba281d05efac654f4740a8074cfc8a Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 23:10:57 +0200 Subject: [PATCH 21/45] Preserve plan progress when geometry-only replans arrive --- .../include/mir_dwb_critics/path_progress.h | 2 + mir_dwb_critics/src/path_progress.cpp | 130 ++++++++++++++++-- 2 files changed, 123 insertions(+), 9 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index f7e4327d..7dc5307b 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -102,6 +102,8 @@ class PathProgressCritic : public dwb_critics::MapGridCritic double plan_position_epsilon_; double plan_yaw_epsilon_; + double plan_alignment_position_tolerance_; + double plan_alignment_yaw_tolerance_; unsigned int last_progress_index_; bool holding_goal_; diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 2ce3c921..a99bbf0d 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include namespace mir_dwb_critics @@ -119,6 +120,8 @@ void PathProgressCritic::onInit() last_plan_.clear(); plan_position_epsilon_ = 1e-4; plan_yaw_epsilon_ = 1e-4; + critic_nh_.param("plan_alignment_position_tolerance", plan_alignment_position_tolerance_, 0.15); + critic_nh_.param("plan_alignment_yaw_tolerance", plan_alignment_yaw_tolerance_, 3.14159265358979323846); } void PathProgressCritic::reset() @@ -176,6 +179,37 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co return true; }; + auto matchPoseToPlanIndex = [&](const geometry_msgs::Pose2D& pose, unsigned int& index_out) { + bool found = false; + double best_distance = std::numeric_limits::infinity(); + + for (unsigned int idx = 0; idx < plan.size(); ++idx) + { + double dx = plan[idx].x - pose.x; + double dy = plan[idx].y - pose.y; + double distance = hypot(dx, dy); + if (distance > plan_alignment_position_tolerance_) + { + continue; + } + + double yaw_error = fabs(angles::shortest_angular_distance(plan[idx].theta, pose.theta)); + if (yaw_error > plan_alignment_yaw_tolerance_) + { + continue; + } + + if (distance < best_distance) + { + best_distance = distance; + index_out = idx; + found = true; + } + } + + return found; + }; + // Reset state if a new global plan arrives (size, geometry, or frame changes) bool plan_changed = false; if (!have_last_plan_) @@ -196,15 +230,93 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co if (plan_changed) { - // Abort everything and start over - reached_intermediate_goals_.clear(); - last_progress_index_ = 0; - holding_goal_ = false; - held_goal_index_ = 0; - held_goal_pose_.x = 0.0; - held_goal_pose_.y = 0.0; - held_goal_pose_.theta = 0.0; - initial_alignment_done_ = false; + bool state_preserved = false; + bool progress_match_found = false; + unsigned int restored_progress_index = 0u; + std::vector> preserved_reached; + preserved_reached.reserve(reached_intermediate_goals_.size()); + + unsigned int previous_progress_index = last_progress_index_; + + for (const auto& reached_pose : reached_intermediate_goals_) + { + unsigned int matched_index = 0u; + if (matchPoseToPlanIndex(reached_pose, matched_index)) + { + geometry_msgs::Pose2D preserved_pose = reached_pose; + preserved_pose.x = plan[matched_index].x; + preserved_pose.y = plan[matched_index].y; + preserved_reached.emplace_back(matched_index, preserved_pose); + state_preserved = true; + progress_match_found = true; + restored_progress_index = std::max(restored_progress_index, matched_index); + } + } + + unsigned int held_match_index = 0u; + if (holding_goal_) + { + if (matchPoseToPlanIndex(held_goal_pose_, held_match_index)) + { + state_preserved = true; + held_goal_index_ = held_match_index; + held_goal_pose_.x = plan[held_match_index].x; + held_goal_pose_.y = plan[held_match_index].y; + } + else + { + holding_goal_ = false; + held_goal_index_ = 0u; + held_goal_pose_.x = 0.0; + held_goal_pose_.y = 0.0; + held_goal_pose_.theta = 0.0; + } + } + + unsigned int robot_match_index = 0u; + if (matchPoseToPlanIndex(robot_pose, robot_match_index)) + { + state_preserved = true; + progress_match_found = true; + restored_progress_index = std::max(restored_progress_index, robot_match_index); + } + + if (state_preserved) + { + if (progress_match_found) + { + std::sort(preserved_reached.begin(), preserved_reached.end(), + [](const std::pair& lhs, + const std::pair& rhs) { + return lhs.first < rhs.first; + }); + reached_intermediate_goals_.clear(); + reached_intermediate_goals_.reserve(preserved_reached.size()); + for (const auto& entry : preserved_reached) + { + reached_intermediate_goals_.push_back(entry.second); + } + unsigned int plan_last_index = plan.empty() ? 0u : static_cast(plan.size() - 1); + last_progress_index_ = std::min(restored_progress_index, plan_last_index); + } + else + { + reached_intermediate_goals_.clear(); + unsigned int plan_last_index = plan.empty() ? 0u : static_cast(plan.size() - 1); + last_progress_index_ = std::min(previous_progress_index, plan_last_index); + } + } + else + { + reached_intermediate_goals_.clear(); + last_progress_index_ = 0u; + holding_goal_ = false; + held_goal_index_ = 0u; + held_goal_pose_.x = 0.0; + held_goal_pose_.y = 0.0; + held_goal_pose_.theta = 0.0; + initial_alignment_done_ = false; + } have_last_plan_ = true; } From 2cebf9d63a833e3cd0160c5f11d0f7546308d138 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Sun, 12 Oct 2025 23:29:14 +0200 Subject: [PATCH 22/45] Tighten orientation handling for path progress --- .../include/mir_dwb_critics/path_progress.h | 3 ++ mir_dwb_critics/src/path_progress.cpp | 43 +++++++++++++++---- 2 files changed, 38 insertions(+), 8 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index 7dc5307b..f6430021 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -78,6 +78,9 @@ class PathProgressCritic : public dwb_critics::MapGridCritic bool computeOutgoingAngle(const std::vector& plan, unsigned int index, double& angle) const; + bool isPoseReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, + double goal_yaw, double xy_tolerance, double yaw_tolerance) const; + bool isGoalReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, double goal_yaw) const; diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index a99bbf0d..7ec972fe 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -557,6 +557,15 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co if (!initial_alignment_done_) { double desired_initial_yaw = plan.front().theta; + if (!plan.empty()) + { + double outgoing_angle = desired_initial_yaw; + if (computeOutgoingAngle(plan, 0u, outgoing_angle)) + { + desired_initial_yaw = outgoing_angle; + } + } + double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, desired_initial_yaw)); if (yaw_error >= yaw_local_goal_tolerance_) { @@ -602,19 +611,22 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } else { + double held_xy_tolerance = (held_goal_index_ == plan_last_index) ? final_goal_xy_tolerance_ : xy_local_goal_tolerance_; + double held_yaw_tolerance = (held_goal_index_ == plan_last_index) ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; + double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, held_goal_pose_.theta)); - if (yaw_error >= final_goal_yaw_tolerance_) + if (yaw_error >= held_yaw_tolerance) { x = held_x; y = held_y; desired_angle = held_goal_pose_.theta; publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); ROS_DEBUG_NAMED("PathProgressCritic", "Holding goal index %u due to yaw error %.3f rad (threshold %.3f)", - held_goal_index_, yaw_error, final_goal_yaw_tolerance_); + held_goal_index_, yaw_error, held_yaw_tolerance); return true; } - if (!isGoalReached(robot_pose, held_goal_pose_, held_goal_pose_.theta)) + if (!isPoseReached(robot_pose, held_goal_pose_, held_goal_pose_.theta, held_xy_tolerance, held_yaw_tolerance)) { x = held_x; y = held_y; @@ -692,7 +704,11 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co bool articulation_has_forward = computeOutgoingAngle(plan, idx, articulation_yaw); double articulation_goal_yaw = articulation_has_forward ? articulation_yaw : plan[idx].theta; - if (isGoalReached(robot_pose, plan[idx], articulation_goal_yaw)) + double articulation_xy_tolerance = (idx == plan_last_index) ? final_goal_xy_tolerance_ : xy_local_goal_tolerance_; + double articulation_yaw_tolerance = (idx == plan_last_index) ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; + + if (isPoseReached(robot_pose, plan[idx], articulation_goal_yaw, articulation_xy_tolerance, + articulation_yaw_tolerance)) { last_progress_index_ = std::max(last_progress_index_, idx); geometry_msgs::Pose2D reached_pose = plan[idx]; @@ -804,7 +820,11 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co break; } - if (isGoalReached(robot_pose, plan[candidate_index], candidate_yaw)) + double candidate_xy_tolerance = (candidate_index == plan_last_index) ? final_goal_xy_tolerance_ : xy_local_goal_tolerance_; + double candidate_yaw_tolerance = (candidate_index == plan_last_index) ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; + + if (isPoseReached(robot_pose, plan[candidate_index], candidate_yaw, candidate_xy_tolerance, + candidate_yaw_tolerance)) { last_progress_index_ = std::max(last_progress_index_, candidate_index); geometry_msgs::Pose2D reached_pose = plan[candidate_index]; @@ -943,6 +963,7 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co double final_distance = hypot(final_dx, final_dy); if (final_distance <= final_goal_xy_tolerance_) { + goal_index = plan_last_index; goal_yaw = plan[plan_last_index].theta; has_forward_direction = false; } @@ -1189,13 +1210,19 @@ bool PathProgressCritic::computeOutgoingAngle(const std::vector Date: Sun, 12 Oct 2025 23:52:45 +0200 Subject: [PATCH 23/45] Require yaw alignment when restoring progress --- mir_dwb_critics/src/path_progress.cpp | 25 +++++++++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 7ec972fe..417ff2fc 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -277,8 +277,29 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co if (matchPoseToPlanIndex(robot_pose, robot_match_index)) { state_preserved = true; - progress_match_found = true; - restored_progress_index = std::max(restored_progress_index, robot_match_index); + + double match_goal_yaw = plan[robot_match_index].theta; + if (!computeOutgoingAngle(plan, robot_match_index, match_goal_yaw)) + { + match_goal_yaw = plan[robot_match_index].theta; + } + + bool match_is_final_index = (robot_match_index + 1u) >= plan.size(); + double match_yaw_tolerance = match_is_final_index ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; + double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, match_goal_yaw)); + + if (yaw_error <= match_yaw_tolerance) + { + progress_match_found = true; + restored_progress_index = std::max(restored_progress_index, robot_match_index); + } + else + { + ROS_DEBUG_NAMED("PathProgressCritic", + "Robot pose matched plan index %u but yaw error %.3f rad exceeds tolerance %.3f." + " Preserving state without increasing progress.", + robot_match_index, yaw_error, match_yaw_tolerance); + } } if (state_preserved) From f92a2792b9aaafa89a8eed50a0079d6dd5b486b4 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Mon, 13 Oct 2025 00:06:41 +0200 Subject: [PATCH 24/45] Snap intermediate goals to final pose when nearby --- mir_dwb_critics/src/path_progress.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 417ff2fc..b6f47b2c 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -967,6 +967,28 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } } + if (plan_last_index <= last_valid_index && goal_index != plan_last_index) + { + double snap_threshold = final_goal_xy_tolerance_; + if (snap_threshold >= 0.0) + { + double final_dx = plan[plan_last_index].x - plan[goal_index].x; + double final_dy = plan[plan_last_index].y - plan[goal_index].y; + double distance_to_final = hypot(final_dx, final_dy); + if (distance_to_final <= snap_threshold) + { + ROS_DEBUG_NAMED("PathProgressCritic", + "Snapping intermediate goal index %u to final goal because distance %.3f <= threshold %.3f", + goal_index, distance_to_final, snap_threshold); + goal_index = plan_last_index; + goal_yaw = plan[plan_last_index].theta; + has_forward_direction = false; + forced_skipped_articulation = false; + has_next_articulation = false; + } + } + } + bool pending_articulation = false; if (last_progress_index_ < goal_index) { From f017461742a23467a578962c95f3ca74259e0737 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Mon, 13 Oct 2025 13:22:25 +0200 Subject: [PATCH 25/45] Add global plan debug visualization --- mir_navigation/CMakeLists.txt | 1 + mir_navigation/launch/move_base.xml | 5 ++ mir_navigation/nodes/global_plan_debug_viz.py | 78 +++++++++++++++++++ mir_navigation/package.xml | 4 + 4 files changed, 88 insertions(+) create mode 100755 mir_navigation/nodes/global_plan_debug_viz.py diff --git a/mir_navigation/CMakeLists.txt b/mir_navigation/CMakeLists.txt index 340cdd1b..b23c5b8a 100644 --- a/mir_navigation/CMakeLists.txt +++ b/mir_navigation/CMakeLists.txt @@ -22,6 +22,7 @@ catkin_package() install(PROGRAMS mprim/genmprim_unicycle_highcost_5cm.py nodes/acc_finder.py + nodes/global_plan_debug_viz.py nodes/min_max_finder.py scripts/plot_mprim.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/mir_navigation/launch/move_base.xml b/mir_navigation/launch/move_base.xml index 090fe7ef..fbc9fe4a 100644 --- a/mir_navigation/launch/move_base.xml +++ b/mir_navigation/launch/move_base.xml @@ -22,4 +22,9 @@ + + + + + diff --git a/mir_navigation/nodes/global_plan_debug_viz.py b/mir_navigation/nodes/global_plan_debug_viz.py new file mode 100755 index 00000000..f4b3874f --- /dev/null +++ b/mir_navigation/nodes/global_plan_debug_viz.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 + +import rospy +from geometry_msgs.msg import PoseArray +from nav_msgs.msg import Path +from visualization_msgs.msg import Marker + + +class GlobalPlanDebugViz: + def __init__(self): + self._plan_counter = 0 + + default_plan_topic = rospy.get_param('~default_global_plan_topic', 'move_base_node/SBPLLatticePlanner/plan') + plan_topic = rospy.get_param('~global_plan_topic', default_plan_topic) + + pose_array_topic = rospy.get_param('~pose_array_topic', 'global_plan_pose_array') + marker_topic = rospy.get_param('~marker_topic', 'global_plan_marker') + + self._marker_frame = rospy.get_param('~marker_frame', 'base_link') + self._marker_height = rospy.get_param('~marker_height', 0.8) + self._marker_scale = rospy.get_param('~marker_scale', 0.2) + self._marker_ns = rospy.get_param('~marker_ns', 'global_plan_counter') + self._marker_id = rospy.get_param('~marker_id', 0) + self._marker_color = { + 'r': rospy.get_param('~marker_color_r', 1.0), + 'g': rospy.get_param('~marker_color_g', 1.0), + 'b': rospy.get_param('~marker_color_b', 1.0), + 'a': rospy.get_param('~marker_color_a', 1.0), + } + + self._pose_array_pub = rospy.Publisher(pose_array_topic, PoseArray, queue_size=1, latch=True) + self._marker_pub = rospy.Publisher(marker_topic, Marker, queue_size=1, latch=True) + self._plan_sub = rospy.Subscriber(plan_topic, Path, self._plan_callback, queue_size=1) + + rospy.loginfo('global_plan_debug_viz listening to %s', plan_topic) + + def _plan_callback(self, msg: Path): + self._plan_counter += 1 + self._publish_pose_array(msg) + self._publish_counter_marker() + + def _publish_pose_array(self, path_msg: Path): + pose_array = PoseArray() + pose_array.header = path_msg.header + pose_array.poses = [pose.pose for pose in path_msg.poses] + self._pose_array_pub.publish(pose_array) + + def _publish_counter_marker(self): + marker = Marker() + marker.header.frame_id = self._marker_frame + marker.header.stamp = rospy.Time.now() + marker.ns = self._marker_ns + marker.id = self._marker_id + marker.type = Marker.TEXT_VIEW_FACING + marker.action = Marker.ADD + marker.pose.position.x = 0.0 + marker.pose.position.y = 0.0 + marker.pose.position.z = self._marker_height + marker.pose.orientation.w = 1.0 + marker.scale.z = self._marker_scale + marker.color.r = self._marker_color['r'] + marker.color.g = self._marker_color['g'] + marker.color.b = self._marker_color['b'] + marker.color.a = self._marker_color['a'] + marker.text = str(self._plan_counter) + marker.lifetime = rospy.Duration(0.0) + + self._marker_pub.publish(marker) + + +def main(): + rospy.init_node('global_plan_debug_viz') + GlobalPlanDebugViz() + rospy.spin() + + +if __name__ == '__main__': + main() diff --git a/mir_navigation/package.xml b/mir_navigation/package.xml index 7c45c984..2b82230d 100644 --- a/mir_navigation/package.xml +++ b/mir_navigation/package.xml @@ -25,13 +25,17 @@ dwb_plugins hector_mapping + geometry_msgs map_server mir_driver mir_dwb_critics mir_gazebo move_base nav_core_adapter + nav_msgs python3-matplotlib sbpl_lattice_planner + rospy teb_local_planner + visualization_msgs From d736341105ece37064cfac3e2c572d42363375f2 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Mon, 13 Oct 2025 15:21:21 +0200 Subject: [PATCH 26/45] Limit published poses in global plan viz --- mir_navigation/nodes/global_plan_debug_viz.py | 20 ++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/mir_navigation/nodes/global_plan_debug_viz.py b/mir_navigation/nodes/global_plan_debug_viz.py index f4b3874f..d048521c 100755 --- a/mir_navigation/nodes/global_plan_debug_viz.py +++ b/mir_navigation/nodes/global_plan_debug_viz.py @@ -28,6 +28,19 @@ def __init__(self): 'a': rospy.get_param('~marker_color_a', 1.0), } + skip_param = rospy.get_param('~number_of_poses_to_skip', 7) + try: + skip_param = int(skip_param) + except (TypeError, ValueError): + rospy.logwarn("number_of_poses_to_skip must be an integer, got %s. Using 0 (no skipping).", skip_param) + skip_param = 0 + + if skip_param < 0: + rospy.logwarn("number_of_poses_to_skip cannot be negative. Using 0 (no skipping).") + skip_param = 0 + + self._poses_to_skip = skip_param + self._pose_array_pub = rospy.Publisher(pose_array_topic, PoseArray, queue_size=1, latch=True) self._marker_pub = rospy.Publisher(marker_topic, Marker, queue_size=1, latch=True) self._plan_sub = rospy.Subscriber(plan_topic, Path, self._plan_callback, queue_size=1) @@ -42,7 +55,12 @@ def _plan_callback(self, msg: Path): def _publish_pose_array(self, path_msg: Path): pose_array = PoseArray() pose_array.header = path_msg.header - pose_array.poses = [pose.pose for pose in path_msg.poses] + + if self._poses_to_skip <= 0: + pose_array.poses = [pose.pose for pose in path_msg.poses] + else: + pose_array.poses = [pose.pose for idx, pose in enumerate(path_msg.poses) if idx % self._poses_to_skip == 0] + self._pose_array_pub.publish(pose_array) def _publish_counter_marker(self): From dfb14031d51fbfd54bdf7e6f355b21bff5859d82 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Mon, 13 Oct 2025 15:52:04 +0200 Subject: [PATCH 27/45] Fix final goal handling in PathProgressCritic window --- .../include/mir_dwb_critics/path_progress.h | 3 +- mir_dwb_critics/src/path_progress.cpp | 59 ++++++++++++++----- 2 files changed, 46 insertions(+), 16 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index f6430021..21314a52 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -69,7 +69,8 @@ class PathProgressCritic : public dwb_critics::MapGridCritic unsigned int& y, double& desired_angle); unsigned int getGoalIndex(const std::vector& plan, unsigned int start_index, - unsigned int last_valid_index, double& desired_angle, bool& has_forward_direction) const; + unsigned int last_valid_index, bool plan_tail_is_final_goal, double& desired_angle, + bool& has_forward_direction) const; bool findNextArticulation(const std::vector& plan, unsigned int start_index, unsigned int end_index, unsigned int last_valid_index, unsigned int& articulation_index, diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index b6f47b2c..44d0e8d9 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -159,6 +159,25 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co std::vector plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses; + auto planPoseMatchesFinalGoal = [&](const geometry_msgs::Pose2D& plan_pose) { + double dx = plan_pose.x - goal.x; + double dy = plan_pose.y - goal.y; + double distance = hypot(dx, dy); + if (distance > plan_alignment_position_tolerance_) + { + return false; + } + + double yaw_error = fabs(angles::shortest_angular_distance(plan_pose.theta, goal.theta)); + return yaw_error <= plan_alignment_yaw_tolerance_; + }; + + bool final_goal_in_plan_window = false; + if (!plan.empty()) + { + final_goal_in_plan_window = planPoseMatchesFinalGoal(plan.back()); + } + auto plansEqual = [&](const std::vector& a, const std::vector& b) { if (a.size() != b.size()) @@ -284,7 +303,7 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co match_goal_yaw = plan[robot_match_index].theta; } - bool match_is_final_index = (robot_match_index + 1u) >= plan.size(); + bool match_is_final_index = ((robot_match_index + 1u) >= plan.size()) && final_goal_in_plan_window; double match_yaw_tolerance = match_is_final_index ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, match_goal_yaw)); @@ -632,8 +651,12 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } else { - double held_xy_tolerance = (held_goal_index_ == plan_last_index) ? final_goal_xy_tolerance_ : xy_local_goal_tolerance_; - double held_yaw_tolerance = (held_goal_index_ == plan_last_index) ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; + double held_xy_tolerance = + (final_goal_in_plan_window && held_goal_index_ == plan_last_index) ? final_goal_xy_tolerance_ : + xy_local_goal_tolerance_; + double held_yaw_tolerance = + (final_goal_in_plan_window && held_goal_index_ == plan_last_index) ? final_goal_yaw_tolerance_ : + yaw_local_goal_tolerance_; double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, held_goal_pose_.theta)); if (yaw_error >= held_yaw_tolerance) @@ -725,8 +748,10 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co bool articulation_has_forward = computeOutgoingAngle(plan, idx, articulation_yaw); double articulation_goal_yaw = articulation_has_forward ? articulation_yaw : plan[idx].theta; - double articulation_xy_tolerance = (idx == plan_last_index) ? final_goal_xy_tolerance_ : xy_local_goal_tolerance_; - double articulation_yaw_tolerance = (idx == plan_last_index) ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; + double articulation_xy_tolerance = + (final_goal_in_plan_window && idx == plan_last_index) ? final_goal_xy_tolerance_ : xy_local_goal_tolerance_; + double articulation_yaw_tolerance = + (final_goal_in_plan_window && idx == plan_last_index) ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; if (isPoseReached(robot_pose, plan[idx], articulation_goal_yaw, articulation_xy_tolerance, articulation_yaw_tolerance)) @@ -802,7 +827,7 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co double candidate_yaw = goal_yaw; bool candidate_has_forward = false; unsigned int candidate_index = - getGoalIndex(plan, search_index, last_valid_index, candidate_yaw, candidate_has_forward); + getGoalIndex(plan, search_index, last_valid_index, final_goal_in_plan_window, candidate_yaw, candidate_has_forward); bool forced_articulation = false; if (candidate_index > last_progress_index_) @@ -841,8 +866,12 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co break; } - double candidate_xy_tolerance = (candidate_index == plan_last_index) ? final_goal_xy_tolerance_ : xy_local_goal_tolerance_; - double candidate_yaw_tolerance = (candidate_index == plan_last_index) ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; + double candidate_xy_tolerance = + (final_goal_in_plan_window && candidate_index == plan_last_index) ? final_goal_xy_tolerance_ : + xy_local_goal_tolerance_; + double candidate_yaw_tolerance = + (final_goal_in_plan_window && candidate_index == plan_last_index) ? final_goal_yaw_tolerance_ : + yaw_local_goal_tolerance_; if (isPoseReached(robot_pose, plan[candidate_index], candidate_yaw, candidate_xy_tolerance, candidate_yaw_tolerance)) @@ -967,7 +996,7 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } } - if (plan_last_index <= last_valid_index && goal_index != plan_last_index) + if (final_goal_in_plan_window && plan_last_index <= last_valid_index && goal_index != plan_last_index) { double snap_threshold = final_goal_xy_tolerance_; if (snap_threshold >= 0.0) @@ -998,8 +1027,8 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co // Only consider snapping to the final goal yaw when we have already selected the last path index as goal. // Otherwise, keep following the path even if the final goal is within tolerance. - if (!pending_articulation && final_goal_xy_tolerance_ >= 0.0 && plan_last_index <= last_valid_index && - goal_index == plan_last_index) + if (!pending_articulation && final_goal_xy_tolerance_ >= 0.0 && final_goal_in_plan_window && + plan_last_index <= last_valid_index && goal_index == plan_last_index) { double final_dx = plan[plan_last_index].x - plan[goal_index].x; double final_dy = plan[plan_last_index].y - plan[goal_index].y; @@ -1051,8 +1080,8 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } unsigned int PathProgressCritic::getGoalIndex(const std::vector& plan, unsigned int start_index, - unsigned int last_valid_index, double& desired_angle, - bool& has_forward_direction) const + unsigned int last_valid_index, bool plan_tail_is_final_goal, + double& desired_angle, bool& has_forward_direction) const { if (plan.empty()) { @@ -1129,7 +1158,7 @@ unsigned int PathProgressCritic::getGoalIndex(const std::vector Date: Mon, 13 Oct 2025 16:05:22 +0200 Subject: [PATCH 28/45] Refine PathProgressCritic plan change detection --- .../include/mir_dwb_critics/path_progress.h | 12 +- mir_dwb_critics/src/path_progress.cpp | 141 +++++++++++------- 2 files changed, 96 insertions(+), 57 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index 21314a52..b87d5109 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -40,6 +40,7 @@ #include #include #include +#include #include #include @@ -65,8 +66,8 @@ class PathProgressCritic : public dwb_critics::MapGridCritic double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override; protected: - bool getGoalPose(const geometry_msgs::Pose2D& robot_pose, const nav_2d_msgs::Path2D& global_plan, unsigned int& x, - unsigned int& y, double& desired_angle); + bool getGoalPose(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& final_goal, + const nav_2d_msgs::Path2D& global_plan, unsigned int& x, unsigned int& y, double& desired_angle); unsigned int getGoalIndex(const std::vector& plan, unsigned int start_index, unsigned int last_valid_index, bool plan_tail_is_final_goal, double& desired_angle, @@ -102,10 +103,11 @@ class PathProgressCritic : public dwb_critics::MapGridCritic geometry_msgs::Pose2D last_plan_end_pose_; std::string last_plan_frame_id_; ros::Time last_plan_stamp_; - std::vector last_plan_; + uint32_t last_plan_seq_; + geometry_msgs::Pose2D last_plan_start_pose_; + geometry_msgs::Pose2D last_final_goal_pose_; + bool have_last_goal_pose_; - double plan_position_epsilon_; - double plan_yaw_epsilon_; double plan_alignment_position_tolerance_; double plan_alignment_yaw_tolerance_; diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index 44d0e8d9..c7060409 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -55,7 +55,7 @@ bool PathProgressCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d dwb_critics::MapGridCritic::reset(); unsigned int local_goal_x, local_goal_y; - if (!getGoalPose(pose, global_plan, local_goal_x, local_goal_y, desired_angle_)) + if (!getGoalPose(pose, goal, global_plan, local_goal_x, local_goal_y, desired_angle_)) { return false; } @@ -117,9 +117,14 @@ void PathProgressCritic::onInit() last_plan_end_pose_.theta = 0.0; last_plan_frame_id_.clear(); last_plan_stamp_ = ros::Time(0); - last_plan_.clear(); - plan_position_epsilon_ = 1e-4; - plan_yaw_epsilon_ = 1e-4; + last_plan_seq_ = 0u; + last_plan_start_pose_.x = 0.0; + last_plan_start_pose_.y = 0.0; + last_plan_start_pose_.theta = 0.0; + last_final_goal_pose_.x = 0.0; + last_final_goal_pose_.y = 0.0; + last_final_goal_pose_.theta = 0.0; + have_last_goal_pose_ = false; critic_nh_.param("plan_alignment_position_tolerance", plan_alignment_position_tolerance_, 0.15); critic_nh_.param("plan_alignment_yaw_tolerance", plan_alignment_yaw_tolerance_, 3.14159265358979323846); } @@ -134,6 +139,21 @@ void PathProgressCritic::reset() held_goal_pose_.y = 0.0; held_goal_pose_.theta = 0.0; initial_alignment_done_ = false; + have_last_plan_ = false; + last_plan_size_ = 0; + last_plan_end_pose_.x = 0.0; + last_plan_end_pose_.y = 0.0; + last_plan_end_pose_.theta = 0.0; + last_plan_frame_id_.clear(); + last_plan_stamp_ = ros::Time(0); + last_plan_seq_ = 0u; + last_plan_start_pose_.x = 0.0; + last_plan_start_pose_.y = 0.0; + last_plan_start_pose_.theta = 0.0; + last_final_goal_pose_.x = 0.0; + last_final_goal_pose_.y = 0.0; + last_final_goal_pose_.theta = 0.0; + have_last_goal_pose_ = false; } double PathProgressCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj) @@ -145,8 +165,9 @@ double PathProgressCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj) return position_score + heading_scale_ * heading_score; } -bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, const nav_2d_msgs::Path2D& global_plan, - unsigned int& x, unsigned int& y, double& desired_angle) +bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& final_goal, + const nav_2d_msgs::Path2D& global_plan, unsigned int& x, unsigned int& y, + double& desired_angle) { const nav_core2::Costmap& costmap = *costmap_; const nav_grid::NavGridInfo& info = costmap.getInfo(); @@ -159,45 +180,32 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co std::vector plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses; - auto planPoseMatchesFinalGoal = [&](const geometry_msgs::Pose2D& plan_pose) { - double dx = plan_pose.x - goal.x; - double dy = plan_pose.y - goal.y; + auto posesDiffer = [&](const geometry_msgs::Pose2D& a, const geometry_msgs::Pose2D& b, double position_tolerance, + double yaw_tolerance) { + double dx = a.x - b.x; + double dy = a.y - b.y; double distance = hypot(dx, dy); - if (distance > plan_alignment_position_tolerance_) + if (distance > position_tolerance) { - return false; + return true; } - double yaw_error = fabs(angles::shortest_angular_distance(plan_pose.theta, goal.theta)); - return yaw_error <= plan_alignment_yaw_tolerance_; + double yaw_error = fabs(angles::shortest_angular_distance(a.theta, b.theta)); + return yaw_error > yaw_tolerance; }; - bool final_goal_in_plan_window = false; - if (!plan.empty()) + if (plan.empty()) { - final_goal_in_plan_window = planPoseMatchesFinalGoal(plan.back()); + ROS_ERROR_NAMED("PathProgressCritic", "The adjusted global plan was empty."); + return false; } - auto plansEqual = [&](const std::vector& a, - const std::vector& b) { - if (a.size() != b.size()) - { - return false; - } - - for (size_t idx = 0; idx < a.size(); ++idx) - { - if (fabs(a[idx].x - b[idx].x) > plan_position_epsilon_ || - fabs(a[idx].y - b[idx].y) > plan_position_epsilon_ || - fabs(angles::shortest_angular_distance(a[idx].theta, b[idx].theta)) > plan_yaw_epsilon_) - { - return false; - } - } - - return true; + auto planPoseMatchesFinalGoal = [&](const geometry_msgs::Pose2D& plan_pose) { + return !posesDiffer(plan_pose, final_goal, plan_alignment_position_tolerance_, plan_alignment_yaw_tolerance_); }; + bool final_goal_in_plan_window = planPoseMatchesFinalGoal(plan.back()); + auto matchPoseToPlanIndex = [&](const geometry_msgs::Pose2D& pose, unsigned int& index_out) { bool found = false; double best_distance = std::numeric_limits::infinity(); @@ -229,22 +237,54 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co return found; }; - // Reset state if a new global plan arrives (size, geometry, or frame changes) + // Reset state if a new global plan arrives (detected via metadata changes or major window shifts) bool plan_changed = false; - if (!have_last_plan_) - { - plan_changed = true; - } - else - { - if (global_plan.header.frame_id != last_plan_frame_id_) + geometry_msgs::Pose2D plan_start_pose = plan.front(); + + auto headerStampChanged = [&]() { + if (last_plan_stamp_.isZero() || global_plan.header.stamp.isZero()) + { + return false; + } + return last_plan_stamp_ != global_plan.header.stamp; + }; + + auto headerSeqChanged = [&]() { + if (!have_last_plan_) + { + return false; + } + return last_plan_seq_ != global_plan.header.seq; + }; + + auto goalPoseChanged = [&]() { + if (!have_last_goal_pose_) { - plan_changed = true; + return true; } - else if (!plansEqual(plan, last_plan_)) + return posesDiffer(final_goal, last_final_goal_pose_, plan_alignment_position_tolerance_, + plan_alignment_yaw_tolerance_); + }; + + auto planStartChanged = [&]() { + if (!have_last_plan_) { - plan_changed = true; + return true; } + // Allow the cropped window to slide by a generous distance without counting as a full plan change. + double position_tolerance = std::max(plan_alignment_position_tolerance_ * 4.0, plan_alignment_position_tolerance_); + return posesDiffer(plan_start_pose, last_plan_start_pose_, position_tolerance, plan_alignment_yaw_tolerance_); + }; + + if (!have_last_plan_ || global_plan.header.frame_id != last_plan_frame_id_ || headerStampChanged() || + headerSeqChanged() || + goalPoseChanged()) + { + plan_changed = true; + } + else if (planStartChanged()) + { + plan_changed = true; } if (plan_changed) @@ -365,14 +405,11 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co last_plan_end_pose_ = plan.back(); last_plan_frame_id_ = global_plan.header.frame_id; last_plan_stamp_ = global_plan.header.stamp; - last_plan_ = plan; - + last_plan_seq_ = global_plan.header.seq; + last_plan_start_pose_ = plan_start_pose; + last_final_goal_pose_ = final_goal; + have_last_goal_pose_ = true; - if (plan.empty()) - { - ROS_ERROR_NAMED("PathProgressCritic", "The adjusted global plan was empty."); - return false; - } unsigned int plan_last_index = static_cast(plan.size() - 1); From 75d05932ecb0810fd4366f9fbece240724f6c566 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Mon, 13 Oct 2025 16:25:24 +0200 Subject: [PATCH 29/45] Refactor advanced path tracking into dedicated critic --- mir_dwb_critics/CMakeLists.txt | 1 + mir_dwb_critics/default_critics.xml | 7 +- .../include/mir_dwb_critics/path_follower.h | 129 ++ .../include/mir_dwb_critics/path_progress.h | 58 +- mir_dwb_critics/src/path_follower.cpp | 1339 +++++++++++++++++ mir_dwb_critics/src/path_progress.cpp | 1195 +-------------- .../config/dwb_local_planner_params.yaml | 14 +- 7 files changed, 1543 insertions(+), 1200 deletions(-) create mode 100644 mir_dwb_critics/include/mir_dwb_critics/path_follower.h create mode 100644 mir_dwb_critics/src/path_follower.cpp diff --git a/mir_dwb_critics/CMakeLists.txt b/mir_dwb_critics/CMakeLists.txt index 89e5c9c8..c40923e0 100644 --- a/mir_dwb_critics/CMakeLists.txt +++ b/mir_dwb_critics/CMakeLists.txt @@ -32,6 +32,7 @@ include_directories( add_library(${PROJECT_NAME} src/path_angle.cpp src/path_progress.cpp + src/path_follower.cpp src/path_dist_pruned.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) diff --git a/mir_dwb_critics/default_critics.xml b/mir_dwb_critics/default_critics.xml index 2ad69cbc..8c561131 100644 --- a/mir_dwb_critics/default_critics.xml +++ b/mir_dwb_critics/default_critics.xml @@ -1,8 +1,13 @@ - + Scores trajectories based on how far along the global path they end up. + + Maintains advanced path following behavior with articulation handling and pose alignment logic. + Scores trajectories based on the difference between the path's current angle and the trajectory's angle diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_follower.h b/mir_dwb_critics/include/mir_dwb_critics/path_follower.h new file mode 100644 index 00000000..5c8fe659 --- /dev/null +++ b/mir_dwb_critics/include/mir_dwb_critics/path_follower.h @@ -0,0 +1,129 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, DFKI GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef MIR_DWB_CRITICS_PATH_FOLLOWER_H_ +#define MIR_DWB_CRITICS_PATH_FOLLOWER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mir_dwb_critics +{ +/** + * @class PathFollowerCritic + * @brief Calculates an intermediate goal along the global path and scores trajectories on the distance to that goal. + * + * This trajectory critic helps ensure progress along the global path. It + * calculates an intermediate goal that is as far as possible along the global + * path as long as the path continues to move in one direction (+/- + * angle_threshold). + */ +class PathFollowerCritic : public dwb_critics::MapGridCritic +{ +public: + bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel, const geometry_msgs::Pose2D& goal, + const nav_2d_msgs::Path2D& global_plan) override; + + void onInit() override; + void reset() override; + double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override; + +protected: + bool getGoalPose(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& final_goal, + const nav_2d_msgs::Path2D& global_plan, unsigned int& x, unsigned int& y, double& desired_angle); + + unsigned int getGoalIndex(const std::vector& plan, unsigned int start_index, + unsigned int last_valid_index, bool plan_tail_is_final_goal, double& desired_angle, + bool& has_forward_direction) const; + + bool findNextArticulation(const std::vector& plan, unsigned int start_index, + unsigned int end_index, unsigned int last_valid_index, unsigned int& articulation_index, + double& articulation_yaw, bool& has_forward_direction) const; + + bool computeOutgoingAngle(const std::vector& plan, unsigned int index, + double& angle) const; + + bool isPoseReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, + double goal_yaw, double xy_tolerance, double yaw_tolerance) const; + + bool isGoalReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, + double goal_yaw) const; + + double xy_local_goal_tolerance_; + double yaw_local_goal_tolerance_; + double final_goal_yaw_tolerance_; + double final_goal_xy_tolerance_; + double angle_threshold_; + double articulation_angle_threshold_; + double heading_scale_; + bool enforce_forward_dot_; + bool always_target_articulations_; + bool initial_alignment_done_; + + // Plan change detection to reset internal state on new global plans + bool have_last_plan_; + size_t last_plan_size_; + geometry_msgs::Pose2D last_plan_end_pose_; + std::string last_plan_frame_id_; + ros::Time last_plan_stamp_; + uint32_t last_plan_seq_; + geometry_msgs::Pose2D last_plan_start_pose_; + geometry_msgs::Pose2D last_final_goal_pose_; + bool have_last_goal_pose_; + + double plan_alignment_position_tolerance_; + double plan_alignment_yaw_tolerance_; + + unsigned int last_progress_index_; + bool holding_goal_; + unsigned int held_goal_index_; + geometry_msgs::Pose2D held_goal_pose_; + double hold_position_epsilon_; + double hold_yaw_epsilon_; + + std::vector reached_intermediate_goals_; + double desired_angle_; + ros::Publisher intermediate_goal_pub_; + ros::Publisher articulation_points_pub_; + ros::Publisher intermediate_goal_tolerance_pub_; +}; + +} // namespace mir_dwb_critics +#endif // MIR_DWB_CRITICS_PATH_FOLLOWER_H_ diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h index b87d5109..bb78175b 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_progress.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_progress.h @@ -35,13 +35,6 @@ #define MIR_DWB_CRITICS_PATH_PROGRESS_H_ #include -#include -#include -#include -#include -#include -#include -#include #include namespace mir_dwb_critics @@ -66,63 +59,18 @@ class PathProgressCritic : public dwb_critics::MapGridCritic double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) override; protected: - bool getGoalPose(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& final_goal, - const nav_2d_msgs::Path2D& global_plan, unsigned int& x, unsigned int& y, double& desired_angle); + bool getGoalPose(const geometry_msgs::Pose2D& robot_pose, const nav_2d_msgs::Path2D& global_plan, unsigned int& x, + unsigned int& y, double& desired_angle); unsigned int getGoalIndex(const std::vector& plan, unsigned int start_index, - unsigned int last_valid_index, bool plan_tail_is_final_goal, double& desired_angle, - bool& has_forward_direction) const; - - bool findNextArticulation(const std::vector& plan, unsigned int start_index, - unsigned int end_index, unsigned int last_valid_index, unsigned int& articulation_index, - double& articulation_yaw, bool& has_forward_direction) const; - - bool computeOutgoingAngle(const std::vector& plan, unsigned int index, - double& angle) const; - - bool isPoseReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, - double goal_yaw, double xy_tolerance, double yaw_tolerance) const; - - bool isGoalReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, - double goal_yaw) const; + unsigned int last_valid_index) const; double xy_local_goal_tolerance_; - double yaw_local_goal_tolerance_; - double final_goal_yaw_tolerance_; - double final_goal_xy_tolerance_; double angle_threshold_; - double articulation_angle_threshold_; double heading_scale_; - bool enforce_forward_dot_; - bool always_target_articulations_; - bool initial_alignment_done_; - - // Plan change detection to reset internal state on new global plans - bool have_last_plan_; - size_t last_plan_size_; - geometry_msgs::Pose2D last_plan_end_pose_; - std::string last_plan_frame_id_; - ros::Time last_plan_stamp_; - uint32_t last_plan_seq_; - geometry_msgs::Pose2D last_plan_start_pose_; - geometry_msgs::Pose2D last_final_goal_pose_; - bool have_last_goal_pose_; - - double plan_alignment_position_tolerance_; - double plan_alignment_yaw_tolerance_; - - unsigned int last_progress_index_; - bool holding_goal_; - unsigned int held_goal_index_; - geometry_msgs::Pose2D held_goal_pose_; - double hold_position_epsilon_; - double hold_yaw_epsilon_; std::vector reached_intermediate_goals_; double desired_angle_; - ros::Publisher intermediate_goal_pub_; - ros::Publisher articulation_points_pub_; - ros::Publisher intermediate_goal_tolerance_pub_; }; } // namespace mir_dwb_critics diff --git a/mir_dwb_critics/src/path_follower.cpp b/mir_dwb_critics/src/path_follower.cpp new file mode 100644 index 00000000..c4388068 --- /dev/null +++ b/mir_dwb_critics/src/path_follower.cpp @@ -0,0 +1,1339 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, DFKI GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mir_dwb_critics +{ +bool PathFollowerCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel, + const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan) +{ + dwb_critics::MapGridCritic::reset(); + + unsigned int local_goal_x, local_goal_y; + if (!getGoalPose(pose, goal, global_plan, local_goal_x, local_goal_y, desired_angle_)) + { + return false; + } + + // Enqueue just the last pose + cell_values_.setValue(local_goal_x, local_goal_y, 0.0); + queue_->enqueueCell(local_goal_x, local_goal_y); + + propogateManhattanDistances(); + + return true; +} + +void PathFollowerCritic::onInit() +{ + dwb_critics::MapGridCritic::onInit(); + critic_nh_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.20); + critic_nh_.param("yaw_local_goal_tolerance", yaw_local_goal_tolerance_, 0.15); + ros::NodeHandle private_nh("~"); + if (!private_nh.getParam("yaw_goal_tolerance", final_goal_yaw_tolerance_)) + { + critic_nh_.param("yaw_goal_tolerance", final_goal_yaw_tolerance_, yaw_local_goal_tolerance_); + } + if (!private_nh.getParam("xy_goal_tolerance", final_goal_xy_tolerance_)) + { + critic_nh_.param("xy_goal_tolerance", final_goal_xy_tolerance_, xy_local_goal_tolerance_); + } + critic_nh_.param("angle_threshold", angle_threshold_, M_PI_4); + critic_nh_.param("articulation_angle_threshold", articulation_angle_threshold_, 1.3089969389957472); + critic_nh_.param("heading_scale", heading_scale_, 1.0); + critic_nh_.param("enforce_forward_dot", enforce_forward_dot_, true); + critic_nh_.param("always_target_articulations", always_target_articulations_, true); + initial_alignment_done_ = false; + + intermediate_goal_pub_ = critic_nh_.advertise("intermediate_goal", 1); + articulation_points_pub_ = critic_nh_.advertise("articulation_points", 1); + intermediate_goal_tolerance_pub_ = + critic_nh_.advertise("intermediate_goal_tolerance", 1); + + articulation_angle_threshold_ = std::max(articulation_angle_threshold_, angle_threshold_); + + // divide heading scale by position scale because the sum will be multiplied by scale again + heading_scale_ /= getScale(); + last_progress_index_ = 0; + reached_intermediate_goals_.clear(); + holding_goal_ = false; + held_goal_index_ = 0; + held_goal_pose_.x = 0.0; + held_goal_pose_.y = 0.0; + held_goal_pose_.theta = 0.0; + hold_position_epsilon_ = 1e-6; + hold_yaw_epsilon_ = 1e-6; + final_goal_yaw_tolerance_ = std::max(final_goal_yaw_tolerance_, 1e-6); + final_goal_xy_tolerance_ = std::max(final_goal_xy_tolerance_, 0.0); + have_last_plan_ = false; + last_plan_size_ = 0; + last_plan_end_pose_.x = 0.0; + last_plan_end_pose_.y = 0.0; + last_plan_end_pose_.theta = 0.0; + last_plan_frame_id_.clear(); + last_plan_stamp_ = ros::Time(0); + last_plan_seq_ = 0u; + last_plan_start_pose_.x = 0.0; + last_plan_start_pose_.y = 0.0; + last_plan_start_pose_.theta = 0.0; + last_final_goal_pose_.x = 0.0; + last_final_goal_pose_.y = 0.0; + last_final_goal_pose_.theta = 0.0; + have_last_goal_pose_ = false; + critic_nh_.param("plan_alignment_position_tolerance", plan_alignment_position_tolerance_, 0.15); + critic_nh_.param("plan_alignment_yaw_tolerance", plan_alignment_yaw_tolerance_, 3.14159265358979323846); +} + +void PathFollowerCritic::reset() +{ + reached_intermediate_goals_.clear(); + last_progress_index_ = 0; + holding_goal_ = false; + held_goal_index_ = 0; + held_goal_pose_.x = 0.0; + held_goal_pose_.y = 0.0; + held_goal_pose_.theta = 0.0; + initial_alignment_done_ = false; + have_last_plan_ = false; + last_plan_size_ = 0; + last_plan_end_pose_.x = 0.0; + last_plan_end_pose_.y = 0.0; + last_plan_end_pose_.theta = 0.0; + last_plan_frame_id_.clear(); + last_plan_stamp_ = ros::Time(0); + last_plan_seq_ = 0u; + last_plan_start_pose_.x = 0.0; + last_plan_start_pose_.y = 0.0; + last_plan_start_pose_.theta = 0.0; + last_final_goal_pose_.x = 0.0; + last_final_goal_pose_.y = 0.0; + last_final_goal_pose_.theta = 0.0; + have_last_goal_pose_ = false; +} + +double PathFollowerCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj) +{ + double position_score = MapGridCritic::scoreTrajectory(traj); + double heading_diff = fabs(angles::shortest_angular_distance(traj.poses.back().theta, desired_angle_)); + double heading_score = heading_diff * heading_diff; + + return position_score + heading_scale_ * heading_score; +} + +bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& final_goal, + const nav_2d_msgs::Path2D& global_plan, unsigned int& x, unsigned int& y, + double& desired_angle) +{ + const nav_core2::Costmap& costmap = *costmap_; + const nav_grid::NavGridInfo& info = costmap.getInfo(); + + if (global_plan.poses.empty()) + { + ROS_ERROR_NAMED("PathFollowerCritic", "The global plan was empty."); + return false; + } + + std::vector plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses; + + auto posesDiffer = [&](const geometry_msgs::Pose2D& a, const geometry_msgs::Pose2D& b, double position_tolerance, + double yaw_tolerance) { + double dx = a.x - b.x; + double dy = a.y - b.y; + double distance = hypot(dx, dy); + if (distance > position_tolerance) + { + return true; + } + + double yaw_error = fabs(angles::shortest_angular_distance(a.theta, b.theta)); + return yaw_error > yaw_tolerance; + }; + + if (plan.empty()) + { + ROS_ERROR_NAMED("PathFollowerCritic", "The adjusted global plan was empty."); + return false; + } + + auto planPoseMatchesFinalGoal = [&](const geometry_msgs::Pose2D& plan_pose) { + return !posesDiffer(plan_pose, final_goal, plan_alignment_position_tolerance_, plan_alignment_yaw_tolerance_); + }; + + bool final_goal_in_plan_window = planPoseMatchesFinalGoal(plan.back()); + + auto matchPoseToPlanIndex = [&](const geometry_msgs::Pose2D& pose, unsigned int& index_out) { + bool found = false; + double best_distance = std::numeric_limits::infinity(); + + for (unsigned int idx = 0; idx < plan.size(); ++idx) + { + double dx = plan[idx].x - pose.x; + double dy = plan[idx].y - pose.y; + double distance = hypot(dx, dy); + if (distance > plan_alignment_position_tolerance_) + { + continue; + } + + double yaw_error = fabs(angles::shortest_angular_distance(plan[idx].theta, pose.theta)); + if (yaw_error > plan_alignment_yaw_tolerance_) + { + continue; + } + + if (distance < best_distance) + { + best_distance = distance; + index_out = idx; + found = true; + } + } + + return found; + }; + + // Reset state if a new global plan arrives (detected via metadata changes or major window shifts) + bool plan_changed = false; + geometry_msgs::Pose2D plan_start_pose = plan.front(); + + auto headerStampChanged = [&]() { + if (last_plan_stamp_.isZero() || global_plan.header.stamp.isZero()) + { + return false; + } + return last_plan_stamp_ != global_plan.header.stamp; + }; + + auto headerSeqChanged = [&]() { + if (!have_last_plan_) + { + return false; + } + return last_plan_seq_ != global_plan.header.seq; + }; + + auto goalPoseChanged = [&]() { + if (!have_last_goal_pose_) + { + return true; + } + return posesDiffer(final_goal, last_final_goal_pose_, plan_alignment_position_tolerance_, + plan_alignment_yaw_tolerance_); + }; + + auto planStartChanged = [&]() { + if (!have_last_plan_) + { + return true; + } + // Allow the cropped window to slide by a generous distance without counting as a full plan change. + double position_tolerance = std::max(plan_alignment_position_tolerance_ * 4.0, plan_alignment_position_tolerance_); + return posesDiffer(plan_start_pose, last_plan_start_pose_, position_tolerance, plan_alignment_yaw_tolerance_); + }; + + if (!have_last_plan_ || global_plan.header.frame_id != last_plan_frame_id_ || headerStampChanged() || + headerSeqChanged() || + goalPoseChanged()) + { + plan_changed = true; + } + else if (planStartChanged()) + { + plan_changed = true; + } + + if (plan_changed) + { + bool state_preserved = false; + bool progress_match_found = false; + unsigned int restored_progress_index = 0u; + std::vector> preserved_reached; + preserved_reached.reserve(reached_intermediate_goals_.size()); + + unsigned int previous_progress_index = last_progress_index_; + + for (const auto& reached_pose : reached_intermediate_goals_) + { + unsigned int matched_index = 0u; + if (matchPoseToPlanIndex(reached_pose, matched_index)) + { + geometry_msgs::Pose2D preserved_pose = reached_pose; + preserved_pose.x = plan[matched_index].x; + preserved_pose.y = plan[matched_index].y; + preserved_reached.emplace_back(matched_index, preserved_pose); + state_preserved = true; + progress_match_found = true; + restored_progress_index = std::max(restored_progress_index, matched_index); + } + } + + unsigned int held_match_index = 0u; + if (holding_goal_) + { + if (matchPoseToPlanIndex(held_goal_pose_, held_match_index)) + { + state_preserved = true; + held_goal_index_ = held_match_index; + held_goal_pose_.x = plan[held_match_index].x; + held_goal_pose_.y = plan[held_match_index].y; + } + else + { + holding_goal_ = false; + held_goal_index_ = 0u; + held_goal_pose_.x = 0.0; + held_goal_pose_.y = 0.0; + held_goal_pose_.theta = 0.0; + } + } + + unsigned int robot_match_index = 0u; + if (matchPoseToPlanIndex(robot_pose, robot_match_index)) + { + state_preserved = true; + + double match_goal_yaw = plan[robot_match_index].theta; + if (!computeOutgoingAngle(plan, robot_match_index, match_goal_yaw)) + { + match_goal_yaw = plan[robot_match_index].theta; + } + + bool match_is_final_index = ((robot_match_index + 1u) >= plan.size()) && final_goal_in_plan_window; + double match_yaw_tolerance = match_is_final_index ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; + double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, match_goal_yaw)); + + if (yaw_error <= match_yaw_tolerance) + { + progress_match_found = true; + restored_progress_index = std::max(restored_progress_index, robot_match_index); + } + else + { + ROS_DEBUG_NAMED("PathFollowerCritic", + "Robot pose matched plan index %u but yaw error %.3f rad exceeds tolerance %.3f." + " Preserving state without increasing progress.", + robot_match_index, yaw_error, match_yaw_tolerance); + } + } + + if (state_preserved) + { + if (progress_match_found) + { + std::sort(preserved_reached.begin(), preserved_reached.end(), + [](const std::pair& lhs, + const std::pair& rhs) { + return lhs.first < rhs.first; + }); + reached_intermediate_goals_.clear(); + reached_intermediate_goals_.reserve(preserved_reached.size()); + for (const auto& entry : preserved_reached) + { + reached_intermediate_goals_.push_back(entry.second); + } + unsigned int plan_last_index = plan.empty() ? 0u : static_cast(plan.size() - 1); + last_progress_index_ = std::min(restored_progress_index, plan_last_index); + } + else + { + reached_intermediate_goals_.clear(); + unsigned int plan_last_index = plan.empty() ? 0u : static_cast(plan.size() - 1); + last_progress_index_ = std::min(previous_progress_index, plan_last_index); + } + } + else + { + reached_intermediate_goals_.clear(); + last_progress_index_ = 0u; + holding_goal_ = false; + held_goal_index_ = 0u; + held_goal_pose_.x = 0.0; + held_goal_pose_.y = 0.0; + held_goal_pose_.theta = 0.0; + initial_alignment_done_ = false; + } + + have_last_plan_ = true; + } + + last_plan_size_ = plan.size(); + last_plan_end_pose_ = plan.back(); + last_plan_frame_id_ = global_plan.header.frame_id; + last_plan_stamp_ = global_plan.header.stamp; + last_plan_seq_ = global_plan.header.seq; + last_plan_start_pose_ = plan_start_pose; + last_final_goal_pose_ = final_goal; + have_last_goal_pose_ = true; + + + unsigned int plan_last_index = static_cast(plan.size() - 1); + + if (initial_alignment_done_ && plan.size() > 1 && last_progress_index_ > 0) + { + unsigned int prune_limit = std::min(last_progress_index_, plan_last_index); + if (prune_limit > 0) + { + plan.erase(plan.begin(), plan.begin() + prune_limit); + last_progress_index_ = last_progress_index_ >= prune_limit ? last_progress_index_ - prune_limit : 0; + plan_last_index = static_cast(plan.size() - 1); + + if (holding_goal_) + { + if (held_goal_index_ < prune_limit) + { + holding_goal_ = false; + held_goal_index_ = 0; + } + else + { + held_goal_index_ -= prune_limit; + } + } + } + } + + if (holding_goal_ && held_goal_index_ >= plan.size()) + { + ROS_DEBUG_NAMED("PathFollowerCritic", "Held goal index %u is out of range for current plan of size %zu. Releasing hold.", + held_goal_index_, plan.size()); + holding_goal_ = false; + held_goal_index_ = 0; + } + + // find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map + unsigned int start_index = 0; + double distance_to_start = std::numeric_limits::infinity(); + bool started_path = false; + for (unsigned int i = 0; i < plan.size(); i++) + { + double g_x = plan[i].x; + double g_y = plan[i].y; + unsigned int map_x, map_y; + if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) + { + // Still on the costmap. Continue. + double distance = nav_2d_utils::poseDistance(plan[i], robot_pose); + if (distance_to_start > distance) + { + start_index = i; + distance_to_start = distance; + started_path = true; + } + else + { + // Plan is going away from the robot again. It's possible that it comes back and we would find a pose that's + // even closer to the robot, but then we would skip over parts of the plan. + break; + } + } + else if (started_path) + { + // Off the costmap after being on the costmap. + break; + } + // else, we have not yet found a point on the costmap, so we just continue + } + + if (!started_path) + { + ROS_ERROR_NAMED("PathFollowerCritic", "None of the points of the global plan were in the local costmap."); + return false; + } + + // find the "last valid pose", i.e. the last pose on the plan after the start pose that is still on the local map + unsigned int last_valid_index = start_index; + for (unsigned int i = start_index + 1; i < plan.size(); i++) + { + double g_x = plan[i].x; + double g_y = plan[i].y; + unsigned int map_x, map_y; + if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) + { + // Still on the costmap. Continue. + last_valid_index = i; + } + else + { + // Off the costmap after being on the costmap. + break; + } + } + + auto collectArticulationIndices = [&](unsigned int scan_start, unsigned int scan_end) { + std::vector articulation_indices; + if (plan.size() < 2) + { + return articulation_indices; + } + + const double epsilon = 1e-9; + unsigned int clamped_start = std::max(scan_start, 1u); + unsigned int clamped_end = std::min(scan_end, plan_last_index); + if (clamped_start > clamped_end) + { + return articulation_indices; + } + + double previous_segment_angle = 0.0; + bool previous_segment_angle_set = false; + unsigned int previous_segment_end_index = 0u; + + for (unsigned int i = clamped_start; i <= clamped_end; ++i) + { + double direction_x = plan[i].x - plan[i - 1].x; + double direction_y = plan[i].y - plan[i - 1].y; + double length = hypot(direction_x, direction_y); + if (length < epsilon) + { + continue; + } + + double current_angle = atan2(direction_y, direction_x); + if (previous_segment_angle_set) + { + double articulation_angle = + fabs(angles::shortest_angular_distance(previous_segment_angle, current_angle)); + if (articulation_angle >= articulation_angle_threshold_) + { + if (articulation_indices.empty() || articulation_indices.back() != previous_segment_end_index) + { + articulation_indices.push_back(previous_segment_end_index); + } + } + } + + previous_segment_angle = current_angle; + previous_segment_angle_set = true; + previous_segment_end_index = i; + } + + return articulation_indices; + }; + + auto publishArticulationPointCloud = [&](const std::vector& articulation_indices) { + if (!articulation_points_pub_) + { + return; + } + + sensor_msgs::PointCloud cloud_msg; + cloud_msg.header = global_plan.header; + cloud_msg.header.stamp = ros::Time::now(); + cloud_msg.points.reserve(articulation_indices.size()); + + for (unsigned int index : articulation_indices) + { + if (index >= plan.size()) + { + continue; + } + + geometry_msgs::Point32 point; + point.x = plan[index].x; + point.y = plan[index].y; + point.z = 0.0; + cloud_msg.points.push_back(point); + } + + articulation_points_pub_.publish(cloud_msg); + }; + + auto publishIntermediateGoal = [&](const geometry_msgs::Pose2D& goal_pose, double goal_yaw) { + if (!intermediate_goal_pub_) + { + return; + } + geometry_msgs::PoseStamped goal_msg; + goal_msg.header = global_plan.header; + goal_msg.header.stamp = ros::Time::now(); + goal_msg.pose.position.x = goal_pose.x; + goal_msg.pose.position.y = goal_pose.y; + goal_msg.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, goal_yaw); + goal_msg.pose.orientation.x = q.x(); + goal_msg.pose.orientation.y = q.y(); + goal_msg.pose.orientation.z = q.z(); + goal_msg.pose.orientation.w = q.w(); + intermediate_goal_pub_.publish(goal_msg); + + if (intermediate_goal_tolerance_pub_) + { + visualization_msgs::MarkerArray marker_array; + visualization_msgs::Marker marker; + marker.header = goal_msg.header; + marker.header.stamp = goal_msg.header.stamp; + marker.ns = "intermediate_goal_tolerance"; + marker.id = 0; + marker.type = visualization_msgs::Marker::SPHERE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position = goal_msg.pose.position; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + double diameter = std::max(2.0 * xy_local_goal_tolerance_, 1e-6); + marker.scale.x = diameter; + marker.scale.y = diameter; + marker.scale.z = diameter; + marker.color.r = 0.2f; + marker.color.g = 0.8f; + marker.color.b = 0.4f; + marker.color.a = 0.35f; + marker.lifetime = ros::Duration(0.0); + marker_array.markers.push_back(marker); + intermediate_goal_tolerance_pub_.publish(marker_array); + } + }; + + if (!initial_alignment_done_) + { + double desired_initial_yaw = plan.front().theta; + if (!plan.empty()) + { + double outgoing_angle = desired_initial_yaw; + if (computeOutgoingAngle(plan, 0u, outgoing_angle)) + { + desired_initial_yaw = outgoing_angle; + } + } + + double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, desired_initial_yaw)); + if (yaw_error >= yaw_local_goal_tolerance_) + { + unsigned int rx = 0; + unsigned int ry = 0; + if (worldToGridBounded(info, robot_pose.x, robot_pose.y, rx, ry)) + { + x = rx; + y = ry; + desired_angle = desired_initial_yaw; + held_goal_pose_.x = robot_pose.x; + held_goal_pose_.y = robot_pose.y; + held_goal_pose_.theta = desired_initial_yaw; + held_goal_index_ = 0; + holding_goal_ = true; + publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); + ROS_DEBUG_NAMED("PathFollowerCritic", + "Initial alignment: rotate in place to yaw %.3f rad at robot pose (x: %.3f, y: %.3f)", + held_goal_pose_.theta, held_goal_pose_.x, held_goal_pose_.y); + return true; + } + } + + initial_alignment_done_ = true; + } + + std::vector articulation_indices = collectArticulationIndices(1u, plan_last_index); + publishArticulationPointCloud(articulation_indices); + + if (holding_goal_) + { + unsigned int held_x = 0; + unsigned int held_y = 0; + bool held_in_costmap = worldToGridBounded(info, held_goal_pose_.x, held_goal_pose_.y, held_x, held_y); + + if (!held_in_costmap) + { + ROS_WARN_NAMED("PathFollowerCritic", + "Held goal (index %u) is outside the local costmap. Releasing hold to search for a new goal.", + held_goal_index_); + holding_goal_ = false; + held_goal_index_ = 0; + } + else + { + double held_xy_tolerance = + (final_goal_in_plan_window && held_goal_index_ == plan_last_index) ? final_goal_xy_tolerance_ : + xy_local_goal_tolerance_; + double held_yaw_tolerance = + (final_goal_in_plan_window && held_goal_index_ == plan_last_index) ? final_goal_yaw_tolerance_ : + yaw_local_goal_tolerance_; + + double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, held_goal_pose_.theta)); + if (yaw_error >= held_yaw_tolerance) + { + x = held_x; + y = held_y; + desired_angle = held_goal_pose_.theta; + publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); + ROS_DEBUG_NAMED("PathFollowerCritic", "Holding goal index %u due to yaw error %.3f rad (threshold %.3f)", + held_goal_index_, yaw_error, held_yaw_tolerance); + return true; + } + + if (!isPoseReached(robot_pose, held_goal_pose_, held_goal_pose_.theta, held_xy_tolerance, held_yaw_tolerance)) + { + x = held_x; + y = held_y; + desired_angle = held_goal_pose_.theta; + publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); + ROS_DEBUG_NAMED("PathFollowerCritic", + "Holding goal index %u until full pose tolerance satisfied (XY + yaw)", held_goal_index_); + return true; + } + + last_progress_index_ = std::max(last_progress_index_, held_goal_index_); + geometry_msgs::Pose2D reached_pose = held_goal_pose_; + if (reached_intermediate_goals_.empty() || + nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || + fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > 1e-6) + { + reached_intermediate_goals_.push_back(reached_pose); + } + ROS_DEBUG_NAMED("PathFollowerCritic", + "Reached held intermediate goal index %u while respecting pose tolerances. last_progress_index_: %u", + held_goal_index_, last_progress_index_); + holding_goal_ = false; + } + } + + // Constrain the search range to enforce monotonic progress with the updated bookkeeping. + last_progress_index_ = std::min(last_progress_index_, plan_last_index); + + unsigned int search_start_index = std::max(start_index, last_progress_index_); + search_start_index = std::min(search_start_index, last_valid_index); + + if (holding_goal_ && held_goal_index_ >= last_progress_index_) + { + search_start_index = std::min(search_start_index, held_goal_index_); + } + + unsigned int articulation_scan_start = 0u; + if (plan_last_index >= 1) + { + unsigned int next_index = last_progress_index_ < plan_last_index ? last_progress_index_ + 1 : plan_last_index; + articulation_scan_start = std::max(next_index, 1u); + } + else + { + articulation_scan_start = plan_last_index; + } + + auto articulationSearchStart = [&](unsigned int candidate_start) { + return std::max({candidate_start, articulation_scan_start, 1u}); + }; + + unsigned int goal_index = search_start_index; + double goal_yaw = plan[goal_index].theta; + bool has_forward_direction = false; + bool found_goal = false; + bool forced_skipped_articulation = false; + unsigned int next_articulation_index = 0; + double next_articulation_yaw = 0.0; + bool next_articulation_has_forward = false; + bool has_next_articulation = false; + + if (initial_alignment_done_ && !articulation_indices.empty()) + { + unsigned int articulation_window_start = std::max(search_start_index, last_progress_index_ + 1); + unsigned int articulation_window_end = last_valid_index; + + for (unsigned int idx : articulation_indices) + { + if (idx < articulation_window_start || idx > articulation_window_end) + { + continue; + } + + double articulation_yaw = plan[idx].theta; + bool articulation_has_forward = computeOutgoingAngle(plan, idx, articulation_yaw); + double articulation_goal_yaw = articulation_has_forward ? articulation_yaw : plan[idx].theta; + + double articulation_xy_tolerance = + (final_goal_in_plan_window && idx == plan_last_index) ? final_goal_xy_tolerance_ : xy_local_goal_tolerance_; + double articulation_yaw_tolerance = + (final_goal_in_plan_window && idx == plan_last_index) ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; + + if (isPoseReached(robot_pose, plan[idx], articulation_goal_yaw, articulation_xy_tolerance, + articulation_yaw_tolerance)) + { + last_progress_index_ = std::max(last_progress_index_, idx); + geometry_msgs::Pose2D reached_pose = plan[idx]; + reached_pose.theta = articulation_goal_yaw; + if (reached_intermediate_goals_.empty() || + nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || + fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > + 1e-6) + { + reached_intermediate_goals_.push_back(reached_pose); + } + continue; + } + + next_articulation_index = idx; + next_articulation_yaw = articulation_goal_yaw; + next_articulation_has_forward = articulation_has_forward; + has_next_articulation = true; + break; + } + } + + if (!found_goal && initial_alignment_done_ && search_start_index > last_progress_index_ + 1 && + !articulation_indices.empty()) + { + unsigned int articulation_lower_bound = std::max(last_progress_index_ + 1, 1u); + unsigned int articulation_upper_bound = std::min(search_start_index - 1, last_valid_index); + + if (articulation_lower_bound <= articulation_upper_bound) + { + auto articulation_it = std::find_if(articulation_indices.begin(), articulation_indices.end(), + [&](unsigned int index) { + return index >= articulation_lower_bound && index <= articulation_upper_bound; + }); + + if (articulation_it != articulation_indices.end()) + { + goal_index = *articulation_it; + has_forward_direction = computeOutgoingAngle(plan, goal_index, goal_yaw); + if (!has_forward_direction) + { + goal_yaw = plan[goal_index].theta; + } + + if (enforce_forward_dot_ && has_forward_direction) + { + double to_goal_x = plan[goal_index].x - robot_pose.x; + double to_goal_y = plan[goal_index].y - robot_pose.y; + double dot = to_goal_x * std::cos(goal_yaw) + to_goal_y * std::sin(goal_yaw); + if (dot < 0.0) + { + ROS_WARN_NAMED("PathFollowerCritic", + "Forcing skipped articulation index %u despite backward dot product %.3f due to policy.", + goal_index, dot); + } + } + + forced_skipped_articulation = true; + found_goal = true; + ROS_DEBUG_NAMED("PathFollowerCritic", + "Recovered skipped articulation index %u between progress %u and search start %u.", + goal_index, last_progress_index_, search_start_index); + } + } + } + + unsigned int search_index = search_start_index; + while (!forced_skipped_articulation && search_index <= last_valid_index) + { + double candidate_yaw = goal_yaw; + bool candidate_has_forward = false; + unsigned int candidate_index = + getGoalIndex(plan, search_index, last_valid_index, final_goal_in_plan_window, candidate_yaw, candidate_has_forward); + + bool forced_articulation = false; + if (candidate_index > last_progress_index_) + { + unsigned int articulation_index = 0; + double articulation_yaw = candidate_yaw; + bool articulation_has_forward = candidate_has_forward; + unsigned int articulation_start_index = articulationSearchStart(search_index); + if (articulation_start_index <= candidate_index && + findNextArticulation(plan, articulation_start_index, candidate_index, last_valid_index, + articulation_index, articulation_yaw, articulation_has_forward)) + { + candidate_index = articulation_index; + candidate_yaw = articulation_yaw; + candidate_has_forward = articulation_has_forward; + forced_articulation = true; + } + } + + if (!forced_articulation) + { + candidate_index = std::max(candidate_index, search_index); + } + + if (has_next_articulation && candidate_index >= next_articulation_index) + { + goal_index = next_articulation_index; + goal_yaw = next_articulation_yaw; + has_forward_direction = next_articulation_has_forward; + found_goal = true; + forced_skipped_articulation = true; + has_next_articulation = false; + ROS_DEBUG_NAMED("PathFollowerCritic", + "Selecting pending articulation index %u reached at candidate %u.", goal_index, + candidate_index); + break; + } + + double candidate_xy_tolerance = + (final_goal_in_plan_window && candidate_index == plan_last_index) ? final_goal_xy_tolerance_ : + xy_local_goal_tolerance_; + double candidate_yaw_tolerance = + (final_goal_in_plan_window && candidate_index == plan_last_index) ? final_goal_yaw_tolerance_ : + yaw_local_goal_tolerance_; + + if (isPoseReached(robot_pose, plan[candidate_index], candidate_yaw, candidate_xy_tolerance, + candidate_yaw_tolerance)) + { + last_progress_index_ = std::max(last_progress_index_, candidate_index); + geometry_msgs::Pose2D reached_pose = plan[candidate_index]; + reached_pose.theta = candidate_yaw; + if (reached_intermediate_goals_.empty() || + nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || + fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > 1e-6) + { + reached_intermediate_goals_.push_back(reached_pose); + } + ROS_DEBUG_NAMED("PathFollowerCritic", + "Reached intermediate goal index %u. last_progress_index_: %u", candidate_index, + last_progress_index_); + + if (candidate_index >= last_valid_index) + { + goal_index = candidate_index; + goal_yaw = candidate_yaw; + has_forward_direction = candidate_has_forward; + found_goal = true; + break; + } + + search_index = candidate_index + 1; + continue; + } + + if (enforce_forward_dot_ && candidate_has_forward && !forced_articulation) + { + double to_goal_x = plan[candidate_index].x - robot_pose.x; + double to_goal_y = plan[candidate_index].y - robot_pose.y; + double dot = to_goal_x * std::cos(candidate_yaw) + to_goal_y * std::sin(candidate_yaw); + if (dot < 0.0) + { + ROS_DEBUG_NAMED("PathFollowerCritic", + "Skipping goal index %u due to backward alignment (dot product %.3f)", candidate_index, dot); + if (candidate_index >= last_valid_index) + { + break; + } + search_index = candidate_index + 1; + continue; + } + } + + goal_index = candidate_index; + goal_yaw = candidate_yaw; + has_forward_direction = candidate_has_forward; + found_goal = true; + break; + } + + if (!found_goal) + { + unsigned int fallback_start = std::min(last_valid_index, search_start_index); + goal_index = fallback_start; + bool selected_fallback = false; + + for (; goal_index <= last_valid_index; ++goal_index) + { + has_forward_direction = computeOutgoingAngle(plan, goal_index, goal_yaw); + if (has_forward_direction) + { + if (!enforce_forward_dot_) + { + selected_fallback = true; + break; + } + + double to_goal_x = plan[goal_index].x - robot_pose.x; + double to_goal_y = plan[goal_index].y - robot_pose.y; + double dot = to_goal_x * std::cos(goal_yaw) + to_goal_y * std::sin(goal_yaw); + if (dot >= 0.0) + { + selected_fallback = true; + break; + } + continue; + } + + goal_yaw = plan[goal_index].theta; + if (!enforce_forward_dot_ || goal_index == last_valid_index) + { + selected_fallback = true; + break; + } + } + + if (!selected_fallback) + { + return false; + } + + if (has_next_articulation && goal_index >= next_articulation_index) + { + goal_index = next_articulation_index; + goal_yaw = next_articulation_yaw; + has_forward_direction = next_articulation_has_forward; + has_next_articulation = false; + forced_skipped_articulation = true; + ROS_DEBUG_NAMED("PathFollowerCritic", + "Fallback switching to pending articulation index %u.", goal_index); + } + + if (goal_index > last_progress_index_) + { + unsigned int articulation_index = 0; + double articulation_yaw = goal_yaw; + bool articulation_has_forward = has_forward_direction; + unsigned int articulation_start_index = articulationSearchStart(search_start_index); + if (articulation_start_index <= goal_index && + findNextArticulation(plan, articulation_start_index, goal_index, last_valid_index, + articulation_index, articulation_yaw, articulation_has_forward)) + { + goal_index = articulation_index; + goal_yaw = articulation_yaw; + has_forward_direction = articulation_has_forward; + } + } + } + + if (final_goal_in_plan_window && plan_last_index <= last_valid_index && goal_index != plan_last_index) + { + double snap_threshold = final_goal_xy_tolerance_; + if (snap_threshold >= 0.0) + { + double final_dx = plan[plan_last_index].x - plan[goal_index].x; + double final_dy = plan[plan_last_index].y - plan[goal_index].y; + double distance_to_final = hypot(final_dx, final_dy); + if (distance_to_final <= snap_threshold) + { + ROS_DEBUG_NAMED("PathFollowerCritic", + "Snapping intermediate goal index %u to final goal because distance %.3f <= threshold %.3f", + goal_index, distance_to_final, snap_threshold); + goal_index = plan_last_index; + goal_yaw = plan[plan_last_index].theta; + has_forward_direction = false; + forced_skipped_articulation = false; + has_next_articulation = false; + } + } + } + + bool pending_articulation = false; + if (last_progress_index_ < goal_index) + { + pending_articulation = std::find(articulation_indices.begin(), articulation_indices.end(), goal_index) != + articulation_indices.end(); + } + + // Only consider snapping to the final goal yaw when we have already selected the last path index as goal. + // Otherwise, keep following the path even if the final goal is within tolerance. + if (!pending_articulation && final_goal_xy_tolerance_ >= 0.0 && final_goal_in_plan_window && + plan_last_index <= last_valid_index && goal_index == plan_last_index) + { + double final_dx = plan[plan_last_index].x - plan[goal_index].x; + double final_dy = plan[plan_last_index].y - plan[goal_index].y; + double final_distance = hypot(final_dx, final_dy); + if (final_distance <= final_goal_xy_tolerance_) + { + goal_index = plan_last_index; + goal_yaw = plan[plan_last_index].theta; + has_forward_direction = false; + } + } + + ROS_ASSERT(goal_index <= last_valid_index); + + worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y); + desired_angle = goal_yaw; + + bool same_as_held = false; + if (holding_goal_) + { + double position_diff_x = plan[goal_index].x - held_goal_pose_.x; + double position_diff_y = plan[goal_index].y - held_goal_pose_.y; + double yaw_diff = angles::shortest_angular_distance(goal_yaw, held_goal_pose_.theta); + same_as_held = (fabs(position_diff_x) <= hold_position_epsilon_) && + (fabs(position_diff_y) <= hold_position_epsilon_) && + (fabs(yaw_diff) <= hold_yaw_epsilon_); + } + + if (!same_as_held) + { + held_goal_pose_ = plan[goal_index]; + held_goal_pose_.theta = goal_yaw; + held_goal_index_ = goal_index; + } + else + { + held_goal_pose_.x = plan[goal_index].x; + held_goal_pose_.y = plan[goal_index].y; + held_goal_pose_.theta = goal_yaw; + } + holding_goal_ = true; + + ROS_DEBUG_NAMED("PathFollowerCritic", + "Selected goal index %u (x: %.3f, y: %.3f, yaw: %.3f rad). last_progress_index_: %u", + goal_index, plan[goal_index].x, plan[goal_index].y, goal_yaw, last_progress_index_); + + publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); + return true; +} + +unsigned int PathFollowerCritic::getGoalIndex(const std::vector& plan, unsigned int start_index, + unsigned int last_valid_index, bool plan_tail_is_final_goal, + double& desired_angle, bool& has_forward_direction) const +{ + if (plan.empty()) + { + desired_angle = 0.0; + has_forward_direction = false; + return 0; + } + + const double epsilon = 1e-9; + unsigned int clamped_start = std::min(start_index, static_cast(plan.size() - 1)); + unsigned int clamped_last = std::min(last_valid_index, static_cast(plan.size() - 1)); + + if (clamped_start >= clamped_last) + { + has_forward_direction = computeOutgoingAngle(plan, clamped_start, desired_angle); + if (!has_forward_direction) + { + desired_angle = plan[clamped_start].theta; + } + return clamped_start; + } + + unsigned int goal_index = clamped_start; + double base_angle = 0.0; + bool base_angle_set = false; + double previous_segment_angle = 0.0; + bool previous_segment_angle_set = false; + unsigned int previous_segment_end_index = clamped_start; + double last_valid_segment_angle = 0.0; + bool last_valid_segment_angle_set = false; + + for (unsigned int i = clamped_start + 1; i <= clamped_last; ++i) + { + double direction_x = plan[i].x - plan[i - 1].x; + double direction_y = plan[i].y - plan[i - 1].y; + double length = hypot(direction_x, direction_y); + if (length < epsilon) + { + continue; + } + + double current_angle = atan2(direction_y, direction_x); + last_valid_segment_angle = current_angle; + last_valid_segment_angle_set = true; + + if (!base_angle_set) + { + base_angle = current_angle; + base_angle_set = true; + } + + if (previous_segment_angle_set) + { + double articulation_angle = fabs(angles::shortest_angular_distance(previous_segment_angle, current_angle)); + if (articulation_angle >= articulation_angle_threshold_) + { + goal_index = previous_segment_end_index; + break; + } + } + + double deviation = fabs(angles::shortest_angular_distance(base_angle, current_angle)); + if (deviation > angle_threshold_) + { + break; + } + + goal_index = i; + previous_segment_angle = current_angle; + previous_segment_end_index = i; + previous_segment_angle_set = true; + } + + has_forward_direction = computeOutgoingAngle(plan, goal_index, desired_angle); + if (!has_forward_direction) + { + if (plan_tail_is_final_goal && goal_index == plan.size() - 1) + { + desired_angle = plan[goal_index].theta; + } + else if (previous_segment_angle_set) + { + desired_angle = previous_segment_angle; + has_forward_direction = true; + } + else if (last_valid_segment_angle_set) + { + desired_angle = last_valid_segment_angle; + has_forward_direction = true; + } + else + { + desired_angle = plan[goal_index].theta; + } + } + + if (plan_tail_is_final_goal && goal_index == plan.size() - 1) + { + has_forward_direction = false; + } + + return goal_index; +} + +bool PathFollowerCritic::findNextArticulation(const std::vector& plan, unsigned int start_index, + unsigned int end_index, unsigned int last_valid_index, + unsigned int& articulation_index, double& articulation_yaw, + bool& has_forward_direction) const +{ + const double epsilon = 1e-9; + articulation_index = 0; + articulation_yaw = 0.0; + has_forward_direction = false; + + if (plan.size() < 2) + { + return false; + } + + unsigned int clamped_end = std::min(end_index, static_cast(plan.size() - 1)); + clamped_end = std::min(clamped_end, last_valid_index); + if (clamped_end < 1) + { + return false; + } + + unsigned int scan_start = std::max(start_index, 1u); + if (scan_start > clamped_end) + { + return false; + } + + double previous_segment_angle = 0.0; + bool previous_segment_angle_set = false; + unsigned int previous_segment_end_index = 0; + + for (unsigned int i = scan_start; i <= clamped_end; ++i) + { + double direction_x = plan[i].x - plan[i - 1].x; + double direction_y = plan[i].y - plan[i - 1].y; + double length = hypot(direction_x, direction_y); + if (length < epsilon) + { + continue; + } + + double current_angle = atan2(direction_y, direction_x); + + if (previous_segment_angle_set) + { + double articulation_angle = fabs(angles::shortest_angular_distance(previous_segment_angle, current_angle)); + if (articulation_angle >= articulation_angle_threshold_) + { + articulation_index = previous_segment_end_index; + articulation_yaw = current_angle; + has_forward_direction = true; + if (!computeOutgoingAngle(plan, articulation_index, articulation_yaw)) + { + has_forward_direction = false; + if (articulation_index == plan.size() - 1) + { + articulation_yaw = plan[articulation_index].theta; + } + } + return true; + } + } + + previous_segment_angle = current_angle; + previous_segment_angle_set = true; + previous_segment_end_index = i; + } + + return false; +} + +bool PathFollowerCritic::computeOutgoingAngle(const std::vector& plan, unsigned int index, + double& angle) const +{ + const double epsilon = 1e-9; + if (plan.empty() || index >= plan.size()) + { + return false; + } + + for (unsigned int i = index + 1; i < plan.size(); ++i) + { + double dx = plan[i].x - plan[index].x; + double dy = plan[i].y - plan[index].y; + double length = hypot(dx, dy); + if (length >= epsilon) + { + angle = atan2(dy, dx); + return true; + } + } + + return false; +} + +bool PathFollowerCritic::isPoseReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, + double goal_yaw, double xy_tolerance, double yaw_tolerance) const +{ + double distance = nav_2d_utils::poseDistance(goal_pose, robot_pose); + double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, goal_yaw)); + + return distance < xy_tolerance && yaw_error < yaw_tolerance; +} + +bool PathFollowerCritic::isGoalReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, + double goal_yaw) const +{ + return isPoseReached(robot_pose, goal_pose, goal_yaw, xy_local_goal_tolerance_, yaw_local_goal_tolerance_); +} + +} // namespace mir_dwb_critics + +PLUGINLIB_EXPORT_CLASS(mir_dwb_critics::PathFollowerCritic, dwb_local_planner::TrajectoryCritic) diff --git a/mir_dwb_critics/src/path_progress.cpp b/mir_dwb_critics/src/path_progress.cpp index c7060409..fa296929 100644 --- a/mir_dwb_critics/src/path_progress.cpp +++ b/mir_dwb_critics/src/path_progress.cpp @@ -32,19 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include namespace mir_dwb_critics @@ -55,7 +45,7 @@ bool PathProgressCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d dwb_critics::MapGridCritic::reset(); unsigned int local_goal_x, local_goal_y; - if (!getGoalPose(pose, goal, global_plan, local_goal_x, local_goal_y, desired_angle_)) + if (!getGoalPose(pose, global_plan, local_goal_x, local_goal_y, desired_angle_)) { return false; } @@ -73,101 +63,29 @@ void PathProgressCritic::onInit() { dwb_critics::MapGridCritic::onInit(); critic_nh_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.20); - critic_nh_.param("yaw_local_goal_tolerance", yaw_local_goal_tolerance_, 0.15); - ros::NodeHandle private_nh("~"); - if (!private_nh.getParam("yaw_goal_tolerance", final_goal_yaw_tolerance_)) - { - critic_nh_.param("yaw_goal_tolerance", final_goal_yaw_tolerance_, yaw_local_goal_tolerance_); - } - if (!private_nh.getParam("xy_goal_tolerance", final_goal_xy_tolerance_)) - { - critic_nh_.param("xy_goal_tolerance", final_goal_xy_tolerance_, xy_local_goal_tolerance_); - } critic_nh_.param("angle_threshold", angle_threshold_, M_PI_4); - critic_nh_.param("articulation_angle_threshold", articulation_angle_threshold_, 1.3089969389957472); critic_nh_.param("heading_scale", heading_scale_, 1.0); - critic_nh_.param("enforce_forward_dot", enforce_forward_dot_, true); - critic_nh_.param("always_target_articulations", always_target_articulations_, true); - initial_alignment_done_ = false; - - intermediate_goal_pub_ = critic_nh_.advertise("intermediate_goal", 1); - articulation_points_pub_ = critic_nh_.advertise("articulation_points", 1); - intermediate_goal_tolerance_pub_ = - critic_nh_.advertise("intermediate_goal_tolerance", 1); - - articulation_angle_threshold_ = std::max(articulation_angle_threshold_, angle_threshold_); // divide heading scale by position scale because the sum will be multiplied by scale again heading_scale_ /= getScale(); - last_progress_index_ = 0; - reached_intermediate_goals_.clear(); - holding_goal_ = false; - held_goal_index_ = 0; - held_goal_pose_.x = 0.0; - held_goal_pose_.y = 0.0; - held_goal_pose_.theta = 0.0; - hold_position_epsilon_ = 1e-6; - hold_yaw_epsilon_ = 1e-6; - final_goal_yaw_tolerance_ = std::max(final_goal_yaw_tolerance_, 1e-6); - final_goal_xy_tolerance_ = std::max(final_goal_xy_tolerance_, 0.0); - have_last_plan_ = false; - last_plan_size_ = 0; - last_plan_end_pose_.x = 0.0; - last_plan_end_pose_.y = 0.0; - last_plan_end_pose_.theta = 0.0; - last_plan_frame_id_.clear(); - last_plan_stamp_ = ros::Time(0); - last_plan_seq_ = 0u; - last_plan_start_pose_.x = 0.0; - last_plan_start_pose_.y = 0.0; - last_plan_start_pose_.theta = 0.0; - last_final_goal_pose_.x = 0.0; - last_final_goal_pose_.y = 0.0; - last_final_goal_pose_.theta = 0.0; - have_last_goal_pose_ = false; - critic_nh_.param("plan_alignment_position_tolerance", plan_alignment_position_tolerance_, 0.15); - critic_nh_.param("plan_alignment_yaw_tolerance", plan_alignment_yaw_tolerance_, 3.14159265358979323846); } void PathProgressCritic::reset() { reached_intermediate_goals_.clear(); - last_progress_index_ = 0; - holding_goal_ = false; - held_goal_index_ = 0; - held_goal_pose_.x = 0.0; - held_goal_pose_.y = 0.0; - held_goal_pose_.theta = 0.0; - initial_alignment_done_ = false; - have_last_plan_ = false; - last_plan_size_ = 0; - last_plan_end_pose_.x = 0.0; - last_plan_end_pose_.y = 0.0; - last_plan_end_pose_.theta = 0.0; - last_plan_frame_id_.clear(); - last_plan_stamp_ = ros::Time(0); - last_plan_seq_ = 0u; - last_plan_start_pose_.x = 0.0; - last_plan_start_pose_.y = 0.0; - last_plan_start_pose_.theta = 0.0; - last_final_goal_pose_.x = 0.0; - last_final_goal_pose_.y = 0.0; - last_final_goal_pose_.theta = 0.0; - have_last_goal_pose_ = false; } double PathProgressCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj) { double position_score = MapGridCritic::scoreTrajectory(traj); - double heading_diff = fabs(angles::shortest_angular_distance(traj.poses.back().theta, desired_angle_)); + double heading_diff = fabs(remainder(traj.poses.back().theta - desired_angle_, 2 * M_PI)); double heading_score = heading_diff * heading_diff; return position_score + heading_scale_ * heading_score; } -bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& final_goal, - const nav_2d_msgs::Path2D& global_plan, unsigned int& x, unsigned int& y, - double& desired_angle) +bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, const nav_2d_msgs::Path2D& global_plan, + unsigned int& x, unsigned int& y, double& desired_angle) { const nav_core2::Costmap& costmap = *costmap_; const nav_grid::NavGridInfo& info = costmap.getInfo(); @@ -180,271 +98,6 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co std::vector plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses; - auto posesDiffer = [&](const geometry_msgs::Pose2D& a, const geometry_msgs::Pose2D& b, double position_tolerance, - double yaw_tolerance) { - double dx = a.x - b.x; - double dy = a.y - b.y; - double distance = hypot(dx, dy); - if (distance > position_tolerance) - { - return true; - } - - double yaw_error = fabs(angles::shortest_angular_distance(a.theta, b.theta)); - return yaw_error > yaw_tolerance; - }; - - if (plan.empty()) - { - ROS_ERROR_NAMED("PathProgressCritic", "The adjusted global plan was empty."); - return false; - } - - auto planPoseMatchesFinalGoal = [&](const geometry_msgs::Pose2D& plan_pose) { - return !posesDiffer(plan_pose, final_goal, plan_alignment_position_tolerance_, plan_alignment_yaw_tolerance_); - }; - - bool final_goal_in_plan_window = planPoseMatchesFinalGoal(plan.back()); - - auto matchPoseToPlanIndex = [&](const geometry_msgs::Pose2D& pose, unsigned int& index_out) { - bool found = false; - double best_distance = std::numeric_limits::infinity(); - - for (unsigned int idx = 0; idx < plan.size(); ++idx) - { - double dx = plan[idx].x - pose.x; - double dy = plan[idx].y - pose.y; - double distance = hypot(dx, dy); - if (distance > plan_alignment_position_tolerance_) - { - continue; - } - - double yaw_error = fabs(angles::shortest_angular_distance(plan[idx].theta, pose.theta)); - if (yaw_error > plan_alignment_yaw_tolerance_) - { - continue; - } - - if (distance < best_distance) - { - best_distance = distance; - index_out = idx; - found = true; - } - } - - return found; - }; - - // Reset state if a new global plan arrives (detected via metadata changes or major window shifts) - bool plan_changed = false; - geometry_msgs::Pose2D plan_start_pose = plan.front(); - - auto headerStampChanged = [&]() { - if (last_plan_stamp_.isZero() || global_plan.header.stamp.isZero()) - { - return false; - } - return last_plan_stamp_ != global_plan.header.stamp; - }; - - auto headerSeqChanged = [&]() { - if (!have_last_plan_) - { - return false; - } - return last_plan_seq_ != global_plan.header.seq; - }; - - auto goalPoseChanged = [&]() { - if (!have_last_goal_pose_) - { - return true; - } - return posesDiffer(final_goal, last_final_goal_pose_, plan_alignment_position_tolerance_, - plan_alignment_yaw_tolerance_); - }; - - auto planStartChanged = [&]() { - if (!have_last_plan_) - { - return true; - } - // Allow the cropped window to slide by a generous distance without counting as a full plan change. - double position_tolerance = std::max(plan_alignment_position_tolerance_ * 4.0, plan_alignment_position_tolerance_); - return posesDiffer(plan_start_pose, last_plan_start_pose_, position_tolerance, plan_alignment_yaw_tolerance_); - }; - - if (!have_last_plan_ || global_plan.header.frame_id != last_plan_frame_id_ || headerStampChanged() || - headerSeqChanged() || - goalPoseChanged()) - { - plan_changed = true; - } - else if (planStartChanged()) - { - plan_changed = true; - } - - if (plan_changed) - { - bool state_preserved = false; - bool progress_match_found = false; - unsigned int restored_progress_index = 0u; - std::vector> preserved_reached; - preserved_reached.reserve(reached_intermediate_goals_.size()); - - unsigned int previous_progress_index = last_progress_index_; - - for (const auto& reached_pose : reached_intermediate_goals_) - { - unsigned int matched_index = 0u; - if (matchPoseToPlanIndex(reached_pose, matched_index)) - { - geometry_msgs::Pose2D preserved_pose = reached_pose; - preserved_pose.x = plan[matched_index].x; - preserved_pose.y = plan[matched_index].y; - preserved_reached.emplace_back(matched_index, preserved_pose); - state_preserved = true; - progress_match_found = true; - restored_progress_index = std::max(restored_progress_index, matched_index); - } - } - - unsigned int held_match_index = 0u; - if (holding_goal_) - { - if (matchPoseToPlanIndex(held_goal_pose_, held_match_index)) - { - state_preserved = true; - held_goal_index_ = held_match_index; - held_goal_pose_.x = plan[held_match_index].x; - held_goal_pose_.y = plan[held_match_index].y; - } - else - { - holding_goal_ = false; - held_goal_index_ = 0u; - held_goal_pose_.x = 0.0; - held_goal_pose_.y = 0.0; - held_goal_pose_.theta = 0.0; - } - } - - unsigned int robot_match_index = 0u; - if (matchPoseToPlanIndex(robot_pose, robot_match_index)) - { - state_preserved = true; - - double match_goal_yaw = plan[robot_match_index].theta; - if (!computeOutgoingAngle(plan, robot_match_index, match_goal_yaw)) - { - match_goal_yaw = plan[robot_match_index].theta; - } - - bool match_is_final_index = ((robot_match_index + 1u) >= plan.size()) && final_goal_in_plan_window; - double match_yaw_tolerance = match_is_final_index ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; - double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, match_goal_yaw)); - - if (yaw_error <= match_yaw_tolerance) - { - progress_match_found = true; - restored_progress_index = std::max(restored_progress_index, robot_match_index); - } - else - { - ROS_DEBUG_NAMED("PathProgressCritic", - "Robot pose matched plan index %u but yaw error %.3f rad exceeds tolerance %.3f." - " Preserving state without increasing progress.", - robot_match_index, yaw_error, match_yaw_tolerance); - } - } - - if (state_preserved) - { - if (progress_match_found) - { - std::sort(preserved_reached.begin(), preserved_reached.end(), - [](const std::pair& lhs, - const std::pair& rhs) { - return lhs.first < rhs.first; - }); - reached_intermediate_goals_.clear(); - reached_intermediate_goals_.reserve(preserved_reached.size()); - for (const auto& entry : preserved_reached) - { - reached_intermediate_goals_.push_back(entry.second); - } - unsigned int plan_last_index = plan.empty() ? 0u : static_cast(plan.size() - 1); - last_progress_index_ = std::min(restored_progress_index, plan_last_index); - } - else - { - reached_intermediate_goals_.clear(); - unsigned int plan_last_index = plan.empty() ? 0u : static_cast(plan.size() - 1); - last_progress_index_ = std::min(previous_progress_index, plan_last_index); - } - } - else - { - reached_intermediate_goals_.clear(); - last_progress_index_ = 0u; - holding_goal_ = false; - held_goal_index_ = 0u; - held_goal_pose_.x = 0.0; - held_goal_pose_.y = 0.0; - held_goal_pose_.theta = 0.0; - initial_alignment_done_ = false; - } - - have_last_plan_ = true; - } - - last_plan_size_ = plan.size(); - last_plan_end_pose_ = plan.back(); - last_plan_frame_id_ = global_plan.header.frame_id; - last_plan_stamp_ = global_plan.header.stamp; - last_plan_seq_ = global_plan.header.seq; - last_plan_start_pose_ = plan_start_pose; - last_final_goal_pose_ = final_goal; - have_last_goal_pose_ = true; - - - unsigned int plan_last_index = static_cast(plan.size() - 1); - - if (initial_alignment_done_ && plan.size() > 1 && last_progress_index_ > 0) - { - unsigned int prune_limit = std::min(last_progress_index_, plan_last_index); - if (prune_limit > 0) - { - plan.erase(plan.begin(), plan.begin() + prune_limit); - last_progress_index_ = last_progress_index_ >= prune_limit ? last_progress_index_ - prune_limit : 0; - plan_last_index = static_cast(plan.size() - 1); - - if (holding_goal_) - { - if (held_goal_index_ < prune_limit) - { - holding_goal_ = false; - held_goal_index_ = 0; - } - else - { - held_goal_index_ -= prune_limit; - } - } - } - } - - if (holding_goal_ && held_goal_index_ >= plan.size()) - { - ROS_DEBUG_NAMED("PathProgressCritic", "Held goal index %u is out of range for current plan of size %zu. Releasing hold.", - held_goal_index_, plan.size()); - holding_goal_ = false; - held_goal_index_ = 0; - } - // find the "start pose", i.e. the pose on the plan closest to the robot that is also on the local map unsigned int start_index = 0; double distance_to_start = std::numeric_limits::infinity(); @@ -504,834 +157,94 @@ bool PathProgressCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } } - auto collectArticulationIndices = [&](unsigned int scan_start, unsigned int scan_end) { - std::vector articulation_indices; - if (plan.size() < 2) - { - return articulation_indices; - } - - const double epsilon = 1e-9; - unsigned int clamped_start = std::max(scan_start, 1u); - unsigned int clamped_end = std::min(scan_end, plan_last_index); - if (clamped_start > clamped_end) - { - return articulation_indices; - } - - double previous_segment_angle = 0.0; - bool previous_segment_angle_set = false; - unsigned int previous_segment_end_index = 0u; + // find the "goal pose" by walking along the plan as long as each step leads far enough away from the starting point, + // i.e. is within angle_threshold_ of the starting direction. + unsigned int goal_index = start_index; - for (unsigned int i = clamped_start; i <= clamped_end; ++i) - { - double direction_x = plan[i].x - plan[i - 1].x; - double direction_y = plan[i].y - plan[i - 1].y; - double length = hypot(direction_x, direction_y); - if (length < epsilon) - { - continue; - } - - double current_angle = atan2(direction_y, direction_x); - if (previous_segment_angle_set) - { - double articulation_angle = - fabs(angles::shortest_angular_distance(previous_segment_angle, current_angle)); - if (articulation_angle >= articulation_angle_threshold_) - { - if (articulation_indices.empty() || articulation_indices.back() != previous_segment_end_index) - { - articulation_indices.push_back(previous_segment_end_index); - } - } - } - - previous_segment_angle = current_angle; - previous_segment_angle_set = true; - previous_segment_end_index = i; - } - - return articulation_indices; - }; - - auto publishArticulationPointCloud = [&](const std::vector& articulation_indices) { - if (!articulation_points_pub_) - { - return; - } - - sensor_msgs::PointCloud cloud_msg; - cloud_msg.header = global_plan.header; - cloud_msg.header.stamp = ros::Time::now(); - cloud_msg.points.reserve(articulation_indices.size()); - - for (unsigned int index : articulation_indices) - { - if (index >= plan.size()) - { - continue; - } - - geometry_msgs::Point32 point; - point.x = plan[index].x; - point.y = plan[index].y; - point.z = 0.0; - cloud_msg.points.push_back(point); - } - - articulation_points_pub_.publish(cloud_msg); - }; - - auto publishIntermediateGoal = [&](const geometry_msgs::Pose2D& goal_pose, double goal_yaw) { - if (!intermediate_goal_pub_) - { - return; - } - geometry_msgs::PoseStamped goal_msg; - goal_msg.header = global_plan.header; - goal_msg.header.stamp = ros::Time::now(); - goal_msg.pose.position.x = goal_pose.x; - goal_msg.pose.position.y = goal_pose.y; - goal_msg.pose.position.z = 0.0; - tf2::Quaternion q; - q.setRPY(0.0, 0.0, goal_yaw); - goal_msg.pose.orientation.x = q.x(); - goal_msg.pose.orientation.y = q.y(); - goal_msg.pose.orientation.z = q.z(); - goal_msg.pose.orientation.w = q.w(); - intermediate_goal_pub_.publish(goal_msg); - - if (intermediate_goal_tolerance_pub_) - { - visualization_msgs::MarkerArray marker_array; - visualization_msgs::Marker marker; - marker.header = goal_msg.header; - marker.header.stamp = goal_msg.header.stamp; - marker.ns = "intermediate_goal_tolerance"; - marker.id = 0; - marker.type = visualization_msgs::Marker::SPHERE; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position = goal_msg.pose.position; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - double diameter = std::max(2.0 * xy_local_goal_tolerance_, 1e-6); - marker.scale.x = diameter; - marker.scale.y = diameter; - marker.scale.z = diameter; - marker.color.r = 0.2f; - marker.color.g = 0.8f; - marker.color.b = 0.4f; - marker.color.a = 0.35f; - marker.lifetime = ros::Duration(0.0); - marker_array.markers.push_back(marker); - intermediate_goal_tolerance_pub_.publish(marker_array); - } - }; - - if (!initial_alignment_done_) - { - double desired_initial_yaw = plan.front().theta; - if (!plan.empty()) - { - double outgoing_angle = desired_initial_yaw; - if (computeOutgoingAngle(plan, 0u, outgoing_angle)) - { - desired_initial_yaw = outgoing_angle; - } - } - - double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, desired_initial_yaw)); - if (yaw_error >= yaw_local_goal_tolerance_) - { - unsigned int rx = 0; - unsigned int ry = 0; - if (worldToGridBounded(info, robot_pose.x, robot_pose.y, rx, ry)) - { - x = rx; - y = ry; - desired_angle = desired_initial_yaw; - held_goal_pose_.x = robot_pose.x; - held_goal_pose_.y = robot_pose.y; - held_goal_pose_.theta = desired_initial_yaw; - held_goal_index_ = 0; - holding_goal_ = true; - publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); - ROS_DEBUG_NAMED("PathProgressCritic", - "Initial alignment: rotate in place to yaw %.3f rad at robot pose (x: %.3f, y: %.3f)", - held_goal_pose_.theta, held_goal_pose_.x, held_goal_pose_.y); - return true; - } - } - - initial_alignment_done_ = true; - } - - std::vector articulation_indices = collectArticulationIndices(1u, plan_last_index); - publishArticulationPointCloud(articulation_indices); - - if (holding_goal_) - { - unsigned int held_x = 0; - unsigned int held_y = 0; - bool held_in_costmap = worldToGridBounded(info, held_goal_pose_.x, held_goal_pose_.y, held_x, held_y); - - if (!held_in_costmap) - { - ROS_WARN_NAMED("PathProgressCritic", - "Held goal (index %u) is outside the local costmap. Releasing hold to search for a new goal.", - held_goal_index_); - holding_goal_ = false; - held_goal_index_ = 0; - } - else - { - double held_xy_tolerance = - (final_goal_in_plan_window && held_goal_index_ == plan_last_index) ? final_goal_xy_tolerance_ : - xy_local_goal_tolerance_; - double held_yaw_tolerance = - (final_goal_in_plan_window && held_goal_index_ == plan_last_index) ? final_goal_yaw_tolerance_ : - yaw_local_goal_tolerance_; - - double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, held_goal_pose_.theta)); - if (yaw_error >= held_yaw_tolerance) - { - x = held_x; - y = held_y; - desired_angle = held_goal_pose_.theta; - publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); - ROS_DEBUG_NAMED("PathProgressCritic", "Holding goal index %u due to yaw error %.3f rad (threshold %.3f)", - held_goal_index_, yaw_error, held_yaw_tolerance); - return true; - } - - if (!isPoseReached(robot_pose, held_goal_pose_, held_goal_pose_.theta, held_xy_tolerance, held_yaw_tolerance)) - { - x = held_x; - y = held_y; - desired_angle = held_goal_pose_.theta; - publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); - ROS_DEBUG_NAMED("PathProgressCritic", - "Holding goal index %u until full pose tolerance satisfied (XY + yaw)", held_goal_index_); - return true; - } - - last_progress_index_ = std::max(last_progress_index_, held_goal_index_); - geometry_msgs::Pose2D reached_pose = held_goal_pose_; - if (reached_intermediate_goals_.empty() || - nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || - fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > 1e-6) - { - reached_intermediate_goals_.push_back(reached_pose); - } - ROS_DEBUG_NAMED("PathProgressCritic", - "Reached held intermediate goal index %u while respecting pose tolerances. last_progress_index_: %u", - held_goal_index_, last_progress_index_); - holding_goal_ = false; - } - } - - // Constrain the search range to enforce monotonic progress with the updated bookkeeping. - last_progress_index_ = std::min(last_progress_index_, plan_last_index); - - unsigned int search_start_index = std::max(start_index, last_progress_index_); - search_start_index = std::min(search_start_index, last_valid_index); - - if (holding_goal_ && held_goal_index_ >= last_progress_index_) - { - search_start_index = std::min(search_start_index, held_goal_index_); - } - - unsigned int articulation_scan_start = 0u; - if (plan_last_index >= 1) - { - unsigned int next_index = last_progress_index_ < plan_last_index ? last_progress_index_ + 1 : plan_last_index; - articulation_scan_start = std::max(next_index, 1u); - } - else - { - articulation_scan_start = plan_last_index; - } - - auto articulationSearchStart = [&](unsigned int candidate_start) { - return std::max({candidate_start, articulation_scan_start, 1u}); - }; - - unsigned int goal_index = search_start_index; - double goal_yaw = plan[goal_index].theta; - bool has_forward_direction = false; - bool found_goal = false; - bool forced_skipped_articulation = false; - unsigned int next_articulation_index = 0; - double next_articulation_yaw = 0.0; - bool next_articulation_has_forward = false; - bool has_next_articulation = false; - - if (initial_alignment_done_ && !articulation_indices.empty()) + while (goal_index < last_valid_index) { - unsigned int articulation_window_start = std::max(search_start_index, last_progress_index_ + 1); - unsigned int articulation_window_end = last_valid_index; + goal_index = getGoalIndex(plan, start_index, last_valid_index); - for (unsigned int idx : articulation_indices) + // check if goal already reached + bool goal_already_reached = false; + for (auto& reached_intermediate_goal : reached_intermediate_goals_) { - if (idx < articulation_window_start || idx > articulation_window_end) + double distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]); + if (distance < xy_local_goal_tolerance_) { - continue; - } - - double articulation_yaw = plan[idx].theta; - bool articulation_has_forward = computeOutgoingAngle(plan, idx, articulation_yaw); - double articulation_goal_yaw = articulation_has_forward ? articulation_yaw : plan[idx].theta; - - double articulation_xy_tolerance = - (final_goal_in_plan_window && idx == plan_last_index) ? final_goal_xy_tolerance_ : xy_local_goal_tolerance_; - double articulation_yaw_tolerance = - (final_goal_in_plan_window && idx == plan_last_index) ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; - - if (isPoseReached(robot_pose, plan[idx], articulation_goal_yaw, articulation_xy_tolerance, - articulation_yaw_tolerance)) - { - last_progress_index_ = std::max(last_progress_index_, idx); - geometry_msgs::Pose2D reached_pose = plan[idx]; - reached_pose.theta = articulation_goal_yaw; - if (reached_intermediate_goals_.empty() || - nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || - fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > - 1e-6) + goal_already_reached = true; + // choose a new start_index by walking along the plan until we're outside xy_local_goal_tolerance_ and try again + // (start_index is now > goal_index) + for (start_index = goal_index; start_index <= last_valid_index; ++start_index) { - reached_intermediate_goals_.push_back(reached_pose); - } - continue; - } - - next_articulation_index = idx; - next_articulation_yaw = articulation_goal_yaw; - next_articulation_has_forward = articulation_has_forward; - has_next_articulation = true; - break; - } - } - - if (!found_goal && initial_alignment_done_ && search_start_index > last_progress_index_ + 1 && - !articulation_indices.empty()) - { - unsigned int articulation_lower_bound = std::max(last_progress_index_ + 1, 1u); - unsigned int articulation_upper_bound = std::min(search_start_index - 1, last_valid_index); - - if (articulation_lower_bound <= articulation_upper_bound) - { - auto articulation_it = std::find_if(articulation_indices.begin(), articulation_indices.end(), - [&](unsigned int index) { - return index >= articulation_lower_bound && index <= articulation_upper_bound; - }); - - if (articulation_it != articulation_indices.end()) - { - goal_index = *articulation_it; - has_forward_direction = computeOutgoingAngle(plan, goal_index, goal_yaw); - if (!has_forward_direction) - { - goal_yaw = plan[goal_index].theta; - } - - if (enforce_forward_dot_ && has_forward_direction) - { - double to_goal_x = plan[goal_index].x - robot_pose.x; - double to_goal_y = plan[goal_index].y - robot_pose.y; - double dot = to_goal_x * std::cos(goal_yaw) + to_goal_y * std::sin(goal_yaw); - if (dot < 0.0) + distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]); + if (distance >= xy_local_goal_tolerance_) { - ROS_WARN_NAMED("PathProgressCritic", - "Forcing skipped articulation index %u despite backward dot product %.3f due to policy.", - goal_index, dot); + break; } } - - forced_skipped_articulation = true; - found_goal = true; - ROS_DEBUG_NAMED("PathProgressCritic", - "Recovered skipped articulation index %u between progress %u and search start %u.", - goal_index, last_progress_index_, search_start_index); - } - } - } - - unsigned int search_index = search_start_index; - while (!forced_skipped_articulation && search_index <= last_valid_index) - { - double candidate_yaw = goal_yaw; - bool candidate_has_forward = false; - unsigned int candidate_index = - getGoalIndex(plan, search_index, last_valid_index, final_goal_in_plan_window, candidate_yaw, candidate_has_forward); - - bool forced_articulation = false; - if (candidate_index > last_progress_index_) - { - unsigned int articulation_index = 0; - double articulation_yaw = candidate_yaw; - bool articulation_has_forward = candidate_has_forward; - unsigned int articulation_start_index = articulationSearchStart(search_index); - if (articulation_start_index <= candidate_index && - findNextArticulation(plan, articulation_start_index, candidate_index, last_valid_index, - articulation_index, articulation_yaw, articulation_has_forward)) - { - candidate_index = articulation_index; - candidate_yaw = articulation_yaw; - candidate_has_forward = articulation_has_forward; - forced_articulation = true; - } - } - - if (!forced_articulation) - { - candidate_index = std::max(candidate_index, search_index); - } - - if (has_next_articulation && candidate_index >= next_articulation_index) - { - goal_index = next_articulation_index; - goal_yaw = next_articulation_yaw; - has_forward_direction = next_articulation_has_forward; - found_goal = true; - forced_skipped_articulation = true; - has_next_articulation = false; - ROS_DEBUG_NAMED("PathProgressCritic", - "Selecting pending articulation index %u reached at candidate %u.", goal_index, - candidate_index); - break; - } - - double candidate_xy_tolerance = - (final_goal_in_plan_window && candidate_index == plan_last_index) ? final_goal_xy_tolerance_ : - xy_local_goal_tolerance_; - double candidate_yaw_tolerance = - (final_goal_in_plan_window && candidate_index == plan_last_index) ? final_goal_yaw_tolerance_ : - yaw_local_goal_tolerance_; - - if (isPoseReached(robot_pose, plan[candidate_index], candidate_yaw, candidate_xy_tolerance, - candidate_yaw_tolerance)) - { - last_progress_index_ = std::max(last_progress_index_, candidate_index); - geometry_msgs::Pose2D reached_pose = plan[candidate_index]; - reached_pose.theta = candidate_yaw; - if (reached_intermediate_goals_.empty() || - nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || - fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > 1e-6) - { - reached_intermediate_goals_.push_back(reached_pose); - } - ROS_DEBUG_NAMED("PathProgressCritic", - "Reached intermediate goal index %u. last_progress_index_: %u", candidate_index, - last_progress_index_); - - if (candidate_index >= last_valid_index) - { - goal_index = candidate_index; - goal_yaw = candidate_yaw; - has_forward_direction = candidate_has_forward; - found_goal = true; break; } - - search_index = candidate_index + 1; - continue; } - - if (enforce_forward_dot_ && candidate_has_forward && !forced_articulation) + if (!goal_already_reached) { - double to_goal_x = plan[candidate_index].x - robot_pose.x; - double to_goal_y = plan[candidate_index].y - robot_pose.y; - double dot = to_goal_x * std::cos(candidate_yaw) + to_goal_y * std::sin(candidate_yaw); - if (dot < 0.0) + // new goal not in reached_intermediate_goals_ + double distance = nav_2d_utils::poseDistance(plan[goal_index], robot_pose); + if (distance < xy_local_goal_tolerance_) { - ROS_DEBUG_NAMED("PathProgressCritic", - "Skipping goal index %u due to backward alignment (dot product %.3f)", candidate_index, dot); - if (candidate_index >= last_valid_index) - { - break; - } - search_index = candidate_index + 1; - continue; + // the robot has currently reached the goal + reached_intermediate_goals_.push_back(plan[goal_index]); + ROS_DEBUG_NAMED("PathProgressCritic", "Number of reached_intermediate goals: %zu", + reached_intermediate_goals_.size()); } - } - - goal_index = candidate_index; - goal_yaw = candidate_yaw; - has_forward_direction = candidate_has_forward; - found_goal = true; - break; - } - - if (!found_goal) - { - unsigned int fallback_start = std::min(last_valid_index, search_start_index); - goal_index = fallback_start; - bool selected_fallback = false; - - for (; goal_index <= last_valid_index; ++goal_index) - { - has_forward_direction = computeOutgoingAngle(plan, goal_index, goal_yaw); - if (has_forward_direction) - { - if (!enforce_forward_dot_) - { - selected_fallback = true; - break; - } - - double to_goal_x = plan[goal_index].x - robot_pose.x; - double to_goal_y = plan[goal_index].y - robot_pose.y; - double dot = to_goal_x * std::cos(goal_yaw) + to_goal_y * std::sin(goal_yaw); - if (dot >= 0.0) - { - selected_fallback = true; - break; - } - continue; - } - - goal_yaw = plan[goal_index].theta; - if (!enforce_forward_dot_ || goal_index == last_valid_index) + else { - selected_fallback = true; + // we've found a new goal! break; } } - - if (!selected_fallback) - { - return false; - } - - if (has_next_articulation && goal_index >= next_articulation_index) - { - goal_index = next_articulation_index; - goal_yaw = next_articulation_yaw; - has_forward_direction = next_articulation_has_forward; - has_next_articulation = false; - forced_skipped_articulation = true; - ROS_DEBUG_NAMED("PathProgressCritic", - "Fallback switching to pending articulation index %u.", goal_index); - } - - if (goal_index > last_progress_index_) - { - unsigned int articulation_index = 0; - double articulation_yaw = goal_yaw; - bool articulation_has_forward = has_forward_direction; - unsigned int articulation_start_index = articulationSearchStart(search_start_index); - if (articulation_start_index <= goal_index && - findNextArticulation(plan, articulation_start_index, goal_index, last_valid_index, - articulation_index, articulation_yaw, articulation_has_forward)) - { - goal_index = articulation_index; - goal_yaw = articulation_yaw; - has_forward_direction = articulation_has_forward; - } - } - } - - if (final_goal_in_plan_window && plan_last_index <= last_valid_index && goal_index != plan_last_index) - { - double snap_threshold = final_goal_xy_tolerance_; - if (snap_threshold >= 0.0) - { - double final_dx = plan[plan_last_index].x - plan[goal_index].x; - double final_dy = plan[plan_last_index].y - plan[goal_index].y; - double distance_to_final = hypot(final_dx, final_dy); - if (distance_to_final <= snap_threshold) - { - ROS_DEBUG_NAMED("PathProgressCritic", - "Snapping intermediate goal index %u to final goal because distance %.3f <= threshold %.3f", - goal_index, distance_to_final, snap_threshold); - goal_index = plan_last_index; - goal_yaw = plan[plan_last_index].theta; - has_forward_direction = false; - forced_skipped_articulation = false; - has_next_articulation = false; - } - } - } - - bool pending_articulation = false; - if (last_progress_index_ < goal_index) - { - pending_articulation = std::find(articulation_indices.begin(), articulation_indices.end(), goal_index) != - articulation_indices.end(); - } - - // Only consider snapping to the final goal yaw when we have already selected the last path index as goal. - // Otherwise, keep following the path even if the final goal is within tolerance. - if (!pending_articulation && final_goal_xy_tolerance_ >= 0.0 && final_goal_in_plan_window && - plan_last_index <= last_valid_index && goal_index == plan_last_index) - { - double final_dx = plan[plan_last_index].x - plan[goal_index].x; - double final_dy = plan[plan_last_index].y - plan[goal_index].y; - double final_distance = hypot(final_dx, final_dy); - if (final_distance <= final_goal_xy_tolerance_) - { - goal_index = plan_last_index; - goal_yaw = plan[plan_last_index].theta; - has_forward_direction = false; - } } - + if (start_index > goal_index) + start_index = goal_index; ROS_ASSERT(goal_index <= last_valid_index); + // save goal in x, y worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y); - desired_angle = goal_yaw; - - bool same_as_held = false; - if (holding_goal_) - { - double position_diff_x = plan[goal_index].x - held_goal_pose_.x; - double position_diff_y = plan[goal_index].y - held_goal_pose_.y; - double yaw_diff = angles::shortest_angular_distance(goal_yaw, held_goal_pose_.theta); - same_as_held = (fabs(position_diff_x) <= hold_position_epsilon_) && - (fabs(position_diff_y) <= hold_position_epsilon_) && - (fabs(yaw_diff) <= hold_yaw_epsilon_); - } - - if (!same_as_held) - { - held_goal_pose_ = plan[goal_index]; - held_goal_pose_.theta = goal_yaw; - held_goal_index_ = goal_index; - } - else - { - held_goal_pose_.x = plan[goal_index].x; - held_goal_pose_.y = plan[goal_index].y; - held_goal_pose_.theta = goal_yaw; - } - holding_goal_ = true; - - ROS_DEBUG_NAMED("PathProgressCritic", - "Selected goal index %u (x: %.3f, y: %.3f, yaw: %.3f rad). last_progress_index_: %u", - goal_index, plan[goal_index].x, plan[goal_index].y, goal_yaw, last_progress_index_); - - publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); + desired_angle = plan[start_index].theta; return true; } unsigned int PathProgressCritic::getGoalIndex(const std::vector& plan, unsigned int start_index, - unsigned int last_valid_index, bool plan_tail_is_final_goal, - double& desired_angle, bool& has_forward_direction) const + unsigned int last_valid_index) const { - if (plan.empty()) - { - desired_angle = 0.0; - has_forward_direction = false; - return 0; - } - - const double epsilon = 1e-9; - unsigned int clamped_start = std::min(start_index, static_cast(plan.size() - 1)); - unsigned int clamped_last = std::min(last_valid_index, static_cast(plan.size() - 1)); - - if (clamped_start >= clamped_last) - { - has_forward_direction = computeOutgoingAngle(plan, clamped_start, desired_angle); - if (!has_forward_direction) - { - desired_angle = plan[clamped_start].theta; - } - return clamped_start; - } - - unsigned int goal_index = clamped_start; - double base_angle = 0.0; - bool base_angle_set = false; - double previous_segment_angle = 0.0; - bool previous_segment_angle_set = false; - unsigned int previous_segment_end_index = clamped_start; - double last_valid_segment_angle = 0.0; - bool last_valid_segment_angle_set = false; + if (start_index >= last_valid_index) + return last_valid_index; - for (unsigned int i = clamped_start + 1; i <= clamped_last; ++i) + unsigned int goal_index = start_index; + const double start_direction_x = plan[start_index + 1].x - plan[start_index].x; + const double start_direction_y = plan[start_index + 1].y - plan[start_index].y; + if (fabs(start_direction_x) > 1e-9 || fabs(start_direction_y) > 1e-9) { - double direction_x = plan[i].x - plan[i - 1].x; - double direction_y = plan[i].y - plan[i - 1].y; - double length = hypot(direction_x, direction_y); - if (length < epsilon) - { - continue; - } - - double current_angle = atan2(direction_y, direction_x); - last_valid_segment_angle = current_angle; - last_valid_segment_angle_set = true; + // make sure that atan2 is defined + double start_angle = atan2(start_direction_y, start_direction_x); - if (!base_angle_set) + for (unsigned int i = start_index + 1; i <= last_valid_index; ++i) { - base_angle = current_angle; - base_angle_set = true; - } - - if (previous_segment_angle_set) - { - double articulation_angle = fabs(angles::shortest_angular_distance(previous_segment_angle, current_angle)); - if (articulation_angle >= articulation_angle_threshold_) + const double current_direction_x = plan[i].x - plan[i - 1].x; + const double current_direction_y = plan[i].y - plan[i - 1].y; + if (fabs(current_direction_x) > 1e-9 || fabs(current_direction_y) > 1e-9) { - goal_index = previous_segment_end_index; - break; - } - } - - double deviation = fabs(angles::shortest_angular_distance(base_angle, current_angle)); - if (deviation > angle_threshold_) - { - break; - } - - goal_index = i; - previous_segment_angle = current_angle; - previous_segment_end_index = i; - previous_segment_angle_set = true; - } - - has_forward_direction = computeOutgoingAngle(plan, goal_index, desired_angle); - if (!has_forward_direction) - { - if (plan_tail_is_final_goal && goal_index == plan.size() - 1) - { - desired_angle = plan[goal_index].theta; - } - else if (previous_segment_angle_set) - { - desired_angle = previous_segment_angle; - has_forward_direction = true; - } - else if (last_valid_segment_angle_set) - { - desired_angle = last_valid_segment_angle; - has_forward_direction = true; - } - else - { - desired_angle = plan[goal_index].theta; - } - } - - if (plan_tail_is_final_goal && goal_index == plan.size() - 1) - { - has_forward_direction = false; - } - - return goal_index; -} + double current_angle = atan2(current_direction_y, current_direction_x); -bool PathProgressCritic::findNextArticulation(const std::vector& plan, unsigned int start_index, - unsigned int end_index, unsigned int last_valid_index, - unsigned int& articulation_index, double& articulation_yaw, - bool& has_forward_direction) const -{ - const double epsilon = 1e-9; - articulation_index = 0; - articulation_yaw = 0.0; - has_forward_direction = false; - - if (plan.size() < 2) - { - return false; - } - - unsigned int clamped_end = std::min(end_index, static_cast(plan.size() - 1)); - clamped_end = std::min(clamped_end, last_valid_index); - if (clamped_end < 1) - { - return false; - } - - unsigned int scan_start = std::max(start_index, 1u); - if (scan_start > clamped_end) - { - return false; - } - - double previous_segment_angle = 0.0; - bool previous_segment_angle_set = false; - unsigned int previous_segment_end_index = 0; - - for (unsigned int i = scan_start; i <= clamped_end; ++i) - { - double direction_x = plan[i].x - plan[i - 1].x; - double direction_y = plan[i].y - plan[i - 1].y; - double length = hypot(direction_x, direction_y); - if (length < epsilon) - { - continue; - } - - double current_angle = atan2(direction_y, direction_x); + // goal pose is found if plan doesn't point far enough away from the starting point + if (fabs(remainder(start_angle - current_angle, 2 * M_PI)) > angle_threshold_) + break; - if (previous_segment_angle_set) - { - double articulation_angle = fabs(angles::shortest_angular_distance(previous_segment_angle, current_angle)); - if (articulation_angle >= articulation_angle_threshold_) - { - articulation_index = previous_segment_end_index; - articulation_yaw = current_angle; - has_forward_direction = true; - if (!computeOutgoingAngle(plan, articulation_index, articulation_yaw)) - { - has_forward_direction = false; - if (articulation_index == plan.size() - 1) - { - articulation_yaw = plan[articulation_index].theta; - } - } - return true; + goal_index = i; } } - - previous_segment_angle = current_angle; - previous_segment_angle_set = true; - previous_segment_end_index = i; - } - - return false; -} - -bool PathProgressCritic::computeOutgoingAngle(const std::vector& plan, unsigned int index, - double& angle) const -{ - const double epsilon = 1e-9; - if (plan.empty() || index >= plan.size()) - { - return false; - } - - for (unsigned int i = index + 1; i < plan.size(); ++i) - { - double dx = plan[i].x - plan[index].x; - double dy = plan[i].y - plan[index].y; - double length = hypot(dx, dy); - if (length >= epsilon) - { - angle = atan2(dy, dx); - return true; - } } - - return false; -} - -bool PathProgressCritic::isPoseReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, - double goal_yaw, double xy_tolerance, double yaw_tolerance) const -{ - double distance = nav_2d_utils::poseDistance(goal_pose, robot_pose); - double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, goal_yaw)); - - return distance < xy_tolerance && yaw_error < yaw_tolerance; -} - -bool PathProgressCritic::isGoalReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, - double goal_yaw) const -{ - return isPoseReached(robot_pose, goal_pose, goal_yaw, xy_local_goal_tolerance_, yaw_local_goal_tolerance_); + return goal_index; } } // namespace mir_dwb_critics diff --git a/mir_navigation/config/dwb_local_planner_params.yaml b/mir_navigation/config/dwb_local_planner_params.yaml index d59aacb5..0596392a 100644 --- a/mir_navigation/config/dwb_local_planner_params.yaml +++ b/mir_navigation/config/dwb_local_planner_params.yaml @@ -49,7 +49,7 @@ DWBLocalPlanner: # Critics (trajectory scoring) #default_critic_namespaces: [dwb_critics, mir_dwb_critics] - critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathProgress] + critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathFollower] RotateToGoal: scale: 100.0 # lookahead_time: -1.0 @@ -65,12 +65,20 @@ DWBLocalPlanner: PathDistPruned: scale: 32.0 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan class: 'mir_dwb_critics::PathDistPruned' - PathProgress: + PathFollower: scale: 24.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal heading_scale: 0.1 - class: 'mir_dwb_critics::PathProgress' + class: 'mir_dwb_critics::PathFollower' xy_local_goal_tolerance: 0.20 + yaw_local_goal_tolerance: 0.15 angle_threshold: 0.78539816 # 45 degrees + articulation_angle_threshold: 1.3089969389957472 # 75 degrees + enforce_forward_dot: true + always_target_articulations: true + plan_alignment_position_tolerance: 0.15 + plan_alignment_yaw_tolerance: 3.14159265358979323846 + xy_goal_tolerance: 0.20 + yaw_goal_tolerance: 0.15 # Prune already passed poses from plan From 5d9dde273383e278b1c3748f416c5ec4621e4622 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Mon, 13 Oct 2025 17:00:56 +0200 Subject: [PATCH 30/45] Register PathFollowerCritic alias --- mir_dwb_critics/default_critics.xml | 4 ++++ mir_navigation/config/dwb_local_planner_params.yaml | 6 +++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/mir_dwb_critics/default_critics.xml b/mir_dwb_critics/default_critics.xml index 8c561131..1963f667 100644 --- a/mir_dwb_critics/default_critics.xml +++ b/mir_dwb_critics/default_critics.xml @@ -8,6 +8,10 @@ base_class_type="dwb_local_planner::TrajectoryCritic"> Maintains advanced path following behavior with articulation handling and pose alignment logic. + + Compatibility alias for the PathFollower critic. + Scores trajectories based on the difference between the path's current angle and the trajectory's angle diff --git a/mir_navigation/config/dwb_local_planner_params.yaml b/mir_navigation/config/dwb_local_planner_params.yaml index 0596392a..5aa30f1e 100644 --- a/mir_navigation/config/dwb_local_planner_params.yaml +++ b/mir_navigation/config/dwb_local_planner_params.yaml @@ -49,7 +49,7 @@ DWBLocalPlanner: # Critics (trajectory scoring) #default_critic_namespaces: [dwb_critics, mir_dwb_critics] - critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathFollower] + critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathFollowerCritic] RotateToGoal: scale: 100.0 # lookahead_time: -1.0 @@ -65,10 +65,10 @@ DWBLocalPlanner: PathDistPruned: scale: 32.0 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan class: 'mir_dwb_critics::PathDistPruned' - PathFollower: + PathFollowerCritic: scale: 24.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal heading_scale: 0.1 - class: 'mir_dwb_critics::PathFollower' + class: 'mir_dwb_critics::PathFollowerCritic' xy_local_goal_tolerance: 0.20 yaw_local_goal_tolerance: 0.15 angle_threshold: 0.78539816 # 45 degrees From 79d117acdb138161a79fe453ea7eba8518c4c8eb Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Mon, 13 Oct 2025 23:33:54 +0200 Subject: [PATCH 31/45] Add non-namespaced PathFollowerCritic alias --- mir_dwb_critics/default_critics.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mir_dwb_critics/default_critics.xml b/mir_dwb_critics/default_critics.xml index 1963f667..f0669a8f 100644 --- a/mir_dwb_critics/default_critics.xml +++ b/mir_dwb_critics/default_critics.xml @@ -12,6 +12,10 @@ base_class_type="dwb_local_planner::TrajectoryCritic"> Compatibility alias for the PathFollower critic. + + Compatibility alias for legacy configurations referencing PathFollowerCritic without a namespace. + Scores trajectories based on the difference between the path's current angle and the trajectory's angle From dbc22d3ec6aba224be4b84e8cfc8cd20a0f7f12a Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Mon, 13 Oct 2025 23:49:24 +0200 Subject: [PATCH 32/45] use transformed_global_plan instead of SBPLLatticePlanner/plan for global plan visualization as it is the one the critic receives --- mir_navigation/launch/move_base.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mir_navigation/launch/move_base.xml b/mir_navigation/launch/move_base.xml index fbc9fe4a..b81e4f0b 100644 --- a/mir_navigation/launch/move_base.xml +++ b/mir_navigation/launch/move_base.xml @@ -24,7 +24,7 @@ - + From 0b6ce91da97a0634f8b2ef647ed102819e3e9305 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Mon, 13 Oct 2025 23:51:53 +0200 Subject: [PATCH 33/45] Add PathFollower alias and update config --- mir_dwb_critics/default_critics.xml | 4 ++++ mir_navigation/config/dwb_local_planner_params.yaml | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/mir_dwb_critics/default_critics.xml b/mir_dwb_critics/default_critics.xml index f0669a8f..fd48c87a 100644 --- a/mir_dwb_critics/default_critics.xml +++ b/mir_dwb_critics/default_critics.xml @@ -12,6 +12,10 @@ base_class_type="dwb_local_planner::TrajectoryCritic"> Compatibility alias for the PathFollower critic. + + Compatibility alias for legacy configurations referencing PathFollower without a namespace. + Compatibility alias for legacy configurations referencing PathFollowerCritic without a namespace. diff --git a/mir_navigation/config/dwb_local_planner_params.yaml b/mir_navigation/config/dwb_local_planner_params.yaml index 5aa30f1e..76473cd8 100644 --- a/mir_navigation/config/dwb_local_planner_params.yaml +++ b/mir_navigation/config/dwb_local_planner_params.yaml @@ -49,7 +49,7 @@ DWBLocalPlanner: # Critics (trajectory scoring) #default_critic_namespaces: [dwb_critics, mir_dwb_critics] - critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathFollowerCritic] + critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathFollower] RotateToGoal: scale: 100.0 # lookahead_time: -1.0 @@ -65,7 +65,7 @@ DWBLocalPlanner: PathDistPruned: scale: 32.0 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan class: 'mir_dwb_critics::PathDistPruned' - PathFollowerCritic: + PathFollower: scale: 24.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal heading_scale: 0.1 class: 'mir_dwb_critics::PathFollowerCritic' From d9820e93a90e8498797bc1dfbd92ffd0fc20efb6 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Tue, 14 Oct 2025 00:03:05 +0200 Subject: [PATCH 34/45] Remove PathFollower critic aliases --- mir_dwb_critics/default_critics.xml | 12 ------------ mir_navigation/config/dwb_local_planner_params.yaml | 2 +- 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/mir_dwb_critics/default_critics.xml b/mir_dwb_critics/default_critics.xml index fd48c87a..8c561131 100644 --- a/mir_dwb_critics/default_critics.xml +++ b/mir_dwb_critics/default_critics.xml @@ -8,18 +8,6 @@ base_class_type="dwb_local_planner::TrajectoryCritic"> Maintains advanced path following behavior with articulation handling and pose alignment logic. - - Compatibility alias for the PathFollower critic. - - - Compatibility alias for legacy configurations referencing PathFollower without a namespace. - - - Compatibility alias for legacy configurations referencing PathFollowerCritic without a namespace. - Scores trajectories based on the difference between the path's current angle and the trajectory's angle diff --git a/mir_navigation/config/dwb_local_planner_params.yaml b/mir_navigation/config/dwb_local_planner_params.yaml index 76473cd8..0596392a 100644 --- a/mir_navigation/config/dwb_local_planner_params.yaml +++ b/mir_navigation/config/dwb_local_planner_params.yaml @@ -68,7 +68,7 @@ DWBLocalPlanner: PathFollower: scale: 24.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal heading_scale: 0.1 - class: 'mir_dwb_critics::PathFollowerCritic' + class: 'mir_dwb_critics::PathFollower' xy_local_goal_tolerance: 0.20 yaw_local_goal_tolerance: 0.15 angle_threshold: 0.78539816 # 45 degrees From 7c13e90cea8d2e06ffabe2bf678fa629ac3c9445 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Tue, 14 Oct 2025 00:24:15 +0200 Subject: [PATCH 35/45] Align PathFollowerCritic plugin name --- mir_dwb_critics/default_critics.xml | 2 +- mir_navigation/config/dwb_local_planner_params.yaml | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/mir_dwb_critics/default_critics.xml b/mir_dwb_critics/default_critics.xml index 8c561131..f3a7a2d5 100644 --- a/mir_dwb_critics/default_critics.xml +++ b/mir_dwb_critics/default_critics.xml @@ -4,7 +4,7 @@ base_class_type="dwb_local_planner::TrajectoryCritic"> Scores trajectories based on how far along the global path they end up. - Maintains advanced path following behavior with articulation handling and pose alignment logic. diff --git a/mir_navigation/config/dwb_local_planner_params.yaml b/mir_navigation/config/dwb_local_planner_params.yaml index 0596392a..5aa30f1e 100644 --- a/mir_navigation/config/dwb_local_planner_params.yaml +++ b/mir_navigation/config/dwb_local_planner_params.yaml @@ -49,7 +49,7 @@ DWBLocalPlanner: # Critics (trajectory scoring) #default_critic_namespaces: [dwb_critics, mir_dwb_critics] - critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathFollower] + critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathFollowerCritic] RotateToGoal: scale: 100.0 # lookahead_time: -1.0 @@ -65,10 +65,10 @@ DWBLocalPlanner: PathDistPruned: scale: 32.0 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan class: 'mir_dwb_critics::PathDistPruned' - PathFollower: + PathFollowerCritic: scale: 24.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal heading_scale: 0.1 - class: 'mir_dwb_critics::PathFollower' + class: 'mir_dwb_critics::PathFollowerCritic' xy_local_goal_tolerance: 0.20 yaw_local_goal_tolerance: 0.15 angle_threshold: 0.78539816 # 45 degrees From 5a0591bfd4a2d232505c48da75cb9df65a574d4d Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Mon, 13 Oct 2025 22:40:04 +0000 Subject: [PATCH 36/45] tune dwb params --- mir_navigation/config/dwb_local_planner_params.yaml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/mir_navigation/config/dwb_local_planner_params.yaml b/mir_navigation/config/dwb_local_planner_params.yaml index 5aa30f1e..cd24cb41 100644 --- a/mir_navigation/config/dwb_local_planner_params.yaml +++ b/mir_navigation/config/dwb_local_planner_params.yaml @@ -4,7 +4,7 @@ LocalPlannerAdapter: DWBLocalPlanner: # Robot configuration max_vel_x: 0.8 - min_vel_x: -0.2 + min_vel_x: -0.8 # NOTE: no preference over going forward max_vel_y: 0.0 # diff drive robot min_vel_y: 0.0 # diff drive robot @@ -49,9 +49,10 @@ DWBLocalPlanner: # Critics (trajectory scoring) #default_critic_namespaces: [dwb_critics, mir_dwb_critics] - critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathFollowerCritic] + # critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathFollowerCritic] + critics: [PathFollowerCritic] RotateToGoal: - scale: 100.0 + scale: 10.0 # lookahead_time: -1.0 # slowing_factor: 5.0 ObstacleFootprint: @@ -68,10 +69,10 @@ DWBLocalPlanner: PathFollowerCritic: scale: 24.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal heading_scale: 0.1 + xy_local_goal_tolerance: 0.1 class: 'mir_dwb_critics::PathFollowerCritic' - xy_local_goal_tolerance: 0.20 yaw_local_goal_tolerance: 0.15 - angle_threshold: 0.78539816 # 45 degrees + angle_threshold: 0.3 # 90 degrees, default: 0.78539816 -> 45 degrees articulation_angle_threshold: 1.3089969389957472 # 75 degrees enforce_forward_dot: true always_target_articulations: true From 33c5335109d3d3747f9171f6cfe2908fe893f7f1 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Tue, 14 Oct 2025 00:52:26 +0200 Subject: [PATCH 37/45] Use plan orientation when computing outgoing angle --- mir_dwb_critics/src/path_follower.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/mir_dwb_critics/src/path_follower.cpp b/mir_dwb_critics/src/path_follower.cpp index c4388068..ef1e32c1 100644 --- a/mir_dwb_critics/src/path_follower.cpp +++ b/mir_dwb_critics/src/path_follower.cpp @@ -1304,6 +1304,12 @@ bool PathFollowerCritic::computeOutgoingAngle(const std::vector Date: Tue, 14 Oct 2025 01:21:42 +0200 Subject: [PATCH 38/45] Use plan orientation for intermediate goals --- mir_dwb_critics/src/path_follower.cpp | 46 +++++++++++++++++++-------- 1 file changed, 32 insertions(+), 14 deletions(-) diff --git a/mir_dwb_critics/src/path_follower.cpp b/mir_dwb_critics/src/path_follower.cpp index ef1e32c1..c4d57ebc 100644 --- a/mir_dwb_critics/src/path_follower.cpp +++ b/mir_dwb_critics/src/path_follower.cpp @@ -180,6 +180,25 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co std::vector plan = nav_2d_utils::adjustPlanResolution(global_plan, info.resolution).poses; + auto nearestPlanOrientation = [&](const geometry_msgs::Pose2D& query_pose) { + double best_distance = std::numeric_limits::infinity(); + double selected_orientation = query_pose.theta; + + for (const auto& pose : global_plan.poses) + { + double dx = pose.x - query_pose.x; + double dy = pose.y - query_pose.y; + double distance = hypot(dx, dy); + if (distance < best_distance) + { + best_distance = distance; + selected_orientation = pose.theta; + } + } + + return selected_orientation; + }; + auto posesDiffer = [&](const geometry_msgs::Pose2D& a, const geometry_msgs::Pose2D& b, double position_tolerance, double yaw_tolerance) { double dx = a.x - b.x; @@ -305,6 +324,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co geometry_msgs::Pose2D preserved_pose = reached_pose; preserved_pose.x = plan[matched_index].x; preserved_pose.y = plan[matched_index].y; + preserved_pose.theta = nearestPlanOrientation(preserved_pose); preserved_reached.emplace_back(matched_index, preserved_pose); state_preserved = true; progress_match_found = true; @@ -321,6 +341,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co held_goal_index_ = held_match_index; held_goal_pose_.x = plan[held_match_index].x; held_goal_pose_.y = plan[held_match_index].y; + held_goal_pose_.theta = nearestPlanOrientation(held_goal_pose_); } else { @@ -337,10 +358,10 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co { state_preserved = true; - double match_goal_yaw = plan[robot_match_index].theta; + double match_goal_yaw = nearestPlanOrientation(plan[robot_match_index]); if (!computeOutgoingAngle(plan, robot_match_index, match_goal_yaw)) { - match_goal_yaw = plan[robot_match_index].theta; + match_goal_yaw = nearestPlanOrientation(plan[robot_match_index]); } bool match_is_final_index = ((robot_match_index + 1u) >= plan.size()) && final_goal_in_plan_window; @@ -795,7 +816,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co { last_progress_index_ = std::max(last_progress_index_, idx); geometry_msgs::Pose2D reached_pose = plan[idx]; - reached_pose.theta = articulation_goal_yaw; + reached_pose.theta = nearestPlanOrientation(reached_pose); if (reached_intermediate_goals_.empty() || nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > @@ -915,7 +936,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co { last_progress_index_ = std::max(last_progress_index_, candidate_index); geometry_msgs::Pose2D reached_pose = plan[candidate_index]; - reached_pose.theta = candidate_yaw; + reached_pose.theta = nearestPlanOrientation(reached_pose); if (reached_intermediate_goals_.empty() || nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > 1e-6) @@ -1081,7 +1102,10 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co ROS_ASSERT(goal_index <= last_valid_index); worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y); - desired_angle = goal_yaw; + geometry_msgs::Pose2D goal_pose = plan[goal_index]; + double goal_plan_yaw = nearestPlanOrientation(goal_pose); + desired_angle = goal_plan_yaw; + goal_yaw = goal_plan_yaw; bool same_as_held = false; if (holding_goal_) @@ -1096,15 +1120,15 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co if (!same_as_held) { - held_goal_pose_ = plan[goal_index]; - held_goal_pose_.theta = goal_yaw; + held_goal_pose_ = goal_pose; + held_goal_pose_.theta = goal_plan_yaw; held_goal_index_ = goal_index; } else { held_goal_pose_.x = plan[goal_index].x; held_goal_pose_.y = plan[goal_index].y; - held_goal_pose_.theta = goal_yaw; + held_goal_pose_.theta = goal_plan_yaw; } holding_goal_ = true; @@ -1304,12 +1328,6 @@ bool PathFollowerCritic::computeOutgoingAngle(const std::vector Date: Tue, 14 Oct 2025 08:34:46 +0200 Subject: [PATCH 39/45] Use transformed plan yaw and add spacing control --- .../include/mir_dwb_critics/path_follower.h | 4 +- mir_dwb_critics/src/path_follower.cpp | 110 +++++++----------- 2 files changed, 44 insertions(+), 70 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_follower.h b/mir_dwb_critics/include/mir_dwb_critics/path_follower.h index 5c8fe659..d744a328 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_follower.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_follower.h @@ -77,8 +77,7 @@ class PathFollowerCritic : public dwb_critics::MapGridCritic unsigned int end_index, unsigned int last_valid_index, unsigned int& articulation_index, double& articulation_yaw, bool& has_forward_direction) const; - bool computeOutgoingAngle(const std::vector& plan, unsigned int index, - double& angle) const; + bool hasForwardProgress(const std::vector& plan, unsigned int index) const; bool isPoseReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, double goal_yaw, double xy_tolerance, double yaw_tolerance) const; @@ -95,6 +94,7 @@ class PathFollowerCritic : public dwb_critics::MapGridCritic double heading_scale_; bool enforce_forward_dot_; bool always_target_articulations_; + double intermediate_goal_spacing_; bool initial_alignment_done_; // Plan change detection to reset internal state on new global plans diff --git a/mir_dwb_critics/src/path_follower.cpp b/mir_dwb_critics/src/path_follower.cpp index c4d57ebc..265a5a83 100644 --- a/mir_dwb_critics/src/path_follower.cpp +++ b/mir_dwb_critics/src/path_follower.cpp @@ -88,6 +88,14 @@ void PathFollowerCritic::onInit() critic_nh_.param("heading_scale", heading_scale_, 1.0); critic_nh_.param("enforce_forward_dot", enforce_forward_dot_, true); critic_nh_.param("always_target_articulations", always_target_articulations_, true); + critic_nh_.param("intermediate_goal_spacing", intermediate_goal_spacing_, 0.5); + if (intermediate_goal_spacing_ < 0.0) + { + ROS_WARN_NAMED("PathFollowerCritic", + "Parameter intermediate_goal_spacing (%.3f) is negative. Clamping to 0.0 to disable spacing limit.", + intermediate_goal_spacing_); + intermediate_goal_spacing_ = 0.0; + } initial_alignment_done_ = false; intermediate_goal_pub_ = critic_nh_.advertise("intermediate_goal", 1); @@ -199,6 +207,11 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co return selected_orientation; }; + for (auto& pose : plan) + { + pose.theta = nearestPlanOrientation(pose); + } + auto posesDiffer = [&](const geometry_msgs::Pose2D& a, const geometry_msgs::Pose2D& b, double position_tolerance, double yaw_tolerance) { double dx = a.x - b.x; @@ -359,10 +372,6 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co state_preserved = true; double match_goal_yaw = nearestPlanOrientation(plan[robot_match_index]); - if (!computeOutgoingAngle(plan, robot_match_index, match_goal_yaw)) - { - match_goal_yaw = nearestPlanOrientation(plan[robot_match_index]); - } bool match_is_final_index = ((robot_match_index + 1u) >= plan.size()) && final_goal_in_plan_window; double match_yaw_tolerance = match_is_final_index ? final_goal_yaw_tolerance_ : yaw_local_goal_tolerance_; @@ -654,15 +663,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co if (!initial_alignment_done_) { - double desired_initial_yaw = plan.front().theta; - if (!plan.empty()) - { - double outgoing_angle = desired_initial_yaw; - if (computeOutgoingAngle(plan, 0u, outgoing_angle)) - { - desired_initial_yaw = outgoing_angle; - } - } + double desired_initial_yaw = nearestPlanOrientation(robot_pose); double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, desired_initial_yaw)); if (yaw_error >= yaw_local_goal_tolerance_) @@ -781,7 +782,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co }; unsigned int goal_index = search_start_index; - double goal_yaw = plan[goal_index].theta; + double goal_yaw = nearestPlanOrientation(plan[goal_index]); bool has_forward_direction = false; bool found_goal = false; bool forced_skipped_articulation = false; @@ -802,9 +803,9 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co continue; } - double articulation_yaw = plan[idx].theta; - bool articulation_has_forward = computeOutgoingAngle(plan, idx, articulation_yaw); - double articulation_goal_yaw = articulation_has_forward ? articulation_yaw : plan[idx].theta; + double articulation_yaw = nearestPlanOrientation(plan[idx]); + bool articulation_has_forward = hasForwardProgress(plan, idx); + double articulation_goal_yaw = articulation_yaw; double articulation_xy_tolerance = (final_goal_in_plan_window && idx == plan_last_index) ? final_goal_xy_tolerance_ : xy_local_goal_tolerance_; @@ -851,11 +852,8 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co if (articulation_it != articulation_indices.end()) { goal_index = *articulation_it; - has_forward_direction = computeOutgoingAngle(plan, goal_index, goal_yaw); - if (!has_forward_direction) - { - goal_yaw = plan[goal_index].theta; - } + has_forward_direction = hasForwardProgress(plan, goal_index); + goal_yaw = nearestPlanOrientation(plan[goal_index]); if (enforce_forward_dot_ && has_forward_direction) { @@ -993,7 +991,8 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co for (; goal_index <= last_valid_index; ++goal_index) { - has_forward_direction = computeOutgoingAngle(plan, goal_index, goal_yaw); + goal_yaw = nearestPlanOrientation(plan[goal_index]); + has_forward_direction = hasForwardProgress(plan, goal_index); if (has_forward_direction) { if (!enforce_forward_dot_) @@ -1013,7 +1012,6 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co continue; } - goal_yaw = plan[goal_index].theta; if (!enforce_forward_dot_ || goal_index == last_valid_index) { selected_fallback = true; @@ -1068,7 +1066,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co "Snapping intermediate goal index %u to final goal because distance %.3f <= threshold %.3f", goal_index, distance_to_final, snap_threshold); goal_index = plan_last_index; - goal_yaw = plan[plan_last_index].theta; + goal_yaw = nearestPlanOrientation(plan[plan_last_index]); has_forward_direction = false; forced_skipped_articulation = false; has_next_articulation = false; @@ -1094,7 +1092,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co if (final_distance <= final_goal_xy_tolerance_) { goal_index = plan_last_index; - goal_yaw = plan[plan_last_index].theta; + goal_yaw = nearestPlanOrientation(plan[plan_last_index]); has_forward_direction = false; } } @@ -1154,13 +1152,17 @@ unsigned int PathFollowerCritic::getGoalIndex(const std::vector(plan.size() - 1)); unsigned int clamped_last = std::min(last_valid_index, static_cast(plan.size() - 1)); + double max_spacing = intermediate_goal_spacing_; + bool spacing_limit_enabled = max_spacing > 0.0; + double accumulated_distance = 0.0; if (clamped_start >= clamped_last) { - has_forward_direction = computeOutgoingAngle(plan, clamped_start, desired_angle); - if (!has_forward_direction) + desired_angle = plan[clamped_start].theta; + has_forward_direction = hasForwardProgress(plan, clamped_start); + if (plan_tail_is_final_goal && clamped_start == plan.size() - 1) { - desired_angle = plan[clamped_start].theta; + has_forward_direction = false; } return clamped_start; } @@ -1171,8 +1173,6 @@ unsigned int PathFollowerCritic::getGoalIndex(const std::vector max_spacing) + { + break; + } + double current_angle = atan2(direction_y, direction_x); - last_valid_segment_angle = current_angle; - last_valid_segment_angle_set = true; if (!base_angle_set) { @@ -1216,28 +1220,8 @@ unsigned int PathFollowerCritic::getGoalIndex(const std::vector= articulation_angle_threshold_) { articulation_index = previous_segment_end_index; - articulation_yaw = current_angle; - has_forward_direction = true; - if (!computeOutgoingAngle(plan, articulation_index, articulation_yaw)) - { - has_forward_direction = false; - if (articulation_index == plan.size() - 1) - { - articulation_yaw = plan[articulation_index].theta; - } - } + articulation_yaw = plan[articulation_index].theta; + has_forward_direction = hasForwardProgress(plan, articulation_index); return true; } } @@ -1319,8 +1295,7 @@ bool PathFollowerCritic::findNextArticulation(const std::vector& plan, unsigned int index, - double& angle) const +bool PathFollowerCritic::hasForwardProgress(const std::vector& plan, unsigned int index) const { const double epsilon = 1e-9; if (plan.empty() || index >= plan.size()) @@ -1335,7 +1310,6 @@ bool PathFollowerCritic::computeOutgoingAngle(const std::vector= epsilon) { - angle = atan2(dy, dx); return true; } } From e8f2c9cd728000f43bfc8d9dce7e985666bb5e2b Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Tue, 14 Oct 2025 09:08:12 +0200 Subject: [PATCH 40/45] Honor plan orientation when choosing intermediate goals --- .../include/mir_dwb_critics/path_follower.h | 2 +- mir_dwb_critics/src/path_follower.cpp | 34 +++++++++++++------ 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_follower.h b/mir_dwb_critics/include/mir_dwb_critics/path_follower.h index d744a328..7371557c 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_follower.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_follower.h @@ -94,7 +94,7 @@ class PathFollowerCritic : public dwb_critics::MapGridCritic double heading_scale_; bool enforce_forward_dot_; bool always_target_articulations_; - double intermediate_goal_spacing_; + unsigned int intermediate_goal_spacing_; bool initial_alignment_done_; // Plan change detection to reset internal state on new global plans diff --git a/mir_dwb_critics/src/path_follower.cpp b/mir_dwb_critics/src/path_follower.cpp index 265a5a83..f937d80f 100644 --- a/mir_dwb_critics/src/path_follower.cpp +++ b/mir_dwb_critics/src/path_follower.cpp @@ -88,14 +88,16 @@ void PathFollowerCritic::onInit() critic_nh_.param("heading_scale", heading_scale_, 1.0); critic_nh_.param("enforce_forward_dot", enforce_forward_dot_, true); critic_nh_.param("always_target_articulations", always_target_articulations_, true); - critic_nh_.param("intermediate_goal_spacing", intermediate_goal_spacing_, 0.5); - if (intermediate_goal_spacing_ < 0.0) + int intermediate_goal_spacing_param = 0; + critic_nh_.param("intermediate_goal_spacing", intermediate_goal_spacing_param, 0); + if (intermediate_goal_spacing_param < 0) { ROS_WARN_NAMED("PathFollowerCritic", - "Parameter intermediate_goal_spacing (%.3f) is negative. Clamping to 0.0 to disable spacing limit.", - intermediate_goal_spacing_); - intermediate_goal_spacing_ = 0.0; + "Parameter intermediate_goal_spacing (%d) is negative. Clamping to 0 to disable spacing limit.", + intermediate_goal_spacing_param); + intermediate_goal_spacing_param = 0; } + intermediate_goal_spacing_ = static_cast(intermediate_goal_spacing_param); initial_alignment_done_ = false; intermediate_goal_pub_ = critic_nh_.advertise("intermediate_goal", 1); @@ -1152,9 +1154,9 @@ unsigned int PathFollowerCritic::getGoalIndex(const std::vector(plan.size() - 1)); unsigned int clamped_last = std::min(last_valid_index, static_cast(plan.size() - 1)); - double max_spacing = intermediate_goal_spacing_; - bool spacing_limit_enabled = max_spacing > 0.0; - double accumulated_distance = 0.0; + unsigned int max_spacing = intermediate_goal_spacing_; + bool spacing_limit_enabled = max_spacing > 0u; + const double orientation_progress_epsilon = 1e-3; if (clamped_start >= clamped_last) { @@ -1181,11 +1183,23 @@ unsigned int PathFollowerCritic::getGoalIndex(const std::vector orientation_progress_epsilon) + { + goal_index = i; + break; + } + + if (spacing_limit_enabled && (i - clamped_start) > max_spacing) + { + break; + } + continue; } - accumulated_distance += length; - if (spacing_limit_enabled && accumulated_distance > max_spacing) + if (spacing_limit_enabled && (i - clamped_start) > max_spacing) { break; } From cfe953b1d66957d59edb70ff008b427b03fed516 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Tue, 14 Oct 2025 11:01:36 +0200 Subject: [PATCH 41/45] Clamp intermediate goal spacing to articulation-aware window --- mir_dwb_critics/src/path_follower.cpp | 55 +++++++++++++++++++++------ 1 file changed, 43 insertions(+), 12 deletions(-) diff --git a/mir_dwb_critics/src/path_follower.cpp b/mir_dwb_critics/src/path_follower.cpp index f937d80f..98d24edc 100644 --- a/mir_dwb_critics/src/path_follower.cpp +++ b/mir_dwb_critics/src/path_follower.cpp @@ -1155,7 +1155,28 @@ unsigned int PathFollowerCritic::getGoalIndex(const std::vector(plan.size() - 1)); unsigned int clamped_last = std::min(last_valid_index, static_cast(plan.size() - 1)); unsigned int max_spacing = intermediate_goal_spacing_; - bool spacing_limit_enabled = max_spacing > 0u; + bool spacing_limit_enabled = true; + unsigned int loop_last = clamped_last; + if (max_spacing >= std::numeric_limits::max()) + { + spacing_limit_enabled = false; + } + else + { + unsigned int allowed_offset = max_spacing + 1u; + if (allowed_offset == 0u) + { + spacing_limit_enabled = false; + } + else if (clamped_start <= std::numeric_limits::max() - allowed_offset) + { + unsigned int spacing_limit_index = clamped_start + allowed_offset; + if (spacing_limit_index < loop_last) + { + loop_last = spacing_limit_index; + } + } + } const double orientation_progress_epsilon = 1e-3; if (clamped_start >= clamped_last) @@ -1169,6 +1190,19 @@ unsigned int PathFollowerCritic::getGoalIndex(const std::vector clamped_start) + { + loop_last = std::min(loop_last, articulation_index); + } + } + unsigned int goal_index = clamped_start; double base_angle = 0.0; bool base_angle_set = false; @@ -1176,7 +1210,8 @@ unsigned int PathFollowerCritic::getGoalIndex(const std::vector max_spacing) - { - break; - } - + goal_index = i; continue; } - if (spacing_limit_enabled && (i - clamped_start) > max_spacing) - { - break; - } - double current_angle = atan2(direction_y, direction_x); if (!base_angle_set) @@ -1234,6 +1260,11 @@ unsigned int PathFollowerCritic::getGoalIndex(const std::vector clamped_start) + { + goal_index = loop_end; + } + desired_angle = plan[goal_index].theta; has_forward_direction = hasForwardProgress(plan, goal_index); From a5a972792b5d64c7b04d459f36c7e413dba3039b Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Tue, 14 Oct 2025 21:56:46 +0200 Subject: [PATCH 42/45] Remove snap-to-final-goal logic in path follower critic --- mir_dwb_critics/src/path_follower.cpp | 22 ---------------------- 1 file changed, 22 deletions(-) diff --git a/mir_dwb_critics/src/path_follower.cpp b/mir_dwb_critics/src/path_follower.cpp index 98d24edc..389a8d52 100644 --- a/mir_dwb_critics/src/path_follower.cpp +++ b/mir_dwb_critics/src/path_follower.cpp @@ -1054,28 +1054,6 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } } - if (final_goal_in_plan_window && plan_last_index <= last_valid_index && goal_index != plan_last_index) - { - double snap_threshold = final_goal_xy_tolerance_; - if (snap_threshold >= 0.0) - { - double final_dx = plan[plan_last_index].x - plan[goal_index].x; - double final_dy = plan[plan_last_index].y - plan[goal_index].y; - double distance_to_final = hypot(final_dx, final_dy); - if (distance_to_final <= snap_threshold) - { - ROS_DEBUG_NAMED("PathFollowerCritic", - "Snapping intermediate goal index %u to final goal because distance %.3f <= threshold %.3f", - goal_index, distance_to_final, snap_threshold); - goal_index = plan_last_index; - goal_yaw = nearestPlanOrientation(plan[plan_last_index]); - has_forward_direction = false; - forced_skipped_articulation = false; - has_next_articulation = false; - } - } - } - bool pending_articulation = false; if (last_progress_index_ < goal_index) { From 4c1d473baefbcb03851ff1cf3d1b5a390945828b Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Thu, 16 Oct 2025 12:19:12 +0000 Subject: [PATCH 43/45] enable RotateToGoal critic, tune xy_goal_tolerance --- mir_navigation/config/dwb_local_planner_params.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mir_navigation/config/dwb_local_planner_params.yaml b/mir_navigation/config/dwb_local_planner_params.yaml index cd24cb41..60635a76 100644 --- a/mir_navigation/config/dwb_local_planner_params.yaml +++ b/mir_navigation/config/dwb_local_planner_params.yaml @@ -50,7 +50,7 @@ DWBLocalPlanner: # Critics (trajectory scoring) #default_critic_namespaces: [dwb_critics, mir_dwb_critics] # critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathFollowerCritic] - critics: [PathFollowerCritic] + critics: [RotateToGoal, PathFollowerCritic] RotateToGoal: scale: 10.0 # lookahead_time: -1.0 @@ -78,7 +78,7 @@ DWBLocalPlanner: always_target_articulations: true plan_alignment_position_tolerance: 0.15 plan_alignment_yaw_tolerance: 3.14159265358979323846 - xy_goal_tolerance: 0.20 + xy_goal_tolerance: 0.10 yaw_goal_tolerance: 0.15 From ad99596c38b73877e8d0a1ea07563f9650258c97 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Thu, 16 Oct 2025 15:38:12 +0200 Subject: [PATCH 44/45] manual cleanup --- mir_dwb_critics/default_critics.xml | 5 ++--- .../config/dwb_local_planner_params.yaml | 14 ++++++++++---- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/mir_dwb_critics/default_critics.xml b/mir_dwb_critics/default_critics.xml index f3a7a2d5..a9c782c1 100644 --- a/mir_dwb_critics/default_critics.xml +++ b/mir_dwb_critics/default_critics.xml @@ -1,12 +1,11 @@ - + Scores trajectories based on how far along the global path they end up. - Maintains advanced path following behavior with articulation handling and pose alignment logic. + Close path following behavior with articulation handling and pose alignment logic. Scores trajectories based on the difference between the path's current angle and the trajectory's angle diff --git a/mir_navigation/config/dwb_local_planner_params.yaml b/mir_navigation/config/dwb_local_planner_params.yaml index 60635a76..09754127 100644 --- a/mir_navigation/config/dwb_local_planner_params.yaml +++ b/mir_navigation/config/dwb_local_planner_params.yaml @@ -49,8 +49,8 @@ DWBLocalPlanner: # Critics (trajectory scoring) #default_critic_namespaces: [dwb_critics, mir_dwb_critics] - # critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathFollowerCritic] - critics: [RotateToGoal, PathFollowerCritic] + # critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathProgress] + critics: [RotateToGoal, PathFollower] RotateToGoal: scale: 10.0 # lookahead_time: -1.0 @@ -66,11 +66,17 @@ DWBLocalPlanner: PathDistPruned: scale: 32.0 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan class: 'mir_dwb_critics::PathDistPruned' - PathFollowerCritic: + PathProgress: + scale: 24.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal + heading_scale: 0.1 + class: 'mir_dwb_critics::PathProgress' + xy_local_goal_tolerance: 0.20 + angle_threshold: 0.78539816 # 45 degrees + PathFollower: scale: 24.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal heading_scale: 0.1 xy_local_goal_tolerance: 0.1 - class: 'mir_dwb_critics::PathFollowerCritic' + class: 'mir_dwb_critics::PathFollower' yaw_local_goal_tolerance: 0.15 angle_threshold: 0.3 # 90 degrees, default: 0.78539816 -> 45 degrees articulation_angle_threshold: 1.3089969389957472 # 75 degrees From f91054f026da7557e9164e66aed9c0368fbc1152 Mon Sep 17 00:00:00 2001 From: Oscar Lima Date: Mon, 20 Oct 2025 08:09:26 +0200 Subject: [PATCH 45/45] Fix formatting per pre-commit --- .../include/mir_dwb_critics/path_follower.h | 4 +- mir_dwb_critics/src/path_follower.cpp | 112 ++++++++---------- mir_navigation/nodes/global_plan_debug_viz.py | 28 +++++ 3 files changed, 81 insertions(+), 63 deletions(-) diff --git a/mir_dwb_critics/include/mir_dwb_critics/path_follower.h b/mir_dwb_critics/include/mir_dwb_critics/path_follower.h index 7371557c..e56c92a4 100644 --- a/mir_dwb_critics/include/mir_dwb_critics/path_follower.h +++ b/mir_dwb_critics/include/mir_dwb_critics/path_follower.h @@ -79,8 +79,8 @@ class PathFollowerCritic : public dwb_critics::MapGridCritic bool hasForwardProgress(const std::vector& plan, unsigned int index) const; - bool isPoseReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, - double goal_yaw, double xy_tolerance, double yaw_tolerance) const; + bool isPoseReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, double goal_yaw, + double xy_tolerance, double yaw_tolerance) const; bool isGoalReached(const geometry_msgs::Pose2D& robot_pose, const geometry_msgs::Pose2D& goal_pose, double goal_yaw) const; diff --git a/mir_dwb_critics/src/path_follower.cpp b/mir_dwb_critics/src/path_follower.cpp index 389a8d52..53241f0e 100644 --- a/mir_dwb_critics/src/path_follower.cpp +++ b/mir_dwb_critics/src/path_follower.cpp @@ -311,8 +311,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co }; if (!have_last_plan_ || global_plan.header.frame_id != last_plan_frame_id_ || headerStampChanged() || - headerSeqChanged() || - goalPoseChanged()) + headerSeqChanged() || goalPoseChanged()) { plan_changed = true; } @@ -399,9 +398,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co { std::sort(preserved_reached.begin(), preserved_reached.end(), [](const std::pair& lhs, - const std::pair& rhs) { - return lhs.first < rhs.first; - }); + const std::pair& rhs) { return lhs.first < rhs.first; }); reached_intermediate_goals_.clear(); reached_intermediate_goals_.reserve(preserved_reached.size()); for (const auto& entry : preserved_reached) @@ -442,7 +439,6 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co last_final_goal_pose_ = final_goal; have_last_goal_pose_ = true; - unsigned int plan_last_index = static_cast(plan.size() - 1); if (initial_alignment_done_ && plan.size() > 1 && last_progress_index_ > 0) @@ -471,7 +467,8 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co if (holding_goal_ && held_goal_index_ >= plan.size()) { - ROS_DEBUG_NAMED("PathFollowerCritic", "Held goal index %u is out of range for current plan of size %zu. Releasing hold.", + ROS_DEBUG_NAMED("PathFollowerCritic", + "Held goal index %u is out of range for current plan of size %zu. Releasing hold.", held_goal_index_, plan.size()); holding_goal_ = false; held_goal_index_ = 0; @@ -568,8 +565,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co double current_angle = atan2(direction_y, direction_x); if (previous_segment_angle_set) { - double articulation_angle = - fabs(angles::shortest_angular_distance(previous_segment_angle, current_angle)); + double articulation_angle = fabs(angles::shortest_angular_distance(previous_segment_angle, current_angle)); if (articulation_angle >= articulation_angle_threshold_) { if (articulation_indices.empty() || articulation_indices.back() != previous_segment_end_index) @@ -712,12 +708,12 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } else { - double held_xy_tolerance = - (final_goal_in_plan_window && held_goal_index_ == plan_last_index) ? final_goal_xy_tolerance_ : - xy_local_goal_tolerance_; - double held_yaw_tolerance = - (final_goal_in_plan_window && held_goal_index_ == plan_last_index) ? final_goal_yaw_tolerance_ : - yaw_local_goal_tolerance_; + double held_xy_tolerance = (final_goal_in_plan_window && held_goal_index_ == plan_last_index) ? + final_goal_xy_tolerance_ : + xy_local_goal_tolerance_; + double held_yaw_tolerance = (final_goal_in_plan_window && held_goal_index_ == plan_last_index) ? + final_goal_yaw_tolerance_ : + yaw_local_goal_tolerance_; double yaw_error = fabs(angles::shortest_angular_distance(robot_pose.theta, held_goal_pose_.theta)); if (yaw_error >= held_yaw_tolerance) @@ -737,8 +733,8 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co y = held_y; desired_angle = held_goal_pose_.theta; publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); - ROS_DEBUG_NAMED("PathFollowerCritic", - "Holding goal index %u until full pose tolerance satisfied (XY + yaw)", held_goal_index_); + ROS_DEBUG_NAMED("PathFollowerCritic", "Holding goal index %u until full pose tolerance satisfied (XY + yaw)", + held_goal_index_); return true; } @@ -750,9 +746,10 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co { reached_intermediate_goals_.push_back(reached_pose); } - ROS_DEBUG_NAMED("PathFollowerCritic", - "Reached held intermediate goal index %u while respecting pose tolerances. last_progress_index_: %u", - held_goal_index_, last_progress_index_); + ROS_DEBUG_NAMED( + "PathFollowerCritic", + "Reached held intermediate goal index %u while respecting pose tolerances. last_progress_index_: %u", + held_goal_index_, last_progress_index_); holding_goal_ = false; } } @@ -780,7 +777,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co } auto articulationSearchStart = [&](unsigned int candidate_start) { - return std::max({candidate_start, articulation_scan_start, 1u}); + return std::max({ candidate_start, articulation_scan_start, 1u }); }; unsigned int goal_index = search_start_index; @@ -822,8 +819,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co reached_pose.theta = nearestPlanOrientation(reached_pose); if (reached_intermediate_goals_.empty() || nav_2d_utils::poseDistance(reached_intermediate_goals_.back(), reached_pose) > 1e-6 || - fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > - 1e-6) + fabs(angles::shortest_angular_distance(reached_intermediate_goals_.back().theta, reached_pose.theta)) > 1e-6) { reached_intermediate_goals_.push_back(reached_pose); } @@ -846,10 +842,10 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co if (articulation_lower_bound <= articulation_upper_bound) { - auto articulation_it = std::find_if(articulation_indices.begin(), articulation_indices.end(), - [&](unsigned int index) { - return index >= articulation_lower_bound && index <= articulation_upper_bound; - }); + auto articulation_it = + std::find_if(articulation_indices.begin(), articulation_indices.end(), [&](unsigned int index) { + return index >= articulation_lower_bound && index <= articulation_upper_bound; + }); if (articulation_it != articulation_indices.end()) { @@ -873,8 +869,8 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co forced_skipped_articulation = true; found_goal = true; ROS_DEBUG_NAMED("PathFollowerCritic", - "Recovered skipped articulation index %u between progress %u and search start %u.", - goal_index, last_progress_index_, search_start_index); + "Recovered skipped articulation index %u between progress %u and search start %u.", goal_index, + last_progress_index_, search_start_index); } } } @@ -884,8 +880,8 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co { double candidate_yaw = goal_yaw; bool candidate_has_forward = false; - unsigned int candidate_index = - getGoalIndex(plan, search_index, last_valid_index, final_goal_in_plan_window, candidate_yaw, candidate_has_forward); + unsigned int candidate_index = getGoalIndex(plan, search_index, last_valid_index, final_goal_in_plan_window, + candidate_yaw, candidate_has_forward); bool forced_articulation = false; if (candidate_index > last_progress_index_) @@ -895,8 +891,8 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co bool articulation_has_forward = candidate_has_forward; unsigned int articulation_start_index = articulationSearchStart(search_index); if (articulation_start_index <= candidate_index && - findNextArticulation(plan, articulation_start_index, candidate_index, last_valid_index, - articulation_index, articulation_yaw, articulation_has_forward)) + findNextArticulation(plan, articulation_start_index, candidate_index, last_valid_index, articulation_index, + articulation_yaw, articulation_has_forward)) { candidate_index = articulation_index; candidate_yaw = articulation_yaw; @@ -918,21 +914,19 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co found_goal = true; forced_skipped_articulation = true; has_next_articulation = false; - ROS_DEBUG_NAMED("PathFollowerCritic", - "Selecting pending articulation index %u reached at candidate %u.", goal_index, - candidate_index); + ROS_DEBUG_NAMED("PathFollowerCritic", "Selecting pending articulation index %u reached at candidate %u.", + goal_index, candidate_index); break; } - double candidate_xy_tolerance = - (final_goal_in_plan_window && candidate_index == plan_last_index) ? final_goal_xy_tolerance_ : - xy_local_goal_tolerance_; - double candidate_yaw_tolerance = - (final_goal_in_plan_window && candidate_index == plan_last_index) ? final_goal_yaw_tolerance_ : - yaw_local_goal_tolerance_; + double candidate_xy_tolerance = (final_goal_in_plan_window && candidate_index == plan_last_index) ? + final_goal_xy_tolerance_ : + xy_local_goal_tolerance_; + double candidate_yaw_tolerance = (final_goal_in_plan_window && candidate_index == plan_last_index) ? + final_goal_yaw_tolerance_ : + yaw_local_goal_tolerance_; - if (isPoseReached(robot_pose, plan[candidate_index], candidate_yaw, candidate_xy_tolerance, - candidate_yaw_tolerance)) + if (isPoseReached(robot_pose, plan[candidate_index], candidate_yaw, candidate_xy_tolerance, candidate_yaw_tolerance)) { last_progress_index_ = std::max(last_progress_index_, candidate_index); geometry_msgs::Pose2D reached_pose = plan[candidate_index]; @@ -943,9 +937,8 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co { reached_intermediate_goals_.push_back(reached_pose); } - ROS_DEBUG_NAMED("PathFollowerCritic", - "Reached intermediate goal index %u. last_progress_index_: %u", candidate_index, - last_progress_index_); + ROS_DEBUG_NAMED("PathFollowerCritic", "Reached intermediate goal index %u. last_progress_index_: %u", + candidate_index, last_progress_index_); if (candidate_index >= last_valid_index) { @@ -967,8 +960,8 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co double dot = to_goal_x * std::cos(candidate_yaw) + to_goal_y * std::sin(candidate_yaw); if (dot < 0.0) { - ROS_DEBUG_NAMED("PathFollowerCritic", - "Skipping goal index %u due to backward alignment (dot product %.3f)", candidate_index, dot); + ROS_DEBUG_NAMED("PathFollowerCritic", "Skipping goal index %u due to backward alignment (dot product %.3f)", + candidate_index, dot); if (candidate_index >= last_valid_index) { break; @@ -1033,8 +1026,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co has_forward_direction = next_articulation_has_forward; has_next_articulation = false; forced_skipped_articulation = true; - ROS_DEBUG_NAMED("PathFollowerCritic", - "Fallback switching to pending articulation index %u.", goal_index); + ROS_DEBUG_NAMED("PathFollowerCritic", "Fallback switching to pending articulation index %u.", goal_index); } if (goal_index > last_progress_index_) @@ -1044,8 +1036,8 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co bool articulation_has_forward = has_forward_direction; unsigned int articulation_start_index = articulationSearchStart(search_start_index); if (articulation_start_index <= goal_index && - findNextArticulation(plan, articulation_start_index, goal_index, last_valid_index, - articulation_index, articulation_yaw, articulation_has_forward)) + findNextArticulation(plan, articulation_start_index, goal_index, last_valid_index, articulation_index, + articulation_yaw, articulation_has_forward)) { goal_index = articulation_index; goal_yaw = articulation_yaw; @@ -1057,8 +1049,8 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co bool pending_articulation = false; if (last_progress_index_ < goal_index) { - pending_articulation = std::find(articulation_indices.begin(), articulation_indices.end(), goal_index) != - articulation_indices.end(); + pending_articulation = + std::find(articulation_indices.begin(), articulation_indices.end(), goal_index) != articulation_indices.end(); } // Only consider snapping to the final goal yaw when we have already selected the last path index as goal. @@ -1092,8 +1084,7 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co double position_diff_y = plan[goal_index].y - held_goal_pose_.y; double yaw_diff = angles::shortest_angular_distance(goal_yaw, held_goal_pose_.theta); same_as_held = (fabs(position_diff_x) <= hold_position_epsilon_) && - (fabs(position_diff_y) <= hold_position_epsilon_) && - (fabs(yaw_diff) <= hold_yaw_epsilon_); + (fabs(position_diff_y) <= hold_position_epsilon_) && (fabs(yaw_diff) <= hold_yaw_epsilon_); } if (!same_as_held) @@ -1111,8 +1102,8 @@ bool PathFollowerCritic::getGoalPose(const geometry_msgs::Pose2D& robot_pose, co holding_goal_ = true; ROS_DEBUG_NAMED("PathFollowerCritic", - "Selected goal index %u (x: %.3f, y: %.3f, yaw: %.3f rad). last_progress_index_: %u", - goal_index, plan[goal_index].x, plan[goal_index].y, goal_yaw, last_progress_index_); + "Selected goal index %u (x: %.3f, y: %.3f, yaw: %.3f rad). last_progress_index_: %u", goal_index, + plan[goal_index].x, plan[goal_index].y, goal_yaw, last_progress_index_); publishIntermediateGoal(held_goal_pose_, held_goal_pose_.theta); return true; @@ -1196,8 +1187,7 @@ unsigned int PathFollowerCritic::getGoalIndex(const std::vector orientation_progress_epsilon) { goal_index = i; diff --git a/mir_navigation/nodes/global_plan_debug_viz.py b/mir_navigation/nodes/global_plan_debug_viz.py index d048521c..756ec4ff 100755 --- a/mir_navigation/nodes/global_plan_debug_viz.py +++ b/mir_navigation/nodes/global_plan_debug_viz.py @@ -1,5 +1,33 @@ #!/usr/bin/env python3 +# Copyright (c) 2018-2022, Martin Günther (DFKI GmbH) and contributors +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + import rospy from geometry_msgs.msg import PoseArray from nav_msgs.msg import Path