Nav2 route bfs

.circleci/config.yml

@@ -123,15 +123,24 @@ _commands:
           type: string
         mixins:
           type: string
+        skip:
+          default: ""
+          type: string
+        restore:
+          default: true
+          type: boolean
         build:
           default: true
           type: boolean
       steps:
         - store_artifacts:
             path: << parameters.workspace >>/lockfile.txt
-        - restore_from_cache:
-            key: << parameters.key >>
-            workspace: << parameters.workspace >>
+        - when:
+            condition: << parameters.restore >>
+            steps:
+              - restore_from_cache:
+                  key: << parameters.key >>
+                  workspace: << parameters.workspace >>
         - when:
             condition: << parameters.build >>
             steps:
@@ -191,6 +200,7 @@ _commands:
                     . << parameters.underlay >>/install/setup.sh
                     colcon build \
                       --packages-select ${BUILD_PACKAGES} \
+                      --packages-skip << parameters.skip >> \
                       --mixin << parameters.mixins >>
               - ccache_stats:
                   workspace: << parameters.workspace >>
@@ -365,6 +375,14 @@ _steps:
       underlay: /opt/underlay_ws
       workspace: /opt/overlay_ws
       mixins: ${OVERLAY_MIXINS}
+  setup_workspace_overlay_1: &setup_workspace_overlay_1
+    setup_workspace:
+      <<: *setup_workspace_overlay
+      skip: nav2_system_tests
+  setup_workspace_overlay_2: &setup_workspace_overlay_2
+    setup_workspace:
+      <<: *setup_workspace_overlay
+      restore: false
   restore_overlay_workspace: &restore_overlay_workspace
     setup_workspace:
       <<: *setup_workspace_overlay
@@ -412,7 +430,8 @@ commands:
   build_source:
     description: "Build Source"
     steps:
-      - *setup_overlay_workspace
+      - *setup_workspace_overlay_1
+      - *setup_workspace_overlay_2
   restore_build:
     description: "Restore Build"
     steps:

nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp

@@ -56,6 +56,11 @@ class CostmapSubscriber
    */
   std::shared_ptr<Costmap2D> getCostmap();
 
+  /**
+   * @brief Get the frame id of the costmap 
+   */
+  std::string getFrameID();
+
   /**
    * @brief Convert an occ grid message into a costmap object
    */

nav2_costmap_2d/src/costmap_subscriber.cpp

@@ -52,7 +52,15 @@ std::shared_ptr<Costmap2D> CostmapSubscriber::getCostmap()
   toCostmap2D();
   return costmap_;
 }
