Refactor build_dispatch_goal_pose to return PoseStamped directly and update related usages

Author: cord-burmeister

include/frontier_exploration_ros2/frontier_explorer_core.hpp

@@ -218,8 +218,8 @@ class FrontierExplorerCore
     const FrontierLike & target_frontier,
     const geometry_msgs::msg::Pose & current_pose) const;
 
-  std::optional<geometry_msgs::msg::PoseStamped> build_dispatch_goal_pose(
-    const FrontierLike & target_frontier,
+  geometry_msgs::msg::PoseStamped build_dispatch_goal_pose(
+      const FrontierLike & target_frontier,
     const geometry_msgs::msg::Pose & current_pose,
     bool bypass_min_distance_dispatch = false) const;
 

src/frontier_explorer_core_dispatch.cpp

@@ -174,7 +174,7 @@ void FrontierExplorerCore::try_send_next_goal()
     frontier_sequence,
     *current_pose,
     "Sending frontier goal (" + selection.mode + "): " + describe_frontier(frontier_sequence.front()),
-    false);
+    escape_mode_active);
 }
 
 void FrontierExplorerCore::reset_exploration_runtime_state(bool clear_maps)
@@ -910,24 +910,19 @@ bool FrontierExplorerCore::send_frontier_goal(
     dispatch_sequence.push_back(frontier_sequence[index]);
   }
 
-  const auto dispatch_goal_pose = build_dispatch_goal_pose(
-    dispatch_sequence.front(),
+const auto goal_pose = build_dispatch_goal_pose(
+      dispatch_sequence.front(),
     current_pose,
     bypass_min_distance_dispatch);
-  if (!dispatch_goal_pose.has_value()) {
-    callbacks.log_info(
-      "No dispatchable frontier goal satisfies the minimum-distance constraint; waiting for updated map data");
-    return false;
-  }
   if (debug_outputs_enabled()) {
-    callbacks.publish_selected_frontier_pose(dispatch_goal_pose.value());
+    callbacks.publish_selected_frontier_pose(goal_pose);
   }
   const std::string dispatch_description = dispatch_index == 0U ?
     description :
     "Sending frontier goal after blocked-goal skip: " + describe_frontier(dispatch_sequence.front());
   // Frontier mode dispatches only the first element from the selected sequence.
   return send_pose_goal(
-    dispatch_goal_pose.value(),
+    goal_pose,
     "frontier",
     dispatch_sequence.front(),
     dispatch_sequence,

src/frontier_explorer_core_frontiers.cpp

@@ -492,7 +492,7 @@ geometry_msgs::msg::PoseStamped FrontierExplorerCore::build_goal_pose(
   return goal_pose;
 }
 
-std::optional<geometry_msgs::msg::PoseStamped> FrontierExplorerCore::build_dispatch_goal_pose(
+geometry_msgs::msg::PoseStamped FrontierExplorerCore::build_dispatch_goal_pose(
   const FrontierLike & target_frontier,
   const geometry_msgs::msg::Pose & current_pose,
   bool bypass_min_distance_dispatch) const
@@ -606,7 +606,7 @@ std::optional<geometry_msgs::msg::PoseStamped> FrontierExplorerCore::build_dispa
     }
   }
 
-  return std::nullopt;
+  return fallback_goal_pose;
 }
 
 std::vector<geometry_msgs::msg::PoseStamped> FrontierExplorerCore::build_goal_pose_sequence(