added getStart method to goal_intent_extract and start pose argument for findRoute

Author: alexanderjyuen

nav2_route/include/nav2_route/goal_intent_extractor.hpp

@@ -111,6 +111,12 @@ class GoalIntentExtractor
    */
   void overrideStart(const geometry_msgs::msg::PoseStamped & start_pose);
 
+  /**
+   * @brief gets the desired start pose
+   * @return PoseStamped of start pose
+   */
+  geometry_msgs::msgs::PoseStamped getStart();
+
 protected:
   rclcpp::Logger logger_{rclcpp::get_logger("GoalIntentExtractor")};
   std::shared_ptr<NodeSpatialTree> node_spatial_tree_;

nav2_route/include/nav2_route/route_planner.hpp

@@ -68,6 +68,7 @@ class RoutePlanner
   Route findRoute(
     Graph & graph, unsigned int start_index, unsigned int goal_index,
     const std::vector<unsigned int> & blocked_ids,
+    const geometry_msgs::msg::PoseStamped & start_pose,
     const geometry_msgs::msg::PoseStamped & goal);
 
 protected:

nav2_route/src/goal_intent_extractor.cpp

@@ -195,6 +195,11 @@ Route GoalIntentExtractor::pruneStartandGoal(
   return pruned_route;
 }
 
+geometry_msgs::msgs::PoseStamped  GoalIntentExtractor::getStart()
+{
+  return start_;
+}
+
 template Route GoalIntentExtractor::pruneStartandGoal<nav2_msgs::action::ComputeRoute::Goal>(
   const Route & input_route,
   const std::shared_ptr<const nav2_msgs::action::ComputeRoute::Goal> goal,

nav2_route/src/route_planner.cpp

@@ -42,6 +42,7 @@ void RoutePlanner::configure(
 Route RoutePlanner::findRoute(
   Graph & graph, unsigned int start_index, unsigned int goal_index,
   const std::vector<unsigned int> & blocked_ids,
+  const geometry_msgs::msg::PoseStamped & /* start_pose */,
   const geometry_msgs::msg::PoseStamped & goal_pose)
 {
   if (graph.empty()) {

nav2_route/src/route_server.cpp

@@ -217,7 +217,7 @@ Route RouteServer::findRoute(
   } else {
     // Compute the route via graph-search, returns a node-edge sequence
     route = route_planner_->findRoute(
-      graph_, start_route, end_route, rerouting_info.blocked_ids,
+      graph_, start_route, end_route, rerouting_info.blocked_ids, goal_intent_extractor->getStart(),
       goal->goal);
   }