-
+std::string CostmapSubscriber::getFrameID()
+{
+  if (!costmap_received_) {
+    throw std::runtime_error("Costmap is not available");
+  }
+  auto current_costmap_msg = std::atomic_load(&costmap_msg_);
+  return current_costmap_msg->header.frame_id;
+}
+
 void CostmapSubscriber::toCostmap2D()
 {
   auto current_costmap_msg = std::atomic_load(&costmap_msg_);

nav2_msgs/CMakeLists.txt

@@ -12,6 +12,9 @@ find_package(action_msgs REQUIRED)
 
 nav2_package()
 
+# TODO(jwallace42): This is a work around for https://github.com/ros2/rosidl_typesupport_fastrtps/issues/28
+add_compile_options(-Wno-error=deprecated)
+
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Costmap.msg"
   "msg/CostmapMetaData.msg"

nav2_msgs/action/ComputeAndTrackRoute.action

@@ -6,6 +6,12 @@ uint16 INDETERMINANT_NODES_ON_GRAPH=403
 uint16 TIMEOUT=404
 uint16 NO_VALID_ROUTE=405
 uint16 OPERATION_FAILED=406
+uint16 START_OUTSIDE_MAP=407
+uint16 GOAL_OUTSIDE_MAP=408
+uint16 START_OCCUPIED=409
+uint16 GOAL_OCCUPIED=410
+uint16 PLANNER_TIMEOUT=411
+uint16 NO_VALID_PATH=412
 
 #goal definition
 uint16 start_id

nav2_msgs/action/ComputeRoute.action

@@ -5,6 +5,12 @@ uint16 NO_VALID_GRAPH=402
 uint16 INDETERMINANT_NODES_ON_GRAPH=403
 uint16 TIMEOUT=404
 uint16 NO_VALID_ROUTE=405
+uint16 START_OUTSIDE_MAP=406
+uint16 GOAL_OUTSIDE_MAP=407
+uint16 START_OCCUPIED=408
+uint16 GOAL_OCCUPIED=409
+uint16 PLANNER_TIMEOUT=410
+uint16 NO_VALID_PATH=411
 
 #goal definition
 uint16 start_id

nav2_route/CMakeLists.txt

@@ -54,6 +54,7 @@ add_library(${library_name} SHARED
   src/path_converter.cpp
   src/graph_loader.cpp
   src/goal_intent_extractor.cpp
+  src/breadth_first_search.cpp
 )
 
 ament_target_dependencies(${library_name}
@@ -119,7 +120,10 @@ install(TARGETS ${executable_name}
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-install(TARGETS ${library_name} edge_scorers route_operations graph_file_loaders
+install(TARGETS ${library_name}
+    edge_scorers
+    route_operations
+    graph_file_loaders
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -138,5 +142,9 @@ endif()
 
 ament_export_include_directories(include)
 ament_export_dependencies(${dependencies})
-ament_export_libraries(${library_name} edge_scorers route_operations graph_file_loaders)
+ament_export_libraries(${library_name}
+    edge_scorers
+    route_operations
+    graph_file_loaders
+)
 ament_package()

nav2_route/include/nav2_route/breadth_first_search.hpp

@@ -0,0 +1,133 @@
+// Copyright (c) 2020, Samsung Research America
+// Copyright (c) 2023 Joshua Wallace
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_ROUTE__BREADTH_FIRST_SEARCH_HPP_
+#define NAV2_ROUTE__BREADTH_FIRST_SEARCH_HPP_
+
+#include <queue>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_util/line_iterator.hpp"
+
+namespace nav2_route
+{
+
+/**
+ * @struct nav2_route::SimpleNode
+ * @brief A Node implementation for the breadth first search
+ */
+struct SimpleNode
+{
+  SimpleNode(unsigned int index)
+  : index(index),
+    explored(false) {}
+
+  unsigned int index;
+  bool explored;
+};
+
+/**
+ * @class nav2_route::BreadthFirstSearch
+ * @brief Preforms a breadth first search between the start and goal
+ */
+class BreadthFirstSearch
+{
+public:
+  typedef SimpleNode * NodePtr;
+  typedef std::vector<NodePtr> NodeVector;
+
+  /**
+   * @brief Initialize the search algorithm
+   * @param costmap Costmap to use to check for state validity
+   */
+  void initialize(std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap, int max_iterations);
+
+  /**
+   * @brief Set the start of the breadth first search
+   * @param mx The x map coordinate of the start
+   * @param my The y map coordinate of the start
+   */
+  void setStart(unsigned int mx, unsigned int my);
+
+  /**
+   * @brief Set the goal of the breadth first search
+   * @param goals The array of goals to search for
+   */
+  void setGoals(std::vector<nav2_costmap_2d::MapLocation> & goals);
+
+  /**
+   * @brief Find the closest goal to the start given a costmap, start and goal
+   * @param goal The index of the goal array provided to the search
+   * @throws nav2_core::PlannerTimedOut If the max iterations were reached
+   * @throws nav2_core::NoValidPathCouldBeFound If no valid path could be found by the search
+   */
+  void search(unsigned int & goal);
+
+  /**
+   * @brief Preform a ray trace check to see if the first node is visable
+   * @param True if the node is visable
+   */
+  bool isFirstGoalVisible();
+
+
+  /**
+   * @brief Get the graph
+   */
+  std::unordered_map<unsigned int, SimpleNode> * getGraph();
+
+  /**
+   * @brief clear the graph
+   */
+  void clearGraph();
+
+private:
+  /**
+   * @brief Adds the node associated with the index to the graph
+   * @param index The index of the node
+   * @return node A pointer to the node added into the graph
+   */
+  NodePtr addToGraph(const unsigned int index);
+
+  /**
+   * @brief Retrieve all valid neighbors of a node
+   * @param parent_index The index to the parent node of the neighbors
+   */
+  void getNeighbors(unsigned int parent_index, NodeVector & neighbors);
+
+  /**
+   * @brief Checks if the index is in collision
+   * @param index The index to check
+   */
+  bool inCollision(unsigned int index);
+
+  std::unordered_map<unsigned int, SimpleNode> graph_;
+
+  NodePtr start_;
+  NodeVector goals_;
+
+  unsigned int x_size_;
+  unsigned int y_size_;
+  unsigned int max_index_;
+  std::vector<int> neighbors_grid_offsets_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_;
+  int max_iterations_;
+};
+}  // namespace nav2_route
+
+#endif  // NAV2_ROUTE__BREADTH_FIRST_SEARCH_HPP_

nav2_route/include/nav2_route/goal_intent_extractor.hpp

@@ -19,16 +19,19 @@
 #include <memory>
 #include <vector>
 
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
 #include "tf2_ros/transform_listener.h"
 #include "nav2_core/route_exceptions.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "nav2_msgs/action/compute_route.hpp"
 #include "nav2_msgs/action/compute_and_track_route.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
 
 #include "nav2_route/types.hpp"
 #include "nav2_route/utils.hpp"
 #include "nav2_route/node_spatial_tree.hpp"
+#include "nav2_route/breadth_first_search.hpp"
 
 namespace nav2_route
 {
@@ -69,6 +72,11 @@ class GoalIntentExtractor
     const std::string & route_frame,
     const std::string & base_frame);
 
+  /**
+   * @brief Initialize extractor
+   */
+  void initialize();
+
   /**
    * @brief Sets a new graph when updated
    * @param graph Graph to populate kD tree using
@@ -77,10 +85,14 @@ class GoalIntentExtractor
   void setGraph(Graph & graph, GraphToIDMap * id_to_graph_map);
 
   /**
-   * @brief Transforms a pose into the route frame
+   * @brief Transforms a pose into the desired frame
    * @param pose Pose to transform (e.g. start, goal)
+   * @param frame_id The frame to transform the pose into
    */
-  geometry_msgs::msg::PoseStamped transformPose(geometry_msgs::msg::PoseStamped & pose);
+  geometry_msgs::msg::PoseStamped transformPose(
+      geometry_msgs::msg::PoseStamped & pose,
+      const std::string & frame_id);
+
   /**
    * @brief Main API to find the start and goal graph IDX (not IDs) for routing
    * @param goal Action request goal
@@ -111,16 +123,46 @@ class GoalIntentExtractor
   void setStart(const geometry_msgs::msg::PoseStamped & start_pose);
 
 protected:
+  /**
+   * @brief Checks if there is a valid connection between a graph node and a pose by 
+   * preforming a breadth first search through the costmap
+   * @param node_indices A list of graph node indices to check
+   * @param pose The pose that needs to be associated with a graph node
+   * @return The index of the closest graph node found in the search 
+   * @throws nav2_core::StartOutsideMapBounds If the start index is not in the costmap
+   * @throws nav2_core::StartOccupied If the start is in lethal cost
+   */
+  unsigned int associatePoseWithGraphNode(
+    std::vector<unsigned int> node_indices,
+    geometry_msgs::msg::PoseStamped & pose);
+
+  /**
+   * @brief Visualize the search expansions
+   * @param occ_grid_pub A occupancy grid publisher to view the expansions
+   */
+  void visualizeExpansions(
+    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_grid_pub);
+
   rclcpp::Logger logger_{rclcpp::get_logger("GoalIntentExtractor")};
+  rclcpp::Clock::SharedPtr clock_;
   std::shared_ptr<NodeSpatialTree> node_spatial_tree_;
   GraphToIDMap * id_to_graph_map_;
   Graph * graph_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::string route_frame_;
   std::string base_frame_;
+  std::string costmap_frame_;
   geometry_msgs::msg::PoseStamped start_, goal_;
   bool prune_goal_;
   float max_dist_from_edge_, min_dist_from_goal_, min_dist_from_start_;
+
+  bool enable_search_;
+  bool enable_search_viz_;
+  int max_iterations_;
+  std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
+  std::unique_ptr<BreadthFirstSearch> bfs_;
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr start_expansion_viz_;
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr goal_expansion_viz_;
 };
 
 }  // namespace nav2_route