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..a9c782c1 100644
--- a/mir_dwb_critics/default_critics.xml
+++ b/mir_dwb_critics/default_critics.xml
@@ -3,6 +3,10 @@
Scores trajectories based on how far along the global path they end up.
+
+ 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_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..e56c92a4
--- /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 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 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_;
+ unsigned int intermediate_goal_spacing_;
+ 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/src/path_follower.cpp b/mir_dwb_critics/src/path_follower.cpp
new file mode 100644
index 00000000..53241f0e
--- /dev/null
+++ b/mir_dwb_critics/src/path_follower.cpp
@@ -0,0 +1,1350 @@
+/*
+ * 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);
+ 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 (%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);
+ 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 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;
+ };
+
+ 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;
+ 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_pose.theta = nearestPlanOrientation(preserved_pose);
+ 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;
+ held_goal_pose_.theta = nearestPlanOrientation(held_goal_pose_);
+ }
+ 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 = 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_;
+ 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 = nearestPlanOrientation(robot_pose);
+
+ 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 = nearestPlanOrientation(plan[goal_index]);
+ 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 = 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_;
+ 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 = 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)
+ {
+ 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 = hasForwardProgress(plan, goal_index);
+ goal_yaw = nearestPlanOrientation(plan[goal_index]);
+
+ 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 = 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)
+ {
+ 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)
+ {
+ goal_yaw = nearestPlanOrientation(plan[goal_index]);
+ has_forward_direction = hasForwardProgress(plan, goal_index);
+ 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;
+ }
+
+ 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;
+ }
+ }
+ }
+
+ 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 = nearestPlanOrientation(plan[plan_last_index]);
+ has_forward_direction = false;
+ }
+ }
+
+ ROS_ASSERT(goal_index <= last_valid_index);
+
+ worldToGridBounded(info, plan[goal_index].x, plan[goal_index].y, x, y);
+ 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_)
+ {
+ 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_ = 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_plan_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));
+ unsigned int max_spacing = intermediate_goal_spacing_;
+ 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)
+ {
+ desired_angle = plan[clamped_start].theta;
+ has_forward_direction = hasForwardProgress(plan, clamped_start);
+ if (plan_tail_is_final_goal && clamped_start == plan.size() - 1)
+ {
+ has_forward_direction = false;
+ }
+ return clamped_start;
+ }
+
+ if (spacing_limit_enabled)
+ {
+ unsigned int articulation_index = 0u;
+ double articulation_yaw = 0.0;
+ bool articulation_has_forward = false;
+ if (findNextArticulation(plan, clamped_start, clamped_last, last_valid_index, articulation_index, articulation_yaw,
+ articulation_has_forward) &&
+ articulation_index > 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;
+ double previous_segment_angle = 0.0;
+ bool previous_segment_angle_set = false;
+ unsigned int previous_segment_end_index = clamped_start;
+
+ unsigned int loop_end = spacing_limit_enabled ? std::min(loop_last, clamped_last) : clamped_last;
+ for (unsigned int i = clamped_start + 1; i <= loop_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)
+ {
+ double orientation_delta = fabs(angles::shortest_angular_distance(plan[goal_index].theta, plan[i].theta));
+ if (orientation_delta > orientation_progress_epsilon)
+ {
+ goal_index = i;
+ break;
+ }
+
+ goal_index = i;
+ continue;
+ }
+
+ double current_angle = atan2(direction_y, direction_x);
+
+ 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;
+ }
+
+ if (goal_index == clamped_start && loop_end > clamped_start)
+ {
+ goal_index = loop_end;
+ }
+
+ desired_angle = plan[goal_index].theta;
+ has_forward_direction = hasForwardProgress(plan, goal_index);
+
+ 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 = plan[articulation_index].theta;
+ has_forward_direction = hasForwardProgress(plan, articulation_index);
+ return true;
+ }
+ }
+
+ previous_segment_angle = current_angle;
+ previous_segment_angle_set = true;
+ previous_segment_end_index = i;
+ }
+
+ return false;
+}
+
+bool PathFollowerCritic::hasForwardProgress(const std::vector& plan, unsigned int index) 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)
+ {
+ 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_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/config/dwb_local_planner_params.yaml b/mir_navigation/config/dwb_local_planner_params.yaml
index d59aacb5..09754127 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, PathProgress]
+ # critics: [RotateToGoal, ObstacleFootprint, PathAlign, PathDistPruned, PathProgress]
+ critics: [RotateToGoal, PathFollower]
RotateToGoal:
- scale: 100.0
+ scale: 10.0
# lookahead_time: -1.0
# slowing_factor: 5.0
ObstacleFootprint:
@@ -71,6 +72,20 @@ DWBLocalPlanner:
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::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
+ enforce_forward_dot: true
+ always_target_articulations: true
+ plan_alignment_position_tolerance: 0.15
+ plan_alignment_yaw_tolerance: 3.14159265358979323846
+ xy_goal_tolerance: 0.10
+ yaw_goal_tolerance: 0.15
# Prune already passed poses from plan
diff --git a/mir_navigation/launch/move_base.xml b/mir_navigation/launch/move_base.xml
index 090fe7ef..b81e4f0b 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..756ec4ff
--- /dev/null
+++ b/mir_navigation/nodes/global_plan_debug_viz.py
@@ -0,0 +1,124 @@
+#!/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
+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),
+ }
+
+ 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)
+
+ 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
+
+ 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):
+ 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