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_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_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
+    breadth_first_search)
 ament_package()

nav2_route/include/nav2_route/breadth_first_search.hpp

@@ -0,0 +1,125 @@
+// 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 Set the costmap to use 
+   * @param costmap Costmap to use to check for state validity
+   */
+  void setCostmap(nav2_costmap_2d::Costmap2D * costmap);
+
+  /**
+   * @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 mx The x map coordinate of the goal
+   * @param my The y map coordinate of the goal
+   */ 
+  void setGoal(unsigned int mx, unsigned int my);
+
+  /**
+   * @brief Find the closest goal to the start given a costmap, start and goal
+   * @return True if the search was successful
+   */ 
+  bool search();
+
+  /**
+   * @brief Preform a ray trace check to see if the node is directly visable
+   * @param True if the node is visable
+   */
+  bool isNodeVisible(); 
+
+  /**
+   * @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_;
+  NodePtr goal_;
+
+  unsigned int x_size_;
+  unsigned int y_size_;
+  unsigned int max_index_;
+  std::vector<int> neighbors_grid_offsets_;
+  nav2_costmap_2d::Costmap2D * costmap_;
+};
+}  // namespace nav2_route
+
+#endif  // NAV2_ROUTE__BREADTH_FIRST_SEARCH_HPP_

nav2_route/include/nav2_route/goal_intent_extractor.hpp

@@ -15,10 +15,14 @@
 #ifndef NAV2_ROUTE__GOAL_INTENT_EXTRACTOR_HPP_
 #define NAV2_ROUTE__GOAL_INTENT_EXTRACTOR_HPP_
 
+#include <algorithm>
+#include <geometry_msgs/msg/detail/pose_stamped__struct.hpp>
 #include <string>
 #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"
@@ -29,6 +33,7 @@
 #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
 {
@@ -110,6 +115,15 @@ class GoalIntentExtractor
    */
   void setStart(const geometry_msgs::msg::PoseStamped & start_pose);
 
+
+  /**
+   * @brief Checks if there is connection between a node and a given pose 
+   * @param node_index The index of the node 
+   * @param pose The pose 
+   * @return True If there is a connection between a node and the pose 
+   */
+  bool isNodeValid(unsigned int node_index, const geometry_msgs::msg::PoseStamped & pose);
+
 protected:
   rclcpp::Logger logger_{rclcpp::get_logger("GoalIntentExtractor")};
   std::shared_ptr<NodeSpatialTree> node_spatial_tree_;
@@ -121,6 +135,9 @@ class GoalIntentExtractor
   geometry_msgs::msg::PoseStamped start_, goal_;
   bool prune_goal_;
   float max_dist_from_edge_, min_dist_from_goal_, min_dist_from_start_;
+
+  std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
+  std::unique_ptr<BreadthFirstSearch> bfs_;
 };
 
 }  // namespace nav2_route