Nav2 route bfs

nav2_route/include/nav2_route/breadth_first_search.hpp

@@ -23,6 +23,7 @@
 
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
+#include "nav2_util/line_iterator.hpp"
 
 namespace nav2_route
 {
@@ -66,17 +67,27 @@ class BreadthFirstSearch
 
   /**
    * @brief Set the goal of the breadth first search
-   * @param mxs The array x map coordinate of the goals
-   * @param my The array y map coordinate of the goals
+   * @param mx The x map coordinate of the goal
+   * @param my The y map coordinate of the goal
    */ 
-  void setGoals(std::vector<unsigned int> mxs, std::vector<unsigned int> mys);
+  void setGoal(unsigned int mx, unsigned int my);
 
   /**
    * @brief Find the closest goal to the start given a costmap, start and goal
-   * @param closest_goal The coordinates of the closest goal
    * @return True if the search was successful
    */ 
-  bool search(unsigned int & closest_goal_index);
+  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:
   /**
@@ -101,7 +112,7 @@ class BreadthFirstSearch
   std::unordered_map<unsigned int, SimpleNode> graph_;
 
   NodePtr start_;
-  NodeVector goals_;
+  NodePtr goal_;
 
   unsigned int x_size_;
   unsigned int y_size_;

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

nav2_route/src/breadth_first_search.cpp

@@ -18,6 +18,8 @@
 #include <iostream>
 #include <queue>
 #include <unordered_map>
+#include "nav2_costmap_2d/cost_values.hpp"
+#include "nav2_util/line_iterator.hpp"
 
 namespace nav2_route
 {
@@ -54,15 +56,12 @@ void BreadthFirstSearch::setStart(unsigned int mx, unsigned int my)
   start_ = addToGraph(costmap_->getIndex(mx, my));
 }
 
-void BreadthFirstSearch::setGoals(std::vector<unsigned int> mxs, std::vector<unsigned int> mys)
+void BreadthFirstSearch::setGoal(unsigned int mx, unsigned int my)
 {
-  goals_.clear();
-  for (unsigned int i = 0; i < (mxs.size() && mys.size()); ++i) {
-    goals_.emplace_back(addToGraph(costmap_->getIndex(mxs[i], mys[i])));
-  }
+  goal_ = addToGraph(costmap_->getIndex(mx, my));
 }
 
-bool BreadthFirstSearch::search(unsigned int & closest_goal_index)
+bool BreadthFirstSearch::search()
 {
   std::queue<NodePtr> queue;
 
@@ -73,13 +72,11 @@ bool BreadthFirstSearch::search(unsigned int & closest_goal_index)
     auto & current = queue.front();
     queue.pop();
 
+    std::cout << "Current index: " << current->index << std::endl;
+
     // Check goals
-    for (const auto & goal : goals_) {
-      if (current->index == goal->index) {
-        closest_goal_index = current->index;
-        graph_.clear();
-        return true;
-      }
+    if (current->index == goal_->index) {
+      return true;
     }
 
     NodeVector neighbors;
@@ -93,7 +90,6 @@ bool BreadthFirstSearch::search(unsigned int & closest_goal_index)
     }
   }
 
-  graph_.clear();
   return false;
 }
 
@@ -106,7 +102,9 @@ void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & ne
   for (const int & neighbors_grid_offset : neighbors_grid_offsets_) {
     // Check if index is negative
     int index = parent_index + neighbors_grid_offset;
+    std::cout << "Neighbor index: " << index << std::endl;
     if(index < 0){
+      std::cout << "Index is negative" << std::endl;
       continue;
     }
 
@@ -118,6 +116,7 @@ void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & ne
     // Check for wrap around conditions
     if (std::fabs(static_cast<float>(p_mx) - static_cast<float>(n_mx)) > 1 || 
         std::fabs(static_cast<float>(p_my) - static_cast<float>(n_my)) > 1) {
+      std::cout << "Wraps " << std::endl; 
       continue;
     }
 
@@ -135,6 +134,25 @@ void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & ne
   }
 }
 
+bool BreadthFirstSearch::isNodeVisible()
+{
+  unsigned int s_mx, s_my, g_mx, g_my; 
+  costmap_->indexToCells(start_->index, s_mx, s_my);
+  costmap_->indexToCells(goal_->index, g_mx, g_my);
+
+  for (nav2_util::LineIterator line(s_mx, s_my, g_mx, g_my); line.isValid(); line.advance()) {
+    double cost = costmap_->getCost(line.getX(), line.getX()); 
+    std::cout << "cost: " << cost << std::endl;
+
+    if (cost == nav2_costmap_2d::LETHAL_OBSTACLE ||
+        cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+    {
+      return false;
+    }
+  }
+  return true;
+}
+
 bool BreadthFirstSearch::inCollision(unsigned int index)
 {
   unsigned char cost = costmap_->getCost(index);
@@ -147,4 +165,9 @@ bool BreadthFirstSearch::inCollision(unsigned int index)
   return false;
 }
 
+void BreadthFirstSearch::clearGraph()
+{
+    graph_.clear();
+}
+
 }  // namespace nav2_route

nav2_route/src/goal_intent_extractor.cpp

@@ -12,11 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <boost/concept/detail/has_constraints.hpp>
+#include <geometry_msgs/msg/detail/pose_stamped__struct.hpp>
+#include <rclcpp/logging.hpp>
 #include <string>
 #include <memory>
 #include <vector>
 
+#include "nav2_costmap_2d/costmap_subscriber.hpp"
+#include "nav2_route/breadth_first_search.hpp"
 #include "nav2_route/goal_intent_extractor.hpp"
+#include "nav2_util/node_utils.hpp"
 
 namespace nav2_route
 {
@@ -53,6 +59,15 @@ void GoalIntentExtractor::configure(
   nav2_util::declare_parameter_if_not_declared(
     node, "min_dist_from_start", rclcpp::ParameterValue(0.10));
   min_dist_from_start_ = static_cast<float>(node->get_parameter("min_dist_from_start").as_double());
+
+  std::string global_costmap_topic;
+  nav2_util::declare_parameter_if_not_declared(
+    node, "global_costmap_topic", rclcpp::ParameterValue(std::string("global_costmap/costmap_raw")));
+  node->get_parameter("global_costmap_topic", global_costmap_topic);
+
+  costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(node, global_costmap_topic);
+  bfs_ = std::make_unique<BreadthFirstSearch>();
+  bfs_->setCostmap(costmap_sub_->getCostmap().get());
 }
 
 void GoalIntentExtractor::setGraph(Graph & graph, GraphToIDMap * id_to_graph_map)
@@ -122,6 +137,10 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr<const GoalT> goal)
             "Could not determine node closest to start or goal pose requested!");
   }
 
+  if (!isNodeValid(start_route, start_))
+  {
+    RCLCPP_INFO(logger_, "Invalid Starting Route index");
+  }
   return {start_route, end_route};
 }
 
@@ -193,6 +212,32 @@ Route GoalIntentExtractor::pruneStartandGoal(
   return pruned_route;
 }
 
+bool GoalIntentExtractor::isNodeValid(unsigned int node_index, 
+                                      const geometry_msgs::msg::PoseStamped & pose)
+{
+  unsigned int s_mx, s_my, g_mx, g_my; 
+  costmap_sub_->getCostmap()->worldToMap(pose.pose.position.x, pose.pose.position.y, 
+                                         s_mx, s_my);
+
+  // double check the frames, probably need to move into the map frame...
+  float goal_x = (*graph_)[node_index].coords.x;
+  float goal_y = (*graph_)[node_index].coords.y;
+  costmap_sub_->getCostmap()->worldToMap(goal_x, goal_y, g_mx, g_my);
+
+  bfs_->setStart(s_mx, s_my);
+  bfs_->setGoal(g_mx, g_my);
+  if (bfs_->isNodeVisible()) {
+    bfs_->clearGraph();
+    return true;
+  }
+
+  if (bfs_->search()) {
+    bfs_->clearGraph();
+    return true;
+  }
+  return false;
+}
+
 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/test/test_breadth_first_search.cpp

@@ -41,41 +41,49 @@ TEST_F(BFSTestFixture, free_space)
 
   bfs.setStart(0u, 0u);
 
-  // need multiple goals
-  std::vector<unsigned int> mxs = {1u};
-  std::vector<unsigned int> mys = {1u};
-  bfs.setGoals(mxs, mys);
+  unsigned int mx = 1u;
+  unsigned int my = 1u;
+  bfs.setGoal(mx, my);
 
-  unsigned int closest_goal;
-  bool result = bfs.search(closest_goal);
+  bool result = bfs.search();
   EXPECT_TRUE(result);
-
-  auto index = costmap->getIndex(mxs.front(), mys.front());
-
-  EXPECT_EQ(closest_goal, index);
+  bfs.clearGraph();
 }
 
 TEST_F(BFSTestFixture, wall)
 {
   initialize(10u, 10u);
 
-  unsigned int mx = 3;
-  for(unsigned int my=0; my < costmap->getSizeInCellsY() -1; ++my) {
-    costmap->setCost(mx, my, LETHAL_OBSTACLE);
+  unsigned int mx_obs = 3;
+  for(unsigned int my_obs=0; my_obs < costmap->getSizeInCellsY(); ++my_obs) {
+    costmap->setCost(mx_obs, my_obs, LETHAL_OBSTACLE);
   }
 
   bfs.setStart(0u, 0u);
 
-  std::vector<unsigned int> mxs = {2u, 5u};
-  std::vector<unsigned int> mys = {8u, 0u};
+  unsigned int mx = 5u;
+  unsigned int my = 0u;
+
+  bfs.setGoal(mx, my);
+
+  bool result = bfs.search();
+  EXPECT_FALSE(result);
+  bfs.clearGraph();
+}
 
-  bfs.setGoals(mxs, mys);
+TEST_F(BFSTestFixture, ray_trace)
+{
+  initialize(10u, 10u);
+
+  bfs.setStart(0u, 0u);
+  bfs.setGoal(5u, 5u);
 
-  unsigned int closest_goal;
-  bool result = bfs.search(closest_goal);
+  bool result = bfs.isNodeVisible();
   EXPECT_TRUE(result);
 
-  auto index = costmap->getIndex(mxs.front(), mys.front());
+  costmap->setCost(2u, 2u, LETHAL_OBSTACLE);
+  result = bfs.isNodeVisible();
+  EXPECT_FALSE(result);
 
-  EXPECT_EQ(closest_goal, index);
+  bfs.clearGraph();
 }