diff --git a/.circleci/config.yml b/.circleci/config.yml index ede7b367e9..7f66fd214a 100644 --- a/.circleci/config.yml +++ b/.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: diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index c08384f53d..dad07c2999 100644 --- a/nav2_bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/rviz/nav2_default_view.rviz @@ -536,6 +536,52 @@ Visualization Manager: Value: true Enabled: false Name: Realsense + - Alpha: 0.699999988079071 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /start_expansions + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /start_expansions_updates + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_expansions + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_expansions_updates + Use Timestamp: false + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Name: MarkerArray diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index e4ea745015..112d96679b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -56,6 +56,11 @@ class CostmapSubscriber */ std::shared_ptr getCostmap(); + /** + * @brief Get the frame id of the costmap + */ + std::string getFrameID(); + /** * @brief Convert an occ grid message into a costmap object */ diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index d56ac942e9..5ea9f1d8bf 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -52,7 +52,15 @@ std::shared_ptr 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_); diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 47093fb5e6..28ee58feb9 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/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" diff --git a/nav2_msgs/action/ComputeAndTrackRoute.action b/nav2_msgs/action/ComputeAndTrackRoute.action index b10629bb14..0ea7c8cd2f 100644 --- a/nav2_msgs/action/ComputeAndTrackRoute.action +++ b/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 diff --git a/nav2_msgs/action/ComputeRoute.action b/nav2_msgs/action/ComputeRoute.action index c702e32a47..72587f5d80 100644 --- a/nav2_msgs/action/ComputeRoute.action +++ b/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 diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index f4a4a477db..911be39ba1 100644 --- a/nav2_route/CMakeLists.txt +++ b/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/dijkstra_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() diff --git a/nav2_route/include/nav2_route/dijkstra_search.hpp b/nav2_route/include/nav2_route/dijkstra_search.hpp new file mode 100644 index 0000000000..f9da222a7e --- /dev/null +++ b/nav2_route/include/nav2_route/dijkstra_search.hpp @@ -0,0 +1,149 @@ +// Copyright (c) 2020, Samsung Research America +// Copyright (c) 2022 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 +#include +#include +#include +#include + +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/line_iterator.hpp" + +namespace nav2_route +{ + +/** + * @class nav2_route::BreadthFirstSearch + * @brief Preforms a breadth first search between the start and goal + */ +class DijkstraSearch +{ +public: + /** + * @struct nav2_route::SimpleNode + * @brief A Node implementation for the breadth first search + */ + struct SimpleNode + { + SimpleNode(unsigned int index) + : index(index), + visited(false), + cost(std::numeric_limits::max()){} + + unsigned int index; + bool visited; + float cost; + }; + typedef SimpleNode * NodePtr; + typedef std::vector NodeVector; + typedef std::pair QueueElement; + + struct CompareNodeCost { + bool operator()(const QueueElement & a, const QueueElement & b) const { + return a.first > b.first; + } + }; + typedef std::priority_queue, CompareNodeCost> NodeQueue; + + /** + * @brief Initialize the search algorithm + * @param costmap Costmap to use to check for state validity + */ + void initialize(std::shared_ptr 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 & 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 * 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); + + /** + * @brief Calculate the cost + */ + float calculateCost(unsigned int current_index, unsigned int neighbor_index); + + std::unordered_map graph_; + + NodePtr start_; + NodeVector goals_; + + unsigned int x_size_; + unsigned int y_size_; + unsigned int max_index_; + std::vector diagonals_; + std::vector neighbors_grid_offsets_; + std::shared_ptr costmap_; + int max_iterations_; +}; +} // namespace nav2_route + +#endif // NAV2_ROUTE__BREADTH_FIRST_SEARCH_HPP_ diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index fb00120bd5..790a62820e 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -19,16 +19,19 @@ #include #include +#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/dijkstra_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,51 @@ 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 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::SharedPtr occ_grid_pub); + rclcpp::Logger logger_{rclcpp::get_logger("GoalIntentExtractor")}; + rclcpp::Clock::SharedPtr clock_; std::shared_ptr node_spatial_tree_; GraphToIDMap * id_to_graph_map_; Graph * graph_; std::shared_ptr 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 use_closest_node_on_search_failure_; + bool enable_search_viz_; + int max_iterations_; + std::unique_ptr costmap_sub_; + std::shared_ptr costmap_; + std::unique_ptr ds_; + rclcpp::Publisher::SharedPtr start_expansion_viz_; + rclcpp::Publisher::SharedPtr goal_expansion_viz_; + + rclcpp::Publisher::SharedPtr route_start_pose_pub_; + rclcpp::Publisher::SharedPtr route_goal_pose_pub_; }; } // namespace nav2_route diff --git a/nav2_route/include/nav2_route/node_spatial_tree.hpp b/nav2_route/include/nav2_route/node_spatial_tree.hpp index 679ee63b4e..5d33fb6ed1 100644 --- a/nav2_route/include/nav2_route/node_spatial_tree.hpp +++ b/nav2_route/include/nav2_route/node_spatial_tree.hpp @@ -69,8 +69,15 @@ class NodeSpatialTree public: /** * @brief Constructor + * @param num_of_nearest_nodes The number of nearest node to return when preforming a + * search in the kd tree */ - NodeSpatialTree() = default; + NodeSpatialTree(int num_of_nearest_nodes); + + /** + * @brief Delete the default constructor + */ + NodeSpatialTree() = delete; /** * @brief Destructor @@ -86,17 +93,18 @@ class NodeSpatialTree /** * @brief Find the closest node to a given pose * @param pose_in Pose to find node near - * @param node_id The return ID of the node + * @param node_ids The return ID of the nodes * @return if successfully found */ - bool findNearestGraphNodeToPose( + bool findNearestGraphNodesToPose( const geometry_msgs::msg::PoseStamped & pose_in, - unsigned int & node_id); + std::vector & node_ids); protected: - kd_tree_t * kdtree_; - GraphAdaptor * adaptor_; - Graph * graph_; + kd_tree_t * kdtree_{nullptr}; + GraphAdaptor * adaptor_{nullptr}; + Graph * graph_{nullptr}; + int num_of_nearest_nodes_; }; } // namespace nav2_route diff --git a/nav2_route/src/dijkstra_search.cpp b/nav2_route/src/dijkstra_search.cpp new file mode 100644 index 0000000000..3d63d7f563 --- /dev/null +++ b/nav2_route/src/dijkstra_search.cpp @@ -0,0 +1,205 @@ +// 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. + +#include "nav2_route/dijkstra_search.hpp" + +#include +#include +#include +#include "nav2_core/planner_exceptions.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/line_iterator.hpp" + +namespace nav2_route +{ + +void DijkstraSearch::initialize(std::shared_ptr costmap, int max_iterations) +{ + costmap_ = costmap; + + int x_size = static_cast(costmap_->getSizeInCellsX()); + int y_size = static_cast(costmap_->getSizeInCellsY()); + + max_index_ = x_size * y_size; + x_size_ = x_size; + + neighbors_grid_offsets_ = {-1, +1, + -x_size, x_size}; + diagonals_ = {-x_size - 1, -x_size + 1, + +x_size -1, +x_size +1}; + + neighbors_grid_offsets_.insert( + neighbors_grid_offsets_.end(), diagonals_.begin(), diagonals_.end()); + + max_iterations_ = max_iterations; +} + +DijkstraSearch::NodePtr DijkstraSearch::addToGraph(const unsigned int index) +{ + auto iter = graph_.find(index); + if (iter != graph_.end()) { + return &(iter->second); + } + + return &(graph_.emplace(index, SimpleNode(index)).first->second); +} + +void DijkstraSearch::setStart(unsigned int mx, unsigned int my) +{ + start_ = addToGraph(costmap_->getIndex(mx, my)); +} + +void DijkstraSearch::setGoals(std::vector & goals) +{ + goals_.clear(); + for (const auto & goal : goals) { + goals_.push_back(addToGraph(costmap_->getIndex(goal.x, goal.y))); + } +} + +void DijkstraSearch::search(unsigned int & goal) +{ + NodeQueue queue; + start_->cost = 0.0f; + queue.push(std::make_pair(start_->cost, start_)); + + int iteration = 0; + while (!queue.empty()) { + + auto current = queue.top().second; + queue.pop(); + + if (iteration > max_iterations_) { + throw nav2_core::PlannerTimedOut("Exceeded maximum iterations"); + } + + for (unsigned int i = 0; i < goals_.size(); ++i) { + if (current->index == goals_[i]->index) { + goal = i; + return; + } + } + + NodeVector neighbors; + getNeighbors(current->index, neighbors); + + for (const auto neighbor : neighbors) { + if (!neighbor->visited) { + float updated_cost = current->cost + calculateCost(current->index, neighbor->index); + if (updated_cost < neighbor->cost) { + neighbor->cost = updated_cost; + queue.push(std::make_pair(neighbor->cost, neighbor)); + } + } + } + current->visited = true; + iteration++; + } + + throw nav2_core::NoValidPathCouldBeFound("No valid path found"); +} + +void DijkstraSearch::getNeighbors(unsigned int parent_index, NodeVector & neighbors) +{ + unsigned int p_mx, p_my; + costmap_->indexToCells(parent_index, p_mx, p_my); + + unsigned int neighbor_index; + for (const int & neighbors_grid_offset : neighbors_grid_offsets_) { + // Check if index is negative + int index = parent_index + neighbors_grid_offset; + if (index < 0) { + continue; + } + + neighbor_index = static_cast(index); + + unsigned int n_mx, n_my; + costmap_->indexToCells(neighbor_index, n_mx, n_my); + + // Check for wrap around conditions + if (std::fabs(static_cast(p_mx) - static_cast(n_mx)) > 1 || + std::fabs(static_cast(p_my) - static_cast(n_my)) > 1) + { + continue; + } + + // Check for out of bounds + if (neighbor_index >= max_index_) { + continue; + } + + if (inCollision(neighbor_index)) { + continue; + } + + neighbors.push_back(addToGraph(neighbor_index)); + } +} + +bool DijkstraSearch::isFirstGoalVisible() +{ + unsigned int s_mx, s_my, g_mx, g_my; + costmap_->indexToCells(start_->index, s_mx, s_my); + costmap_->indexToCells(goals_.front()->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.getY()); + if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + { + return false; + } + } + graph_.clear(); + return true; +} + +bool DijkstraSearch::inCollision(unsigned int index) +{ + unsigned char cost = costmap_->getCost(index); + + if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + { + return true; + } + return false; +} + + +float DijkstraSearch::calculateCost(unsigned int current_index, unsigned int neighbor_index) +{ + auto diff = static_cast(neighbor_index) - static_cast(current_index); + for (const auto & offset : diagonals_) { + if (diff == offset) { + return 14.0f; + } + } + return 10.0f; +} + +void DijkstraSearch::clearGraph() +{ + graph_.clear(); +} + +std::unordered_map * DijkstraSearch::getGraph() +{ + return &graph_; +} + +} // namespace nav2_route diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index 0def632920..147adb24d2 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -12,11 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include +#include "nav2_core/planner_exceptions.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_route/goal_intent_extractor.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" namespace nav2_route { @@ -32,13 +39,12 @@ void GoalIntentExtractor::configure( const std::string & base_frame) { logger_ = node->get_logger(); + clock_ = node->get_clock(); id_to_graph_map_ = id_to_graph_map; graph_ = &graph; tf_ = tf; route_frame_ = route_frame; base_frame_ = base_frame; - node_spatial_tree_ = std::make_shared(); - node_spatial_tree_->computeTree(graph); nav2_util::declare_parameter_if_not_declared( node, "prune_goal", rclcpp::ParameterValue(true)); @@ -53,6 +59,63 @@ 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(node->get_parameter("min_dist_from_start").as_double()); + + nav2_util::declare_parameter_if_not_declared( + node, "num_of_nearest_nodes", rclcpp::ParameterValue(3)); + int num_of_nearest_nodes = node->get_parameter("num_of_nearest_nodes").as_int(); + + node_spatial_tree_ = std::make_shared(num_of_nearest_nodes); + node_spatial_tree_->computeTree(graph); + + nav2_util::declare_parameter_if_not_declared( + node, "enable_search", rclcpp::ParameterValue(false)); + enable_search_ = node->get_parameter("enable_search").as_bool(); + + route_start_pose_pub_ = + node->create_publisher( + "route_start_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + route_goal_pose_pub_ = + node->create_publisher( + "route_goal_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + if (enable_search_) { + nav2_util::declare_parameter_if_not_declared( + node, "use_closest_node_on_search_failure", rclcpp::ParameterValue(true)); + use_closest_node_on_search_failure_ = node->get_parameter("use_closest_node_on_search_failure").as_bool(); + + std::string costmap_topic; + nav2_util::declare_parameter_if_not_declared( + node, "costmap_topic", + rclcpp::ParameterValue(std::string("global_costmap/costmap_raw"))); + node->get_parameter("costmap_topic", costmap_topic); + + nav2_util::declare_parameter_if_not_declared( + node, "max_iterations", rclcpp::ParameterValue(10000)); + max_iterations_ = node->get_parameter("max_iterations").as_int(); + + nav2_util::declare_parameter_if_not_declared( + node, "enable_search_viz", rclcpp::ParameterValue(true)); + enable_search_viz_ = node->get_parameter("enable_search_viz").as_bool(); + + costmap_sub_ = + std::make_unique(node, costmap_topic); + ds_ = std::make_unique(); + start_expansion_viz_ = node->create_publisher( + "start_expansions", + 1); + goal_expansion_viz_ = + node->create_publisher("goal_expansions", 1); + } +} + +void GoalIntentExtractor::initialize() +{ + if (enable_search_) { + costmap_ = costmap_sub_->getCostmap(); + ds_->initialize(costmap_, max_iterations_); + costmap_frame_ = costmap_sub_->getFrameID(); + } } void GoalIntentExtractor::setGraph(Graph & graph, GraphToIDMap * id_to_graph_map) @@ -62,15 +125,16 @@ void GoalIntentExtractor::setGraph(Graph & graph, GraphToIDMap * id_to_graph_map } geometry_msgs::msg::PoseStamped GoalIntentExtractor::transformPose( - geometry_msgs::msg::PoseStamped & pose) + geometry_msgs::msg::PoseStamped & pose, + const std::string & frame_id) { - if (pose.header.frame_id != route_frame_) { + if (pose.header.frame_id != frame_id) { RCLCPP_INFO( logger_, "Request pose in %s frame. Converting to route server frame: %s.", pose.header.frame_id.c_str(), route_frame_.c_str()); - if (!nav2_util::transformPoseInTargetFrame(pose, pose, *tf_, route_frame_)) { - throw nav2_core::RouteTFError("Failed to transform starting pose to: " + route_frame_); + if (!nav2_util::transformPoseInTargetFrame(pose, pose, *tf_, frame_id)) { + throw nav2_core::RouteTFError("Failed to transform pose to: " + frame_id); } } return pose; @@ -108,21 +172,56 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) } } - // transform to route_frame - start_ = transformPose(start_pose); - goal_ = transformPose(goal_pose); + route_start_pose_pub_->publish(start_pose); + route_goal_pose_pub_->publish(goal_pose); + + start_ = transformPose(start_pose, route_frame_); + goal_ = transformPose(goal_pose, route_frame_); // Find closest route graph nodes to start and goal to plan between. // Note that these are the location indices in the graph - unsigned int start_route = 0, end_route = 0; - if (!node_spatial_tree_->findNearestGraphNodeToPose(start_, start_route) || - !node_spatial_tree_->findNearestGraphNodeToPose(goal_, end_route)) + std::vector start_route_ids, end_route_ids; + if (!node_spatial_tree_->findNearestGraphNodesToPose(start_, start_route_ids) || + !node_spatial_tree_->findNearestGraphNodesToPose(goal_, end_route_ids)) { throw nav2_core::IndeterminantNodesOnGraph( "Could not determine node closest to start or goal pose requested!"); } - return {start_route, end_route}; + if (enable_search_) { + costmap_ = costmap_sub_->getCostmap(); + + unsigned int valid_start_route_id; + try { + valid_start_route_id = associatePoseWithGraphNode(start_route_ids, start_); + } catch (nav2_core::PlannerException &ex) { + if(use_closest_node_on_search_failure_) { + RCLCPP_WARN(logger_, "Node association failed to find start id. Using closeset node"); + return {start_route_ids.front(), end_route_ids.front()}; + } + throw ex; + } + + visualizeExpansions(start_expansion_viz_); + ds_->clearGraph(); + + unsigned int valid_end_route_id; + try { + valid_end_route_id = associatePoseWithGraphNode(end_route_ids, goal_); + } catch (nav2_core::PlannerException &ex) { + if(use_closest_node_on_search_failure_) { + RCLCPP_WARN(logger_, "Node association failed to find end id. Using closeset node"); + return {start_route_ids.front(), end_route_ids.front()}; + } + throw ex; + } + visualizeExpansions(goal_expansion_viz_); + ds_->clearGraph(); + + return {valid_start_route_id, valid_end_route_id}; + } + + return {start_route_ids.front(), end_route_ids.front()}; } template @@ -193,6 +292,104 @@ Route GoalIntentExtractor::pruneStartandGoal( return pruned_route; } +unsigned int GoalIntentExtractor::associatePoseWithGraphNode( + std::vector node_indices, + geometry_msgs::msg::PoseStamped & pose) +{ + auto pose_transformed = transformPose(pose, costmap_frame_); + unsigned int s_mx, s_my, g_mx, g_my; + if (!costmap_->worldToMap( + pose_transformed.pose.position.x, pose_transformed.pose.position.y, + s_mx, s_my)) + { + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(pose_transformed.pose.position.x) + ", " + + std::to_string(pose_transformed.pose.position.y) + ") was outside bounds"); + } + + auto cost = costmap_->getCost(s_mx, s_my); + if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { + throw nav2_core::StartOccupied("Start was in lethal cost"); + } + + std::vector valid_node_indices; + std::vector goals; + for (const auto & node_index : node_indices) { + geometry_msgs::msg::PoseStamped route_goal; + geometry_msgs::msg::PoseStamped goal; + auto node = graph_->at(node_index); + route_goal.pose.position.x = node.coords.x; + route_goal.pose.position.y = node.coords.y; + route_goal.header.frame_id = node.coords.frame_id; + goal = transformPose(route_goal, costmap_frame_); + + float goal_x = goal.pose.position.x; + float goal_y = goal.pose.position.y; + if (!costmap_->worldToMap(goal_x, goal_y, g_mx, g_my)) { + RCLCPP_WARN_STREAM( + logger_, "Goal coordinate of(" + std::to_string(goal_x) + ", " + + std::to_string(goal_y) + ") was outside bounds. Removing from goal list"); + continue; + } + + if (costmap_->getCost(g_mx, g_my) == nav2_costmap_2d::LETHAL_OBSTACLE) { + RCLCPP_WARN_STREAM( + logger_, "Goal corrdinate of(" + std::to_string(goal_x) + ", " + + std::to_string(goal_y) + ") was in lethal cost. Removing from goal list."); + continue; + } + valid_node_indices.push_back(node_index); + goals.push_back({g_mx, g_my}); + } + + if (goals.empty()) { + RCLCPP_WARN(logger_, "All goals for association were invalid"); + throw nav2_core::PlannerException("All goals were invalid"); + } + + ds_->clearGraph(); + ds_->setStart(s_mx, s_my); + ds_->setGoals(goals); + if (ds_->isFirstGoalVisible()) { + // The visiblity check only validates the first node in goal array + return valid_node_indices.front(); + } + RCLCPP_WARN(logger_, "Visability Check failed"); + + unsigned int goal; + ds_->search(goal); + + return valid_node_indices[goal]; +} + +void GoalIntentExtractor::visualizeExpansions( + rclcpp::Publisher::SharedPtr occ_grid_pub) +{ + if (!enable_search_viz_) { + return; + } + + nav_msgs::msg::OccupancyGrid occ_grid_msg; + unsigned int width = costmap_->getSizeInCellsX(); + unsigned int height = costmap_->getSizeInCellsY(); + occ_grid_msg.header.frame_id = "map"; + occ_grid_msg.info.resolution = costmap_->getResolution(); + occ_grid_msg.info.width = width; + occ_grid_msg.info.height = height; + occ_grid_msg.info.origin.position.x = costmap_->getOriginX(); + occ_grid_msg.info.origin.position.y = costmap_->getOriginY(); + occ_grid_msg.info.origin.orientation.w = 1.0; + occ_grid_msg.data.assign(width * height, 0); + + auto graph = ds_->getGraph(); + + for (const auto & node : *graph) { + occ_grid_msg.data[node.first] = 100; + } + occ_grid_pub->publish(occ_grid_msg); +} + template Route GoalIntentExtractor::pruneStartandGoal( const Route & input_route, const std::shared_ptr goal, diff --git a/nav2_route/src/graph_loader.cpp b/nav2_route/src/graph_loader.cpp index 25b268bc70..6bb460ba09 100644 --- a/nav2_route/src/graph_loader.cpp +++ b/nav2_route/src/graph_loader.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include "nav2_route/graph_loader.hpp" @@ -32,7 +33,9 @@ GraphLoader::GraphLoader( route_frame_ = frame; nav2_util::declare_parameter_if_not_declared( - node, "graph_filepath", rclcpp::ParameterValue(std::string(""))); + node, "graph_filepath", rclcpp::ParameterValue( + ament_index_cpp::get_package_share_directory("nav2_route") + + "/graphs/aws_graph.geojson")); graph_filepath_ = node->get_parameter("graph_filepath").as_string(); // Default Graph Parser diff --git a/nav2_route/src/node_spatial_tree.cpp b/nav2_route/src/node_spatial_tree.cpp index 45c4a65bbd..c1f81d6102 100644 --- a/nav2_route/src/node_spatial_tree.cpp +++ b/nav2_route/src/node_spatial_tree.cpp @@ -21,6 +21,11 @@ namespace nav2_route { +NodeSpatialTree::NodeSpatialTree(int num_of_nearest_nodes) +{ + num_of_nearest_nodes_ = num_of_nearest_nodes; +} + NodeSpatialTree::~NodeSpatialTree() { if (kdtree_) { @@ -52,10 +57,10 @@ void NodeSpatialTree::computeTree(Graph & graph) graph_ = &graph; } -bool NodeSpatialTree::findNearestGraphNodeToPose( - const geometry_msgs::msg::PoseStamped & pose_in, unsigned int & node_id) +bool NodeSpatialTree::findNearestGraphNodesToPose( + const geometry_msgs::msg::PoseStamped & pose_in, std::vector & node_ids) { - size_t num_results = 1; + size_t num_results = num_of_nearest_nodes_; std::vector ret_index(num_results); std::vector out_dist_sqr(num_results); const double query_pt[2] = {pose_in.pose.position.x, pose_in.pose.position.y}; @@ -65,7 +70,10 @@ bool NodeSpatialTree::findNearestGraphNodeToPose( return false; } - node_id = ret_index[0]; + for (int i = 0; i < num_of_nearest_nodes_ && i < static_cast(ret_index.size()); ++i) { + node_ids.push_back(ret_index[i]); + } + return true; } diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 86c1a19d92..c5679a1c55 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -13,6 +13,7 @@ // limitations under the License. Reserved. #include "nav2_route/route_server.hpp" +#include "nav2_core/planner_exceptions.hpp" using nav2_util::declare_parameter_if_not_declared; using std::placeholders::_1; @@ -42,6 +43,7 @@ RouteServer::on_configure(const rclcpp_lifecycle::State & /*state*/) node->create_publisher( "route_graph", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + compute_route_server_ = std::make_shared( node, "compute_route", std::bind(&RouteServer::computeRoute, this), @@ -100,6 +102,7 @@ nav2_util::CallbackReturn RouteServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); + goal_intent_extractor_->initialize(); compute_route_server_->activate(); compute_and_track_route_server_->activate(); graph_vis_publisher_->on_activate(); @@ -297,6 +300,27 @@ RouteServer::processRouteRequest( exceptionWarning(goal, ex); result->error_code = ActionT::Goal::UNKNOWN; action_server->terminate_current(result); + } catch (nav2_core::StartOccupied & ex) { + result->error_code = ActionT::Goal::START_OCCUPIED; + action_server->terminate_current(result); + } catch (nav2_core::GoalOccupied & ex) { + result->error_code = ActionT::Goal::GOAL_OCCUPIED; + action_server->terminate_current(result); + } catch (nav2_core::NoValidPathCouldBeFound & ex) { + result->error_code = ActionT::Goal::NO_VALID_PATH; + action_server->terminate_current(result); + } catch (nav2_core::PlannerTimedOut & ex) { + result->error_code = ActionT::Goal::TIMEOUT; + action_server->terminate_current(result); + } catch (nav2_core::StartOutsideMapBounds & ex) { + result->error_code = ActionT::Goal::START_OUTSIDE_MAP; + action_server->terminate_current(result); + } catch (nav2_core::GoalOutsideMapBounds & ex) { + result->error_code = ActionT::Goal::GOAL_OUTSIDE_MAP; + action_server->terminate_current(result); + } catch (nav2_core::PlannerTFError & ex) { + result->error_code = ActionT::Goal::TF_ERROR; + action_server->terminate_current(result); } catch (std::exception & ex) { exceptionWarning(goal, ex); result->error_code = ActionT::Goal::UNKNOWN; diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt index df2c544d9e..d6109ef2dc 100644 --- a/nav2_route/test/CMakeLists.txt +++ b/nav2_route/test/CMakeLists.txt @@ -142,3 +142,14 @@ ament_target_dependencies(test_route_server target_link_libraries(test_route_server ${library_name} ) + +# Test Breadth first search +ament_add_gtest(test_ds + test_dijkstra_search.cpp +) +ament_target_dependencies(test_ds + ${dependencies} +) +target_link_libraries(test_ds + ${library_name} +) diff --git a/nav2_route/test/performance_benchmarking.cpp b/nav2_route/test/performance_benchmarking.cpp index a4a9682c0b..c527e4e386 100644 --- a/nav2_route/test/performance_benchmarking.cpp +++ b/nav2_route/test/performance_benchmarking.cpp @@ -54,8 +54,8 @@ inline Graph createGraph() } if (j > 0) { // (i, j - 1) - node.addEdge(e_cost, &graph[curr_graph_idx - DIM], curr_edge_idx++); - graph[curr_graph_idx - DIM].addEdge(e_cost, &node, curr_edge_idx++); + node.addEdge(e_cost, &graph[curr_graph_idx - DIM], curr_edge_idx++); + graph[curr_graph_idx - DIM].addEdge(e_cost, &node, curr_edge_idx++); } curr_graph_idx++; @@ -119,16 +119,18 @@ int main(int argc, char const * argv[]) // "Averaged route size: %f", static_cast(route_legs) / static_cast(NUM_TESTS)); // Third test: Check how much time it takes to do random lookups in the Kd-tree of various graph sizes - std::shared_ptr kd_tree = std::make_shared(); + int num_of_nearest_nodes = 1; + std::shared_ptr kd_tree = + std::make_shared(num_of_nearest_nodes); kd_tree->computeTree(graph); auto start = node->now(); for (unsigned int i = 0; i != NUM_TESTS; i++) { - unsigned int kd_tree_idx; + std::vector kd_tree_idx; geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = static_cast(rand() % DIM); pose.pose.position.y = static_cast(rand() % DIM); - kd_tree->findNearestGraphNodeToPose(pose, kd_tree_idx); + kd_tree->findNearestGraphNodesToPose(pose, kd_tree_idx); } auto end = node->now(); RCLCPP_INFO( diff --git a/nav2_route/test/test_dijkstra_search.cpp b/nav2_route/test/test_dijkstra_search.cpp new file mode 100644 index 0000000000..a1c662054b --- /dev/null +++ b/nav2_route/test/test_dijkstra_search.cpp @@ -0,0 +1,100 @@ +// 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. + +#include +#include + +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_route/dijkstra_search.hpp" + +using namespace nav2_costmap_2d; //NOLINT +using namespace nav2_route; //NOLINT + +class DSTestFixture : public ::testing::Test +{ +public: + void initialize(unsigned int x_size, unsigned int y_size) + { + costmap = std::make_unique(x_size, y_size, 0.0, 0.0, 1); + + ds.initialize(costmap, 200); + } + + DijkstraSearch ds; + std::shared_ptr costmap; +}; + +TEST_F(DSTestFixture, free_space) +{ + initialize(100u, 100u); + + ds.setStart(0u, 0u); + + std::vector goals; + goals.push_back({50u, 50u}); + ds.setGoals(goals); + + unsigned int goal = 0; + ASSERT_NO_THROW(ds.search(goal)); + + EXPECT_EQ(costmap->getIndex(goals[goal].x, goals[goal].y), costmap->getIndex(50u, 50u)); + ds.clearGraph(); +} + +TEST_F(DSTestFixture, wall) +{ + initialize(10u, 10u); + + 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); + } + + ds.setStart(0u, 0u); + + std::vector goals; + goals.push_back({5u, 0u}); + goals.push_back({0u, 4u}); + ds.setGoals(goals); + + unsigned int goal = 0; + EXPECT_NO_THROW(ds.search(goal)); + + EXPECT_EQ(costmap->getIndex(goals[goal].x, goals[goal].y), costmap->getIndex(0u, 4u)); + ds.clearGraph(); +} + +TEST_F(DSTestFixture, ray_trace) +{ + initialize(10u, 10u); + + ds.setStart(0u, 0u); + + std::vector goals; + goals.push_back({5u, 5u}); + ds.setGoals(goals); + + bool result = ds.isFirstGoalVisible(); + EXPECT_TRUE(result); + ds.clearGraph(); + + ds.setStart(0u, 0u); + ds.setGoals(goals); + costmap->setCost(2u, 2u, LETHAL_OBSTACLE); + result = ds.isFirstGoalVisible(); + EXPECT_FALSE(result); + + ds.clearGraph(); +} diff --git a/nav2_route/test/test_goal_intent_extractor.cpp b/nav2_route/test/test_goal_intent_extractor.cpp index 5435764b8e..92ed00e346 100644 --- a/nav2_route/test/test_goal_intent_extractor.cpp +++ b/nav2_route/test/test_goal_intent_extractor.cpp @@ -88,11 +88,11 @@ TEST(GoalIntentExtractorTest, test_transform_pose) // Test transformations same frame, should pass geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = "map"; - EXPECT_NO_THROW(extractor.transformPose(pose)); + EXPECT_NO_THROW(extractor.transformPose(pose, "map")); // Test transformations when nothing on TF buffer of different frames pose.header.frame_id = "gps"; - EXPECT_THROW(extractor.transformPose(pose), nav2_core::RouteTFError); + EXPECT_THROW(extractor.transformPose(pose, "map"), nav2_core::RouteTFError); // Now transforms are available, should work geometry_msgs::msg::TransformStamped transform; @@ -100,7 +100,7 @@ TEST(GoalIntentExtractorTest, test_transform_pose) transform.header.stamp = node->now(); transform.child_frame_id = "gps"; broadcaster.sendTransform(transform); - EXPECT_NO_THROW(extractor.transformPose(pose)); + EXPECT_NO_THROW(extractor.transformPose(pose, "map")); } TEST(GoalIntentExtractorTest, test_start_goal_finder) diff --git a/nav2_route/test/test_spatial_tree.cpp b/nav2_route/test/test_spatial_tree.cpp index 8f8aa548e7..d9363026ac 100644 --- a/nav2_route/test/test_spatial_tree.cpp +++ b/nav2_route/test/test_spatial_tree.cpp @@ -38,19 +38,22 @@ TEST(NodeSpatialTreeTest, test_kd_tree) } // Compute our kd tree for this graph - std::shared_ptr kd_tree = std::make_shared(); + int num_of_nearest_nodes = 1; + std::shared_ptr kd_tree = + std::make_shared(num_of_nearest_nodes); + kd_tree->computeTree(graph); // Test a bunch of times to find the nearest neighbor to this node // By checking for the idx in the Kd-tree and then brute force searching unsigned int num_tests = 50; for (unsigned int i = 0; i != num_tests; i++) { - unsigned int kd_tree_idx; + std::vector kd_tree_idx; geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = static_cast((rand_r(&seed) % 6000000) + 1); pose.pose.position.y = static_cast((rand_r(&seed) % 6000000) + 1); - if (!kd_tree->findNearestGraphNodeToPose(pose, kd_tree_idx)) { + if (!kd_tree->findNearestGraphNodesToPose(pose, kd_tree_idx)) { EXPECT_TRUE(false); // Unable to find nearest neighbor! } @@ -66,6 +69,6 @@ TEST(NodeSpatialTreeTest, test_kd_tree) } } - EXPECT_EQ(kd_tree_idx, closest_via_brute_force); + EXPECT_EQ(kd_tree_idx.front(), closest_via_brute_force); } } diff --git a/nav2_simple_commander/nav2_simple_commander/example_route.py b/nav2_simple_commander/nav2_simple_commander/example_route.py index f2f61263c9..d5de59485c 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_route.py +++ b/nav2_simple_commander/nav2_simple_commander/example_route.py @@ -16,6 +16,7 @@ from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult import rclpy +import time from rclpy.duration import Duration """ @@ -68,6 +69,8 @@ def main(): goal_pose.pose.position.y = -5.42 goal_pose.pose.orientation.w = 1.0 + while(True): + # Sanity check a valid route exists using PoseStamped. # May also use NodeIDs on the graph if they are known by passing them instead as `int` # [path, route] = navigator.getRoute(initial_pose, goal_pose)