From b792e5b52de93732af58227d7dfd7c5b1ea1d40f Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 27 Mar 2023 12:10:11 -0400 Subject: [PATCH 01/36] initial structure --- nav2_route/CMakeLists.txt | 21 +- .../graphs/scripts/export_shapefiles.py | 8 +- .../nav2_route/breadth_first_search.hpp | 18 ++ nav2_route/include/nav2_route/node.hpp | 103 +++++++ nav2_route/src/breadth_first_search.cpp | 21 ++ nav2_route/src/node.cpp | 71 +++++ nav2_route/test/CMakeLists.txt | 289 +++++++++--------- nav2_route/test/performance_benchmarking.cpp | 11 +- nav2_route/test/test_node.cpp | 32 ++ nav2_system_tests/COLCON_IGNORE | 0 10 files changed, 424 insertions(+), 150 deletions(-) create mode 100644 nav2_route/include/nav2_route/breadth_first_search.hpp create mode 100644 nav2_route/include/nav2_route/node.hpp create mode 100644 nav2_route/src/breadth_first_search.cpp create mode 100644 nav2_route/src/node.cpp create mode 100644 nav2_route/test/test_node.cpp create mode 100644 nav2_system_tests/COLCON_IGNORE diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index f4a4a477db9..34cb07cbcc6 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -111,6 +111,15 @@ ament_target_dependencies(graph_file_loaders pluginlib_export_plugin_description_file(nav2_route plugins.xml) +# Breadth first search library +add_library(breadth_first_search SHARED + src/breadth_first_search.cpp + src/node.cpp) + +ament_target_dependencies(breadth_first_search + ${dependencies} +) + install(DIRECTORY include/ DESTINATION include/ ) @@ -119,7 +128,11 @@ 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 + breadth_first_search ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -138,5 +151,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() diff --git a/nav2_route/graphs/scripts/export_shapefiles.py b/nav2_route/graphs/scripts/export_shapefiles.py index 4addaac6ca4..47f5a682f61 100644 --- a/nav2_route/graphs/scripts/export_shapefiles.py +++ b/nav2_route/graphs/scripts/export_shapefiles.py @@ -3,11 +3,11 @@ import sys from datetime import datetime -try: - file_prefix = sys.argv[1] +try: + file_prefix = sys.argv[1] edges = gpd.read_file(sys.argv[2]) nodes = gpd.read_file(sys.argv[3]) -except: +except IndexError: raise Exception("Incorrect arguements provided") now = datetime.now() @@ -16,4 +16,4 @@ file_name = file_prefix + "_" + now.strftime("%m_%d_%Y_%H_%M_%S") + ".geojson" -graph.to_file(file_name, driver='GeoJSON') \ No newline at end of file +graph.to_file(file_name, driver='GeoJSON') diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp new file mode 100644 index 00000000000..450488409f9 --- /dev/null +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -0,0 +1,18 @@ +// 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_ + +#endif // NAV2_ROUTE__BREADTH_FIRST_SEARCH_HPP_ diff --git a/nav2_route/include/nav2_route/node.hpp b/nav2_route/include/nav2_route/node.hpp new file mode 100644 index 00000000000..5bf02f19139 --- /dev/null +++ b/nav2_route/include/nav2_route/node.hpp @@ -0,0 +1,103 @@ +// 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__NODE_HPP_ +#define NAV2_ROUTE__NODE_HPP_ + +#include +namespace nav2_route +{ + +class Node +{ +public: + typedef Node * NodePtr; + typedef std::vector NodeVector; + + struct Coordinates + { + Coordinates(const float & x_in, const float & y_in) + : x(x_in), y(y_in) + {} + + float x, y; + }; + typedef std::vector CoordinateVector; + + explicit Node(const unsigned int index); + + ~Node(); + + inline bool & wasVisited() + { + return visited_; + } + + inline void visit() + { + visited_ = true; + queued_ = false; + } + + inline bool & isQueued() + { + return queued_; + } + + inline void queue() + { + queued_ = true; + } + + inline unsigned int & getIndex() + { + return index_; + } + + + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & width) + { + return x + y * width; + } + + static inline Coordinates getCoords( + const unsigned int & index) + { + // Unsigned and sign ints... bad. Get andrew to take a look + const unsigned int & width = neighbors_grid_offsets[3]; + return {static_cast(index % width), static_cast(index / width)}; + } + + + void initMotionModel(int x_size); + + bool isNodeValid(); + + void getNeighbors(NodeVector & neighbors); + + bool backtracePath(CoordinateVector & path); + + + NodePtr parent; + static std::vector neighbors_grid_offsets; + +private: + unsigned int index_; + bool visited_; + bool queued_; +}; + +} // namespace nav2_route +#endif // NAV2_ROUTE__NODE_HPP_ diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp new file mode 100644 index 00000000000..3451c1c7e82 --- /dev/null +++ b/nav2_route/src/breadth_first_search.cpp @@ -0,0 +1,21 @@ +// 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/breadth_first_search.hpp" +#include "nav2_route/node.hpp" + +namespace nav2_route +{ + +} // namespace nav2_route diff --git a/nav2_route/src/node.cpp b/nav2_route/src/node.cpp new file mode 100644 index 00000000000..9ef1ecbb5e9 --- /dev/null +++ b/nav2_route/src/node.cpp @@ -0,0 +1,71 @@ +// 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/node.hpp" + +namespace nav2_route +{ + +// Defining static members to be shared +std::vector Node::neighbors_grid_offsets; + + +Node::Node(const unsigned int index) +: parent(nullptr), + index_(index), + visited_(false), + queued_(false) +{} + +Node::~Node() +{ + parent = nullptr; +} + +void Node::initMotionModel(int x_size) +{ + neighbors_grid_offsets = {-1, +1, + -x_size, +x_size, + -x_size - 1, -x_size + 1, + +x_size - 1, +x_size + 1}; +} + +bool Node::isNodeValid() +{ + return true; +} + +void Node::getNeighbors(nav2_route::Node::NodeVector &) +{} + +bool Node::backtracePath(nav2_route::Node::CoordinateVector & path) +{ + if (!this->parent) { + return false; + } + + NodePtr current_node = this; + + while (current_node->parent) { + path.push_back(Node::getCoords(current_node->getIndex())); + } + + // add the start pose + path.push_back(Node::getCoords(current_node->getIndex())); + + return true; +} + +} // namespace nav2_route diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt index df2c544d9e3..a1cfef15572 100644 --- a/nav2_route/test/CMakeLists.txt +++ b/nav2_route/test/CMakeLists.txt @@ -1,144 +1,155 @@ -# Route planner benchmarking script -add_executable(performance_benchmarking performance_benchmarking.cpp) -target_link_libraries(performance_benchmarking - ${library_name} -) -ament_target_dependencies(performance_benchmarking - ${dependencies} -) -install(TARGETS - performance_benchmarking - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -# Test utilities and basic types -ament_add_gtest(test_utils_and_types - test_utils_and_types.cpp -) -ament_target_dependencies(test_utils_and_types - ${dependencies} -) -target_link_libraries(test_utils_and_types - ${library_name} -) - -# Test edge scorer + plugins -ament_add_gtest(test_edge_scorers - test_edge_scorers.cpp -) -ament_target_dependencies(test_edge_scorers - ${dependencies} -) -target_link_libraries(test_edge_scorers - ${library_name} edge_scorers -) - -# Test path converter -ament_add_gtest(test_path_converter - test_path_converter.cpp -) -ament_target_dependencies(test_path_converter - ${dependencies} -) -target_link_libraries(test_path_converter - ${library_name} -) - -# Test node spatial tree -ament_add_gtest(test_spatial_tree - test_spatial_tree.cpp -) -ament_target_dependencies(test_spatial_tree - ${dependencies} -) -target_link_libraries(test_spatial_tree - ${library_name} -) - -# Test operation manager + plugins -ament_add_gtest(test_operations - test_operations.cpp -) -ament_target_dependencies(test_operations - ${dependencies} -) -target_link_libraries(test_operations - ${library_name} route_operations -) +## Route planner benchmarking script +#add_executable(performance_benchmarking performance_benchmarking.cpp) +#target_link_libraries(performance_benchmarking +# ${library_name} +#) +#ament_target_dependencies(performance_benchmarking +# ${dependencies} +#) +#install(TARGETS +# performance_benchmarking +# RUNTIME DESTINATION lib/${PROJECT_NAME} +#) +# +## Test utilities and basic types +#ament_add_gtest(test_utils_and_types +# test_utils_and_types.cpp +#) +#ament_target_dependencies(test_utils_and_types +# ${dependencies} +#) +#target_link_libraries(test_utils_and_types +# ${library_name} +#) +# +## Test edge scorer + plugins +#ament_add_gtest(test_edge_scorers +# test_edge_scorers.cpp +#) +#ament_target_dependencies(test_edge_scorers +# ${dependencies} +#) +#target_link_libraries(test_edge_scorers +# ${library_name} edge_scorers +#) +# +## Test path converter +#ament_add_gtest(test_path_converter +# test_path_converter.cpp +#) +#ament_target_dependencies(test_path_converter +# ${dependencies} +#) +#target_link_libraries(test_path_converter +# ${library_name} +#) +# +## Test node spatial tree +#ament_add_gtest(test_spatial_tree +# test_spatial_tree.cpp +#) +#ament_target_dependencies(test_spatial_tree +# ${dependencies} +#) +#target_link_libraries(test_spatial_tree +# ${library_name} +#) +# +## Test operation manager + plugins +#ament_add_gtest(test_operations +# test_operations.cpp +#) +#ament_target_dependencies(test_operations +# ${dependencies} +#) +#target_link_libraries(test_operations +# ${library_name} route_operations +#) +# +## Test graph loader +#ament_add_gtest(test_graph_loader +# test_graph_loader.cpp +#) +#ament_target_dependencies(test_graph_loader +# ${dependencies} +#) +#target_link_libraries(test_graph_loader +# ${library_name} +#) +# +## Test geojson parser +#ament_add_gtest(test_geojson_graph_file_loader +# test_geojson_graph_file_loader.cpp +#) +#ament_target_dependencies(test_geojson_graph_file_loader +# ${dependencies} +#) +#target_link_libraries(test_geojson_graph_file_loader +# ${library_name} graph_file_loaders +#) +# +## Test collision monitor seperately due to relative complexity +#ament_add_gtest(test_collision_operation +# test_collision_operation.cpp +#) +#ament_target_dependencies(test_collision_operation +# ${dependencies} +#) +#target_link_libraries(test_collision_operation +# ${library_name} route_operations +#) +# +## Test route planner +#ament_add_gtest(test_route_planner +# test_route_planner.cpp +#) +#ament_target_dependencies(test_route_planner +# ${dependencies} +#) +#target_link_libraries(test_route_planner +# ${library_name} edge_scorers +#) +# +## Test goal intent extractor +#ament_add_gtest(test_goal_intent_extractor +# test_goal_intent_extractor.cpp +#) +#ament_target_dependencies(test_goal_intent_extractor +# ${dependencies} +#) +#target_link_libraries(test_goal_intent_extractor +# ${library_name} +#) +# +## Test route tracker +#ament_add_gtest(test_route_tracker +# test_route_tracker.cpp +#) +#ament_target_dependencies(test_route_tracker +# ${dependencies} +#) +#target_link_libraries(test_route_tracker +# ${library_name} +#) +# +## Test route server +#ament_add_gtest(test_route_server +# test_route_server.cpp +#) +#ament_target_dependencies(test_route_server +# ${dependencies} +#) +#target_link_libraries(test_route_server +# ${library_name} +#) -# Test graph loader -ament_add_gtest(test_graph_loader - test_graph_loader.cpp +# Test breadth first search node +ament_add_gtest(test_node + test_node.cpp ) -ament_target_dependencies(test_graph_loader +ament_target_dependencies(test_node ${dependencies} ) -target_link_libraries(test_graph_loader +target_link_libraries(test_node ${library_name} -) - -# Test geojson parser -ament_add_gtest(test_geojson_graph_file_loader - test_geojson_graph_file_loader.cpp -) -ament_target_dependencies(test_geojson_graph_file_loader - ${dependencies} -) -target_link_libraries(test_geojson_graph_file_loader - ${library_name} graph_file_loaders -) - -# Test collision monitor seperately due to relative complexity -ament_add_gtest(test_collision_operation - test_collision_operation.cpp -) -ament_target_dependencies(test_collision_operation - ${dependencies} -) -target_link_libraries(test_collision_operation - ${library_name} route_operations -) - -# Test route planner -ament_add_gtest(test_route_planner - test_route_planner.cpp -) -ament_target_dependencies(test_route_planner - ${dependencies} -) -target_link_libraries(test_route_planner - ${library_name} edge_scorers -) - -# Test goal intent extractor -ament_add_gtest(test_goal_intent_extractor - test_goal_intent_extractor.cpp -) -ament_target_dependencies(test_goal_intent_extractor - ${dependencies} -) -target_link_libraries(test_goal_intent_extractor - ${library_name} -) - -# Test route tracker -ament_add_gtest(test_route_tracker - test_route_tracker.cpp -) -ament_target_dependencies(test_route_tracker - ${dependencies} -) -target_link_libraries(test_route_tracker - ${library_name} -) - -# Test route server -ament_add_gtest(test_route_server - test_route_server.cpp -) -ament_target_dependencies(test_route_server - ${dependencies} -) -target_link_libraries(test_route_server - ${library_name} -) +) \ No newline at end of file diff --git a/nav2_route/test/performance_benchmarking.cpp b/nav2_route/test/performance_benchmarking.cpp index a4a9682c0b1..c123e2557b0 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++; @@ -118,7 +118,8 @@ int main(int argc, char const * argv[]) // node->get_logger(), // "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 + // 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(); kd_tree->computeTree(graph); @@ -126,8 +127,8 @@ int main(int argc, char const * argv[]) for (unsigned int i = 0; i != NUM_TESTS; i++) { unsigned int kd_tree_idx; geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = static_cast(rand() % DIM); - pose.pose.position.y = static_cast(rand() % DIM); + pose.pose.position.x = static_cast(rand_r() % DIM); + pose.pose.position.y = static_cast(rand_r() % DIM); kd_tree->findNearestGraphNodeToPose(pose, kd_tree_idx); } auto end = node->now(); diff --git a/nav2_route/test/test_node.cpp b/nav2_route/test/test_node.cpp new file mode 100644 index 00000000000..66580f642d9 --- /dev/null +++ b/nav2_route/test/test_node.cpp @@ -0,0 +1,32 @@ +// Copyright (c) 2023 Samsung Research America +// +// 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_route/node.hpp" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + + +TEST(test_node, node_test) +{ + EXPECT_TRUE(true); +} diff --git a/nav2_system_tests/COLCON_IGNORE b/nav2_system_tests/COLCON_IGNORE new file mode 100644 index 00000000000..e69de29bb2d From 9b88d4da36e3b057de793da9f5e09bcf568fa79f Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 27 Mar 2023 13:50:40 -0400 Subject: [PATCH 02/36] test simple node methods --- nav2_route/include/nav2_route/node.hpp | 2 +- nav2_route/test/CMakeLists.txt | 2 +- nav2_route/test/test_node.cpp | 32 ++++++++++++++++++++++++-- 3 files changed, 32 insertions(+), 4 deletions(-) diff --git a/nav2_route/include/nav2_route/node.hpp b/nav2_route/include/nav2_route/node.hpp index 5bf02f19139..0f3ecf81933 100644 --- a/nav2_route/include/nav2_route/node.hpp +++ b/nav2_route/include/nav2_route/node.hpp @@ -81,7 +81,7 @@ class Node } - void initMotionModel(int x_size); + static void initMotionModel(int x_size); bool isNodeValid(); diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt index a1cfef15572..c23b38fb666 100644 --- a/nav2_route/test/CMakeLists.txt +++ b/nav2_route/test/CMakeLists.txt @@ -151,5 +151,5 @@ ament_target_dependencies(test_node ${dependencies} ) target_link_libraries(test_node - ${library_name} + ${library_name} breadth_first_search ) \ No newline at end of file diff --git a/nav2_route/test/test_node.cpp b/nav2_route/test/test_node.cpp index 66580f642d9..48940c90440 100644 --- a/nav2_route/test/test_node.cpp +++ b/nav2_route/test/test_node.cpp @@ -15,6 +15,9 @@ #include #include +#include + + #include "nav2_route/node.hpp" class RclCppFixture @@ -25,8 +28,33 @@ class RclCppFixture }; RclCppFixture g_rclcppfixture; - -TEST(test_node, node_test) +namespace nav2_route { + +TEST(test_node, node_test) { + Node::initMotionModel(10); + + // Check State visiting + Node node(10); + + EXPECT_FALSE(node.wasVisited()); + EXPECT_FALSE(node.isQueued()); + + node.queue(); + EXPECT_TRUE(node.isQueued()); + EXPECT_FALSE(node.wasVisited()); + + node.visit(); + EXPECT_TRUE(node.wasVisited()); + EXPECT_FALSE(node.isQueued()); + + // Check static index functions + EXPECT_EQ(Node::getIndex(1u, 1u, 10u), 11u); + EXPECT_EQ(Node::getIndex(6u, 42, 10u), 426u); + EXPECT_EQ(Node::getCoords(10u).x, 0.0); + EXPECT_EQ(Node::getCoords(10u).y, 1.0); + EXPECT_TRUE(true); } + +} // namespace nav2_route From 3ccc3296ab85e0e8b60c8f8875172bc49b6997c3 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 2 Apr 2023 18:19:28 -0400 Subject: [PATCH 03/36] get neighbors structure --- nav2_bringup/launch/bringup_launch.py | 4 +- nav2_bringup/launch/tb3_simulation_launch.py | 2 +- nav2_route/CMakeLists.txt | 1 + .../nav2_route/breadth_first_search.hpp | 50 ++++++++++++++ .../include/nav2_route/collision_checker.hpp | 19 +++++ nav2_route/include/nav2_route/node.hpp | 69 ++++++++++--------- nav2_route/src/breadth_first_search.cpp | 35 ++++++++++ nav2_route/src/collision_checker.cpp | 13 ++++ nav2_route/src/node.cpp | 37 ++++++++-- 9 files changed, 188 insertions(+), 42 deletions(-) create mode 100644 nav2_route/include/nav2_route/collision_checker.hpp create mode 100644 nav2_route/src/collision_checker.cpp diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 8fddcc32531..aa64fe9443d 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -113,9 +113,9 @@ def generate_launch_description(): PushRosNamespace( condition=IfCondition(use_namespace), namespace=namespace), - + PushNodeRemapping Node( - condition=IfCondition(use_composition), + condition=IfCondition(use_composition),: name='nav2_container', package='rclcpp_components', executable='component_container_isolated', diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index b4dfd3bd167..36dfcff5d07 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -134,7 +134,7 @@ def generate_launch_description(): declare_simulator_cmd = DeclareLaunchArgument( 'headless', - default_value='True', + default_value='False', description='Whether to execute gzclient)') declare_world_cmd = DeclareLaunchArgument( diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index 34cb07cbcc6..6811dfb71b6 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -113,6 +113,7 @@ pluginlib_export_plugin_description_file(nav2_route plugins.xml) # Breadth first search library add_library(breadth_first_search SHARED + src/collision_checker.cpp src/breadth_first_search.cpp src/node.cpp) diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index 450488409f9..963c8bd0193 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -15,4 +15,54 @@ #ifndef NAV2_ROUTE__BREADTH_FIRST_SEARCH_HPP_ #define NAV2_ROUTE__BREADTH_FIRST_SEARCH_HPP_ + +#include +#include +#include + +#include "nav2_route/node.hpp" + +namespace nav2_route +{ + +class BreadthFirstSearch +{ +public: + typedef std::unordered_map Graph; + typedef Node::NodePtr NodePtr; + typedef Node::Coordinates Coordinates; + typedef Node::CoordinateVector CoordinateVector; + typedef std::queue NodeQueue; + + BreadthFirstSearch(); + + void setStart( + const unsigned int & mx, + const unsigned int & my); + + // TODO(jwallace): should pass a list of goals + void setGoal( + const unsigned int & mx, + const unsigned int & my); + + + Coordinates search(const Coordinates & start, const Coordinates & goal); + + void initialize(unsigned int x_size, unsigned int y_size); + +private: + inline bool isGoal(NodePtr & node); + + Graph graph_; + NodePtr start_; + NodePtr goal_; + NodeQueue queue_; + + unsigned int x_size_; + unsigned int y_size_; + + NodePtr addToGraph(const unsigned int & index); +}; +} // namespace nav2_route + #endif // NAV2_ROUTE__BREADTH_FIRST_SEARCH_HPP_ diff --git a/nav2_route/include/nav2_route/collision_checker.hpp b/nav2_route/include/nav2_route/collision_checker.hpp new file mode 100644 index 00000000000..24ce8aa48db --- /dev/null +++ b/nav2_route/include/nav2_route/collision_checker.hpp @@ -0,0 +1,19 @@ +#include + + +#ifndef NAV2_ROUTE__COLLISION_MONITOR_HPP_ +#define NAV2_ROUTE__COLLISION_MONITOR_HPP_ + +namespace nav2_route { + +class CollisionChecker +{ +public: + + CollisionChecker(nav2_costmap_2d::Costmap2D * costmap); + + bool inCollision(const unsigned int & i, const bool traverse_unknown); +}; +} // namepsace nav2_route + +#endif // NAV2_ROUTE__COLLISION_MONITOR_HPP_ \ No newline at end of file diff --git a/nav2_route/include/nav2_route/node.hpp b/nav2_route/include/nav2_route/node.hpp index 0f3ecf81933..0ed1921183a 100644 --- a/nav2_route/include/nav2_route/node.hpp +++ b/nav2_route/include/nav2_route/node.hpp @@ -16,20 +16,23 @@ #define NAV2_ROUTE__NODE_HPP_ #include -namespace nav2_route -{ +#include -class Node -{ -public: - typedef Node * NodePtr; +#include + +namespace nav2_route { + +class Node { + public: + typedef Node *NodePtr; typedef std::vector NodeVector; + typedef std::function NodeGetter; - struct Coordinates - { - Coordinates(const float & x_in, const float & y_in) - : x(x_in), y(y_in) - {} + struct Coordinates { + Coordinates() {} + + Coordinates(const float &x_in, const float &y_in) + : x(x_in), y(y_in) {} float x, y; }; @@ -39,65 +42,63 @@ class Node ~Node(); - inline bool & wasVisited() - { + bool operator==(NodePtr &rhs) const { + return this->index_ == rhs->index_; + } + + inline bool &wasVisited() { return visited_; } - inline void visit() - { + inline void visit() { visited_ = true; queued_ = false; } - inline bool & isQueued() - { + inline bool &isQueued() { return queued_; } - inline void queue() - { + inline void queue() { queued_ = true; } - inline unsigned int & getIndex() - { + inline unsigned int &getIndex() { return index_; } - static inline unsigned int getIndex( - const unsigned int & x, const unsigned int & y, const unsigned int & width) - { + const unsigned int &x, const unsigned int &y, const unsigned int &width) { return x + y * width; } static inline Coordinates getCoords( - const unsigned int & index) - { + const unsigned int &index) { // Unsigned and sign ints... bad. Get andrew to take a look - const unsigned int & width = neighbors_grid_offsets[3]; + const unsigned int &width = neighbors_grid_offsets[3]; return {static_cast(index % width), static_cast(index / width)}; } - static void initMotionModel(int x_size); - bool isNodeValid(); - - void getNeighbors(NodeVector & neighbors); + bool isNodeValid(CollisionChecker *collision_checker, const bool &traverse_unknown); - bool backtracePath(CoordinateVector & path); + void getNeighbors( + NodeGetter &node_getter, + CollisionChecker *collision_checker, + const bool &traverse_unknown, + NodeVector &neighbors); + bool backtracePath(CoordinateVector &path); NodePtr parent; static std::vector neighbors_grid_offsets; -private: + private: unsigned int index_; bool visited_; bool queued_; }; -} // namespace nav2_route +} // namespace nav2_route #endif // NAV2_ROUTE__NODE_HPP_ diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 3451c1c7e82..656d4cc8c1e 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -18,4 +18,39 @@ namespace nav2_route { +BreadthFirstSearch::BreadthFirstSearch() +: start_(nullptr), + goal_(nullptr), + x_size_(0), + y_size_(0) +{} + + +void BreadthFirstSearch::setStart(const unsigned int & mx, const unsigned int & my) +{ + start_ = addToGraph(Node::getIndex(mx, my, x_size_)); +} + +void BreadthFirstSearch::setGoal(const unsigned int & mx, const unsigned int & my) +{ + goal_ = addToGraph(Node::getIndex(mx, my, x_size_)); +} + +BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int & index) +{ + auto iter = graph_.find(index); + + if (iter != graph_.end()) { + return &(iter->second); + } + + return &(graph_.emplace(index, Node(index)).first->second); +} + +void BreadthFirstSearch::initialize(unsigned int x_size, unsigned int y_size) +{ + x_size_ = x_size; + y_size_ = y_size; +} + } // namespace nav2_route diff --git a/nav2_route/src/collision_checker.cpp b/nav2_route/src/collision_checker.cpp new file mode 100644 index 00000000000..9adba388040 --- /dev/null +++ b/nav2_route/src/collision_checker.cpp @@ -0,0 +1,13 @@ +#include + +namespace nav2_route +{ +CollisionChecker::CollisionChecker(nav2_costmap_2d::Costmap2D *) +{ +} + +bool CollisionChecker::inCollision(const unsigned int & , const bool ) +{ + return false; +} +} diff --git a/nav2_route/src/node.cpp b/nav2_route/src/node.cpp index 9ef1ecbb5e9..2127a662409 100644 --- a/nav2_route/src/node.cpp +++ b/nav2_route/src/node.cpp @@ -42,13 +42,40 @@ void Node::initMotionModel(int x_size) +x_size - 1, +x_size + 1}; } -bool Node::isNodeValid() +void Node::getNeighbors( + NodeGetter & node_getter, + CollisionChecker* collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors) { - return true; + int index; + NodePtr neighbor; + int node_i = this->getIndex(); + const Coordinates parent = getCoords(node_i); + Coordinates child; + + for(unsigned int i = 0; i != neighbors_grid_offsets.size(); ++i) + { + index = node_i + i; + child = getCoords(index); + + // Check for wrap around conditions + if (fabs(parent.x - child.x) > 1 || fabs(parent.y - child.y) > 1) + continue; + + if (node_getter(index, neighbor)) { + if (neighbor->isNodeValid(collision_checker, traverse_unknown) && !neighbor->wasVisited()) { + neighbors.push_back(neighbor); + } + } + } } -void Node::getNeighbors(nav2_route::Node::NodeVector &) -{} + +bool Node::isNodeValid(CollisionChecker * collision_checker, const bool &traverse_unknown) +{ + return collision_checker->inCollision(getIndex(), traverse_unknown); +} bool Node::backtracePath(nav2_route::Node::CoordinateVector & path) { @@ -68,4 +95,4 @@ bool Node::backtracePath(nav2_route::Node::CoordinateVector & path) return true; } -} // namespace nav2_route +} From 8f373af7be3bd63a3af6858472bf81ef0b590535 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 20 Apr 2023 16:09:01 -0400 Subject: [PATCH 04/36] finished search algorthim --- .../nav2_route/breadth_first_search.hpp | 74 ++++++++++-- .../include/nav2_route/collision_checker.hpp | 9 +- nav2_route/include/nav2_route/node.hpp | 15 ++- nav2_route/src/breadth_first_search.cpp | 106 ++++++++++++++++-- nav2_route/src/collision_checker.cpp | 23 +++- nav2_route/src/node.cpp | 52 +++++---- nav2_route/test/test_node.cpp | 73 +++++++++++- 7 files changed, 297 insertions(+), 55 deletions(-) diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index 963c8bd0193..01ea1f0b45d 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -30,38 +30,94 @@ class BreadthFirstSearch public: typedef std::unordered_map Graph; typedef Node::NodePtr NodePtr; + typedef Node::NodeVector NodeVector; + typedef Node::NodeVector::iterator NeighborIterator; typedef Node::Coordinates Coordinates; typedef Node::CoordinateVector CoordinateVector; + typedef Node::NodeGetter NodeGetter; typedef std::queue NodeQueue; - BreadthFirstSearch(); + /** + * @brief A constructor for nav2_route::BreadthFirstSearch + */ + BreadthFirstSearch() = default; + /** + * @brief Set the starting pose for the search + * @param mx The node X index of the start + * @param my The node Y index of the start + */ void setStart( const unsigned int & mx, const unsigned int & my); - // TODO(jwallace): should pass a list of goals + /** + * @brief Set the goal for the search + * @param mx The node X index of the start + * @param my The node Y index of the start + */ void setGoal( const unsigned int & mx, const unsigned int & my); - - Coordinates search(const Coordinates & start, const Coordinates & goal); + /** + * @brief Create a path from the given costmap, start and goal + * @param path The output path if the search was successful + * @return True if a plan was successfully calculated + */ + bool search(CoordinateVector & path); void initialize(unsigned int x_size, unsigned int y_size); + void setCollisionChecker(CollisionChecker * collision_checker); + private: + + /** + * @brief Checks if node is the goal node + * @param node The node to check + * @return True if the node is the goal node + */ inline bool isGoal(NodePtr & node); + /** + * @brief Adds a node to the graph + * @param index The index of the node to add + * @return The node added to the graph + */ + NodePtr addToGraph(const unsigned int & index); + + /** + * @brief Adds a node to the queue + * @param node The node to add + */ + void addToQueue(NodePtr & node); + + /** + * @brief Gets the next node in the queue + * @return The next node in the queue + */ + NodePtr getNextNode(); + + /** + * @brief Clear the queue + */ + void clearQueue(); + + /** + * @brief Clear the graph + */ + void clearGraph(); + Graph graph_; - NodePtr start_; - NodePtr goal_; + NodePtr start_{nullptr}; + NodePtr goal_{nullptr}; NodeQueue queue_; - unsigned int x_size_; - unsigned int y_size_; + unsigned int x_size_{0}; + unsigned int y_size_{0}; - NodePtr addToGraph(const unsigned int & index); + CollisionChecker *collision_checker_{nullptr}; }; } // namespace nav2_route diff --git a/nav2_route/include/nav2_route/collision_checker.hpp b/nav2_route/include/nav2_route/collision_checker.hpp index 24ce8aa48db..393b9925f8b 100644 --- a/nav2_route/include/nav2_route/collision_checker.hpp +++ b/nav2_route/include/nav2_route/collision_checker.hpp @@ -12,8 +12,13 @@ class CollisionChecker CollisionChecker(nav2_costmap_2d::Costmap2D * costmap); - bool inCollision(const unsigned int & i, const bool traverse_unknown); + bool inCollision(const unsigned int & i, const bool traverse_unknown); + + nav2_costmap_2d::Costmap2D * getCostmap(); +private: + + nav2_costmap_2d::Costmap2D * costmap_; }; } // namepsace nav2_route -#endif // NAV2_ROUTE__COLLISION_MONITOR_HPP_ \ No newline at end of file +#endif // NAV2_ROUTE__COLLISION_MONITOR_HPP_ diff --git a/nav2_route/include/nav2_route/node.hpp b/nav2_route/include/nav2_route/node.hpp index 0ed1921183a..0bec3194977 100644 --- a/nav2_route/include/nav2_route/node.hpp +++ b/nav2_route/include/nav2_route/node.hpp @@ -26,7 +26,7 @@ class Node { public: typedef Node *NodePtr; typedef std::vector NodeVector; - typedef std::function NodeGetter; + typedef std::function NodeGetter; struct Coordinates { Coordinates() {} @@ -75,11 +75,11 @@ class Node { static inline Coordinates getCoords( const unsigned int &index) { // Unsigned and sign ints... bad. Get andrew to take a look - const unsigned int &width = neighbors_grid_offsets[3]; + const unsigned int &width = x_size_; return {static_cast(index % width), static_cast(index / width)}; } - static void initMotionModel(int x_size); + static void initMotionModel(int x_size, int y_size); bool isNodeValid(CollisionChecker *collision_checker, const bool &traverse_unknown); @@ -91,13 +91,16 @@ class Node { bool backtracePath(CoordinateVector &path); - NodePtr parent; + NodePtr parent{nullptr}; static std::vector neighbors_grid_offsets; private: unsigned int index_; - bool visited_; - bool queued_; + bool visited_{false}; + bool queued_{false}; + + static unsigned int x_size_; + static unsigned int max_index_; }; } // namespace nav2_route diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 656d4cc8c1e..497db774368 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -18,14 +18,6 @@ namespace nav2_route { -BreadthFirstSearch::BreadthFirstSearch() -: start_(nullptr), - goal_(nullptr), - x_size_(0), - y_size_(0) -{} - - void BreadthFirstSearch::setStart(const unsigned int & mx, const unsigned int & my) { start_ = addToGraph(Node::getIndex(mx, my, x_size_)); @@ -49,8 +41,106 @@ BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int & void BreadthFirstSearch::initialize(unsigned int x_size, unsigned int y_size) { + // Add in iterations and max time parameters x_size_ = x_size; y_size_ = y_size; } +void BreadthFirstSearch::setCollisionChecker(nav2_route::CollisionChecker *collision_checker) +{ + collision_checker_ = collision_checker; + unsigned int x_size = collision_checker_->getCostmap()->getSizeInCellsX(); + unsigned int y_size = collision_checker->getCostmap()->getSizeInCellsY(); + + clearGraph(); + + if (x_size_ != x_size || y_size_ != y_size) { + x_size_ = x_size; + y_size_ = y_size; + Node::initMotionModel(static_cast(x_size_), static_cast(y_size_)); + } +} + +bool BreadthFirstSearch::search(CoordinateVector & path) +{ + clearQueue(); + + NodePtr current_node = nullptr; + NodePtr neighbor = nullptr; + NeighborIterator neighbor_iterator; + NodeVector neighbors; + + const unsigned int max_index = x_size_ * y_size_; + NodeGetter node_getter = + [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool + { + if(index >= max_index) { + return false; + } + + neighbor_rtn = addToGraph(index); + return true; + }; + + addToQueue(start_); + while(!queue_.empty()) { + current_node = getNextNode(); + + current_node->visit(); + + if(isGoal(current_node)) { + return current_node->backtracePath(path); + } + + neighbors.clear(); + neighbor = nullptr; + + current_node->getNeighbors(node_getter, collision_checker_, false, neighbors); + + for (neighbor_iterator = neighbors.begin(); + neighbor_iterator != neighbors.end(); ++neighbor_iterator) + { + neighbor = *neighbor_iterator; + + if (!neighbor->wasVisited() || !neighbor->isQueued()) { + neighbor->parent = current_node; + addToQueue(neighbor); + } + } + } + return false; +} + +void BreadthFirstSearch::addToQueue(NodePtr &node) { + if(node->isQueued()) { + return; + } + + node->queue(); + queue_.emplace(node); +} + +BreadthFirstSearch::NodePtr BreadthFirstSearch::getNextNode() +{ + NodePtr next = queue_.front(); + queue_.pop(); + return next; +} + +bool BreadthFirstSearch::isGoal(NodePtr &node) { + return node == goal_; +} + +void BreadthFirstSearch::clearQueue() +{ + NodeQueue q; + std::swap(queue_, q); +} + +void BreadthFirstSearch::clearGraph() +{ + Graph g; + std::swap(graph_, g); +} + } // namespace nav2_route diff --git a/nav2_route/src/collision_checker.cpp b/nav2_route/src/collision_checker.cpp index 9adba388040..edd093b1c9b 100644 --- a/nav2_route/src/collision_checker.cpp +++ b/nav2_route/src/collision_checker.cpp @@ -1,13 +1,28 @@ -#include +#include "nav2_route/collision_checker.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include namespace nav2_route { -CollisionChecker::CollisionChecker(nav2_costmap_2d::Costmap2D *) +CollisionChecker::CollisionChecker(nav2_costmap_2d::Costmap2D *costmap) { + costmap_ = costmap; } -bool CollisionChecker::inCollision(const unsigned int & , const bool ) +bool CollisionChecker::inCollision(const unsigned int & i , const bool traverse_unknown) { - return false; + unsigned char cost = costmap_->getCost(i); + + if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE || + (cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) ) { + std::cout << "In collision" << std::endl; + return true; + } + return false; } +nav2_costmap_2d::Costmap2D *CollisionChecker::getCostmap() { + return costmap_; } + +} // namespace nav2_route \ No newline at end of file diff --git a/nav2_route/src/node.cpp b/nav2_route/src/node.cpp index 2127a662409..fbde3b71c83 100644 --- a/nav2_route/src/node.cpp +++ b/nav2_route/src/node.cpp @@ -13,6 +13,9 @@ // limitations under the License. +#include +#include + #include "nav2_route/node.hpp" namespace nav2_route @@ -20,13 +23,11 @@ namespace nav2_route // Defining static members to be shared std::vector Node::neighbors_grid_offsets; - +unsigned int Node::max_index_; +unsigned int Node::x_size_; Node::Node(const unsigned int index) -: parent(nullptr), - index_(index), - visited_(false), - queued_(false) +: index_(index) {} Node::~Node() @@ -34,8 +35,11 @@ Node::~Node() parent = nullptr; } -void Node::initMotionModel(int x_size) +void Node::initMotionModel(int x_size, int y_size) { + max_index_ = x_size * y_size; + x_size_ = x_size; + neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1, -x_size + 1, @@ -48,33 +52,41 @@ void Node::getNeighbors( const bool & traverse_unknown, NodeVector & neighbors) { - int index; + unsigned int index; NodePtr neighbor; - int node_i = this->getIndex(); - const Coordinates parent = getCoords(node_i); - Coordinates child; + int node_i = static_cast(getIndex()); + const Coordinates p_coord = getCoords(node_i); + Coordinates c_coord; - for(unsigned int i = 0; i != neighbors_grid_offsets.size(); ++i) + for(const int & neighbors_grid_offset : neighbors_grid_offsets) { - index = node_i + i; - child = getCoords(index); + index = node_i + neighbors_grid_offset; + std::cout << "Index" << index << std::endl; + c_coord = getCoords(index); // Check for wrap around conditions - if (fabs(parent.x - child.x) > 1 || fabs(parent.y - child.y) > 1) + if (std::fabs(p_coord.x - c_coord.x) > 1 || std::fabs(p_coord.y - c_coord.y) > 1) { + std::cout << "Index wraps" << std::endl; continue; + } - if (node_getter(index, neighbor)) { - if (neighbor->isNodeValid(collision_checker, traverse_unknown) && !neighbor->wasVisited()) { - neighbors.push_back(neighbor); - } + // Check for out of bounds + if (index >= max_index_) { + std::cout << "Max index hit" << std::endl; + continue; + } + + node_getter(index, neighbor); + + if(neighbor->isNodeValid(collision_checker, traverse_unknown) ) { + neighbors.push_back(neighbor); } } } - bool Node::isNodeValid(CollisionChecker * collision_checker, const bool &traverse_unknown) { - return collision_checker->inCollision(getIndex(), traverse_unknown); + return !collision_checker->inCollision(getIndex(), traverse_unknown); } bool Node::backtracePath(nav2_route::Node::CoordinateVector & path) diff --git a/nav2_route/test/test_node.cpp b/nav2_route/test/test_node.cpp index 48940c90440..a8880d28d39 100644 --- a/nav2_route/test/test_node.cpp +++ b/nav2_route/test/test_node.cpp @@ -14,10 +14,9 @@ #include -#include -#include - - +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" #include "nav2_route/node.hpp" class RclCppFixture @@ -32,7 +31,7 @@ namespace nav2_route { TEST(test_node, node_test) { - Node::initMotionModel(10); + Node::initMotionModel(10, 15); // Check State visiting Node node(10); @@ -53,8 +52,70 @@ TEST(test_node, node_test) { EXPECT_EQ(Node::getIndex(6u, 42, 10u), 426u); EXPECT_EQ(Node::getCoords(10u).x, 0.0); EXPECT_EQ(Node::getCoords(10u).y, 1.0); +} + +TEST(test_node, test_node_neighbors) +{ + int x_size = 100; + int y_size = 40; + Node::initMotionModel(x_size, y_size); + + EXPECT_EQ(Node::neighbors_grid_offsets[0], -1); + EXPECT_EQ(Node::neighbors_grid_offsets[1], 1); + EXPECT_EQ(Node::neighbors_grid_offsets[2], -x_size); + EXPECT_EQ(Node::neighbors_grid_offsets[3], x_size); + EXPECT_EQ(Node::neighbors_grid_offsets[4], -x_size - 1); + EXPECT_EQ(Node::neighbors_grid_offsets[5], -x_size + 1); + EXPECT_EQ(Node::neighbors_grid_offsets[6], +x_size - 1); + EXPECT_EQ(Node::neighbors_grid_offsets[7], +x_size + 1); + + std::unordered_map graph; + + //Add start to graph + + graph.emplace(0, Node(0)); + + Node::NodeGetter neighbor_getter = + [&, this](const unsigned int & index, Node::NodePtr & neighbor_rtn) + { + auto iter = graph.find(index); + + if (iter != graph.end()) + { + neighbor_rtn = &(iter->second); + } + + neighbor_rtn = &(graph.emplace(index, Node(index)).first->second); + }; + + nav2_costmap_2d::Costmap2D costmap(x_size, y_size, 0.0, 0.0, 1); + CollisionChecker collision_checker(&costmap); + + Node::NodePtr start; + neighbor_getter(0, start); + + Node::NodeVector neighbors; + + start->getNeighbors(neighbor_getter, &collision_checker, false, neighbors); + + EXPECT_EQ(neighbors.size(), 3u); + + for (const auto neighbor : neighbors) { + auto coords = Node::getCoords(neighbor->getIndex()); + + EXPECT_TRUE(coords.x >= 0.0); + EXPECT_TRUE(coords.y >= 0.0); + } +} + +TEST(test_collision_checker, collision_checker_test) +{ + // Create a dummy costmap + nav2_costmap_2d::Costmap2D costmap(10u, 10u, 1, 0, 0); + costmap.setCost(0, 0, nav2_costmap_2d::LETHAL_OBSTACLE); - EXPECT_TRUE(true); + CollisionChecker collision_checker(&costmap); + EXPECT_TRUE(collision_checker.inCollision(0u, true)); } } // namespace nav2_route From 31841be7615cc534f121a6c29814c0d7dfb44083 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 21 Apr 2023 11:03:06 -0400 Subject: [PATCH 05/36] passing tests --- .../nav2_route/breadth_first_search.hpp | 22 +++---- .../include/nav2_route/collision_checker.hpp | 41 ++++++++---- nav2_route/include/nav2_route/node.hpp | 65 +++++++++++-------- nav2_route/src/breadth_first_search.cpp | 65 ++++++++++++------- nav2_route/src/collision_checker.cpp | 34 +++++++--- nav2_route/src/node.cpp | 30 ++++----- nav2_route/test/CMakeLists.txt | 13 +++- nav2_route/test/test_breadth_first_search.cpp | 50 ++++++++++++++ nav2_route/test/test_node.cpp | 5 +- 9 files changed, 221 insertions(+), 104 deletions(-) create mode 100644 nav2_route/test/test_breadth_first_search.cpp diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index 01ea1f0b45d..c099fc20431 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "nav2_route/node.hpp" @@ -51,14 +52,14 @@ class BreadthFirstSearch const unsigned int & mx, const unsigned int & my); - /** - * @brief Set the goal for the search - * @param mx The node X index of the start - * @param my The node Y index of the start - */ - void setGoal( - const unsigned int & mx, - const unsigned int & my); + /** + * @brief Set the goal for the search + * @param mx The node X index of the start + * @param my The node Y index of the start + */ + void setGoals( + const std::vector & mxs, + const std::vector & mys); /** * @brief Create a path from the given costmap, start and goal @@ -72,7 +73,6 @@ class BreadthFirstSearch void setCollisionChecker(CollisionChecker * collision_checker); private: - /** * @brief Checks if node is the goal node * @param node The node to check @@ -111,13 +111,13 @@ class BreadthFirstSearch Graph graph_; NodePtr start_{nullptr}; - NodePtr goal_{nullptr}; + NodeVector goals_{nullptr}; NodeQueue queue_; unsigned int x_size_{0}; unsigned int y_size_{0}; - CollisionChecker *collision_checker_{nullptr}; + CollisionChecker * collision_checker_{nullptr}; }; } // namespace nav2_route diff --git a/nav2_route/include/nav2_route/collision_checker.hpp b/nav2_route/include/nav2_route/collision_checker.hpp index 393b9925f8b..880f03b3c2f 100644 --- a/nav2_route/include/nav2_route/collision_checker.hpp +++ b/nav2_route/include/nav2_route/collision_checker.hpp @@ -1,24 +1,37 @@ -#include - - -#ifndef NAV2_ROUTE__COLLISION_MONITOR_HPP_ -#define NAV2_ROUTE__COLLISION_MONITOR_HPP_ - -namespace nav2_route { +// 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__COLLISION_CHECKER_HPP_ +#define NAV2_ROUTE__COLLISION_CHECKER_HPP_ + +#include "nav2_costmap_2d/costmap_2d.hpp" + +namespace nav2_route +{ -class CollisionChecker +class CollisionChecker { -public: +public: + explicit CollisionChecker(nav2_costmap_2d::Costmap2D * costmap); - CollisionChecker(nav2_costmap_2d::Costmap2D * costmap); - bool inCollision(const unsigned int & i, const bool traverse_unknown); nav2_costmap_2d::Costmap2D * getCostmap(); -private: +private: nav2_costmap_2d::Costmap2D * costmap_; }; -} // namepsace nav2_route +} // namespace nav2_route -#endif // NAV2_ROUTE__COLLISION_MONITOR_HPP_ +#endif // NAV2_ROUTE__COLLISION_CHECKER_HPP_ diff --git a/nav2_route/include/nav2_route/node.hpp b/nav2_route/include/nav2_route/node.hpp index 0bec3194977..6ab1ef8bb74 100644 --- a/nav2_route/include/nav2_route/node.hpp +++ b/nav2_route/include/nav2_route/node.hpp @@ -16,23 +16,26 @@ #define NAV2_ROUTE__NODE_HPP_ #include -#include +#include -#include +#include -namespace nav2_route { +namespace nav2_route +{ -class Node { - public: - typedef Node *NodePtr; +class Node +{ +public: + typedef Node * NodePtr; typedef std::vector NodeVector; - typedef std::function NodeGetter; + typedef std::function NodeGetter; - struct Coordinates { + struct Coordinates + { Coordinates() {} - Coordinates(const float &x_in, const float &y_in) - : x(x_in), y(y_in) {} + Coordinates(const float & x_in, const float & y_in) + : x(x_in), y(y_in) {} float x, y; }; @@ -42,59 +45,67 @@ class Node { ~Node(); - bool operator==(NodePtr &rhs) const { + bool operator==(NodePtr & rhs) const + { return this->index_ == rhs->index_; } - inline bool &wasVisited() { + inline bool & wasVisited() + { return visited_; } - inline void visit() { + inline void visit() + { visited_ = true; queued_ = false; } - inline bool &isQueued() { + inline bool & isQueued() + { return queued_; } - inline void queue() { + inline void queue() + { queued_ = true; } - inline unsigned int &getIndex() { + inline unsigned int & getIndex() + { return index_; } static inline unsigned int getIndex( - const unsigned int &x, const unsigned int &y, const unsigned int &width) { + const unsigned int & x, const unsigned int & y, const unsigned int & width) + { return x + y * width; } static inline Coordinates getCoords( - const unsigned int &index) { + const unsigned int & index) + { // Unsigned and sign ints... bad. Get andrew to take a look - const unsigned int &width = x_size_; + const unsigned int & width = x_size_; return {static_cast(index % width), static_cast(index / width)}; } static void initMotionModel(int x_size, int y_size); - bool isNodeValid(CollisionChecker *collision_checker, const bool &traverse_unknown); + bool isNodeValid(CollisionChecker * collision_checker, const bool & traverse_unknown); void getNeighbors( - NodeGetter &node_getter, - CollisionChecker *collision_checker, - const bool &traverse_unknown, - NodeVector &neighbors); + NodeGetter & node_getter, + CollisionChecker * collision_checker, + const bool & traverse_unknown, + NodeVector & neighbors); - bool backtracePath(CoordinateVector &path); + bool backtracePath(CoordinateVector & path); NodePtr parent{nullptr}; static std::vector neighbors_grid_offsets; - private: +private: unsigned int index_; bool visited_{false}; bool queued_{false}; @@ -103,5 +114,5 @@ class Node { static unsigned int max_index_; }; -} // namespace nav2_route +} // namespace nav2_route #endif // NAV2_ROUTE__NODE_HPP_ diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 497db774368..339cf525447 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include "nav2_route/breadth_first_search.hpp" + +#include #include "nav2_route/node.hpp" namespace nav2_route @@ -23,9 +25,13 @@ void BreadthFirstSearch::setStart(const unsigned int & mx, const unsigned int & start_ = addToGraph(Node::getIndex(mx, my, x_size_)); } -void BreadthFirstSearch::setGoal(const unsigned int & mx, const unsigned int & my) +void BreadthFirstSearch::setGoals( + const std::vector & mxs, + const std::vector & mys) { - goal_ = addToGraph(Node::getIndex(mx, my, x_size_)); + for (unsigned int i = 0; i < mxs.size(); ++i) { + goals_.push_back(addToGraph(Node::getIndex(mxs[i], mys[i], x_size_))); + } } BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int & index) @@ -41,12 +47,12 @@ BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int & void BreadthFirstSearch::initialize(unsigned int x_size, unsigned int y_size) { - // Add in iterations and max time parameters + // TODO(jwallace42): Add in iterations and max time parameters, needs to be completed x_size_ = x_size; y_size_ = y_size; } -void BreadthFirstSearch::setCollisionChecker(nav2_route::CollisionChecker *collision_checker) +void BreadthFirstSearch::setCollisionChecker(nav2_route::CollisionChecker * collision_checker) { collision_checker_ = collision_checker; unsigned int x_size = collision_checker_->getCostmap()->getSizeInCellsX(); @@ -72,23 +78,30 @@ bool BreadthFirstSearch::search(CoordinateVector & path) const unsigned int max_index = x_size_ * y_size_; NodeGetter node_getter = - [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool - { - if(index >= max_index) { - return false; - } + [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool + { + if (index >= max_index) { + return false; + } - neighbor_rtn = addToGraph(index); - return true; - }; + neighbor_rtn = addToGraph(index); + return true; + }; addToQueue(start_); - while(!queue_.empty()) { + std::cout << "Start: " << start_ << std::endl; + while (!queue_.empty()) { current_node = getNextNode(); + std::cout << "Graph Size: " << graph_.size() << std::endl; + std::cout << "Queue Size: " << queue_.size() << std::endl; + current_node->visit(); + std::cout << "Current node: " << current_node << " with index " << current_node->getIndex() << + std::endl; - if(isGoal(current_node)) { + if (isGoal(current_node)) { + std::cout << "Found Goal " << std::endl; return current_node->backtracePath(path); } @@ -98,21 +111,28 @@ bool BreadthFirstSearch::search(CoordinateVector & path) current_node->getNeighbors(node_getter, collision_checker_, false, neighbors); for (neighbor_iterator = neighbors.begin(); - neighbor_iterator != neighbors.end(); ++neighbor_iterator) + neighbor_iterator != neighbors.end(); ++neighbor_iterator) { neighbor = *neighbor_iterator; + std::cout << "neighbor: " << neighbor << " with index: " << neighbor->getIndex() + << " and parent " << current_node << std::endl; - if (!neighbor->wasVisited() || !neighbor->isQueued()) { + std::cout << "Was vistited " << neighbor->wasVisited() << " " << "was queued " << + neighbor->isQueued() << std::endl; + + if (!neighbor->wasVisited() && !neighbor->isQueued()) { + std::cout << "Adding to queue" << std::endl; neighbor->parent = current_node; addToQueue(neighbor); } } - } - return false; + } + return false; } -void BreadthFirstSearch::addToQueue(NodePtr &node) { - if(node->isQueued()) { +void BreadthFirstSearch::addToQueue(NodePtr & node) +{ + if (node->isQueued()) { return; } @@ -127,8 +147,9 @@ BreadthFirstSearch::NodePtr BreadthFirstSearch::getNextNode() return next; } -bool BreadthFirstSearch::isGoal(NodePtr &node) { - return node == goal_; +bool BreadthFirstSearch::isGoal(NodePtr & node) +{ + return std::any_of(goals_.begin(), goals_.end(), [&node](NodePtr & goal) {return goal == node;}); } void BreadthFirstSearch::clearQueue() diff --git a/nav2_route/src/collision_checker.cpp b/nav2_route/src/collision_checker.cpp index edd093b1c9b..d3a8a9fee17 100644 --- a/nav2_route/src/collision_checker.cpp +++ b/nav2_route/src/collision_checker.cpp @@ -1,28 +1,46 @@ +// 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/collision_checker.hpp" -#include "nav2_costmap_2d/cost_values.hpp" + #include -namespace nav2_route +#include "nav2_costmap_2d/cost_values.hpp" + +namespace nav2_route { -CollisionChecker::CollisionChecker(nav2_costmap_2d::Costmap2D *costmap) +CollisionChecker::CollisionChecker(nav2_costmap_2d::Costmap2D * costmap) { costmap_ = costmap; } -bool CollisionChecker::inCollision(const unsigned int & i , const bool traverse_unknown) +bool CollisionChecker::inCollision(const unsigned int & i, const bool traverse_unknown) { unsigned char cost = costmap_->getCost(i); if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || - cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE || - (cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) ) { + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE || + (cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) ) + { std::cout << "In collision" << std::endl; return true; } return false; } -nav2_costmap_2d::Costmap2D *CollisionChecker::getCostmap() { +nav2_costmap_2d::Costmap2D * CollisionChecker::getCostmap() +{ return costmap_; } -} // namespace nav2_route \ No newline at end of file +} // namespace nav2_route diff --git a/nav2_route/src/node.cpp b/nav2_route/src/node.cpp index fbde3b71c83..e908d8a79c6 100644 --- a/nav2_route/src/node.cpp +++ b/nav2_route/src/node.cpp @@ -47,58 +47,54 @@ void Node::initMotionModel(int x_size, int y_size) } void Node::getNeighbors( - NodeGetter & node_getter, - CollisionChecker* collision_checker, - const bool & traverse_unknown, + NodeGetter & node_getter, + CollisionChecker * collision_checker, + const bool & traverse_unknown, NodeVector & neighbors) { unsigned int index; - NodePtr neighbor; + NodePtr neighbor; int node_i = static_cast(getIndex()); const Coordinates p_coord = getCoords(node_i); Coordinates c_coord; - for(const int & neighbors_grid_offset : neighbors_grid_offsets) - { + for (const int & neighbors_grid_offset : neighbors_grid_offsets) { index = node_i + neighbors_grid_offset; - std::cout << "Index" << index << std::endl; + // std::cout << "Index" << index << std::endl; c_coord = getCoords(index); - // Check for wrap around conditions + // Check for wrap around conditions if (std::fabs(p_coord.x - c_coord.x) > 1 || std::fabs(p_coord.y - c_coord.y) > 1) { - std::cout << "Index wraps" << std::endl; +// std::cout << "Index wraps" << std::endl; continue; } // Check for out of bounds if (index >= max_index_) { - std::cout << "Max index hit" << std::endl; + // std::cout << "Max index hit" << std::endl; continue; } node_getter(index, neighbor); - if(neighbor->isNodeValid(collision_checker, traverse_unknown) ) { + if (neighbor->isNodeValid(collision_checker, traverse_unknown) ) { neighbors.push_back(neighbor); } } } -bool Node::isNodeValid(CollisionChecker * collision_checker, const bool &traverse_unknown) +bool Node::isNodeValid(CollisionChecker * collision_checker, const bool & traverse_unknown) { return !collision_checker->inCollision(getIndex(), traverse_unknown); } bool Node::backtracePath(nav2_route::Node::CoordinateVector & path) { - if (!this->parent) { - return false; - } - NodePtr current_node = this; while (current_node->parent) { path.push_back(Node::getCoords(current_node->getIndex())); + current_node = current_node->parent; } // add the start pose @@ -107,4 +103,4 @@ bool Node::backtracePath(nav2_route::Node::CoordinateVector & path) return true; } -} +} // namespace nav2_route diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt index c23b38fb666..2055df1fe5f 100644 --- a/nav2_route/test/CMakeLists.txt +++ b/nav2_route/test/CMakeLists.txt @@ -152,4 +152,15 @@ ament_target_dependencies(test_node ) target_link_libraries(test_node ${library_name} breadth_first_search -) \ No newline at end of file +) + +# Test breadth first search +ament_add_gtest(test_bfs + test_breadth_first_search.cpp +) +ament_target_dependencies(test_bfs + ${dependencies} +) +target_link_libraries(test_bfs + ${library_name} breadth_first_search +) diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp new file mode 100644 index 00000000000..e04bead4c87 --- /dev/null +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -0,0 +1,50 @@ +// 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 "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_route/collision_checker.hpp" +#include "nav2_route/breadth_first_search.hpp" + +using namespace nav2_costmap_2d; //NOLINT +using namespace nav2_route; //NOLINT + +TEST(test_breadth_first_search, bfs_test) +{ + unsigned int x_size = 10; + unsigned int y_size = 10; + + Costmap2D costmap(x_size, y_size, 0.0, 0.0, 1); + CollisionChecker collision_checker(&costmap); + + BreadthFirstSearch bfs; + + bfs.setCollisionChecker(&collision_checker); + + bfs.setStart(0u, 0u); + std::vector mxs = {2u, 5u}; + std::vector mys = {3u, 5u}; + + bfs.setGoals(mxs, mys); + + BreadthFirstSearch::CoordinateVector path; + bool result = bfs.search(path); + + EXPECT_TRUE(result); + + for (const auto pose : path) { + std::cout << pose.x << " " << pose.y << std::endl; + } +} diff --git a/nav2_route/test/test_node.cpp b/nav2_route/test/test_node.cpp index a8880d28d39..b247f2b8fce 100644 --- a/nav2_route/test/test_node.cpp +++ b/nav2_route/test/test_node.cpp @@ -71,8 +71,6 @@ TEST(test_node, test_node_neighbors) std::unordered_map graph; - //Add start to graph - graph.emplace(0, Node(0)); Node::NodeGetter neighbor_getter = @@ -80,8 +78,7 @@ TEST(test_node, test_node_neighbors) { auto iter = graph.find(index); - if (iter != graph.end()) - { + if (iter != graph.end()) { neighbor_rtn = &(iter->second); } From 0090f5b8e171bb63460d8860681bce7f4651eb27 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 21 Apr 2023 15:25:48 -0400 Subject: [PATCH 06/36] completed bfs --- .../nav2_route/breadth_first_search.hpp | 8 ++- nav2_route/src/breadth_first_search.cpp | 9 ++-- nav2_route/test/test_breadth_first_search.cpp | 54 +++++++++++++++---- 3 files changed, 55 insertions(+), 16 deletions(-) diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index c099fc20431..7c4fd3b5946 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -68,8 +68,12 @@ class BreadthFirstSearch */ bool search(CoordinateVector & path); - void initialize(unsigned int x_size, unsigned int y_size); + void initialize(int max_iterations); + /** + * @brief Set the collision checker + * @param collision_checker + */ void setCollisionChecker(CollisionChecker * collision_checker); private: @@ -118,6 +122,8 @@ class BreadthFirstSearch unsigned int y_size_{0}; CollisionChecker * collision_checker_{nullptr}; + + int max_iterations_{0}; }; } // namespace nav2_route diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 339cf525447..475f0844fd9 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -45,11 +45,10 @@ BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int & return &(graph_.emplace(index, Node(index)).first->second); } -void BreadthFirstSearch::initialize(unsigned int x_size, unsigned int y_size) +void BreadthFirstSearch::initialize(int max_iterations) { // TODO(jwallace42): Add in iterations and max time parameters, needs to be completed - x_size_ = x_size; - y_size_ = y_size; + max_iterations_ = max_iterations; } void BreadthFirstSearch::setCollisionChecker(nav2_route::CollisionChecker * collision_checker) @@ -75,6 +74,7 @@ bool BreadthFirstSearch::search(CoordinateVector & path) NodePtr neighbor = nullptr; NeighborIterator neighbor_iterator; NodeVector neighbors; + int iterations = 0; const unsigned int max_index = x_size_ * y_size_; NodeGetter node_getter = @@ -90,7 +90,8 @@ bool BreadthFirstSearch::search(CoordinateVector & path) addToQueue(start_); std::cout << "Start: " << start_ << std::endl; - while (!queue_.empty()) { + while (iterations < max_iterations_ && !queue_.empty()) { + iterations++; current_node = getNextNode(); std::cout << "Graph Size: " << graph_.size() << std::endl; diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index e04bead4c87..87af941d330 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -15,36 +15,68 @@ #include #include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" #include "nav2_route/collision_checker.hpp" #include "nav2_route/breadth_first_search.hpp" using namespace nav2_costmap_2d; //NOLINT using namespace nav2_route; //NOLINT -TEST(test_breadth_first_search, bfs_test) +class BFSTestFixture : public ::testing::Test { - unsigned int x_size = 10; - unsigned int y_size = 10; +public: + void initialize(unsigned int x_size, unsigned int y_size) + { + costmap = std::make_unique(x_size, y_size, 0.0, 0.0, 1); + collision_checker = std::make_unique(costmap.get()); - Costmap2D costmap(x_size, y_size, 0.0, 0.0, 1); - CollisionChecker collision_checker(&costmap); + bfs.initialize(1000); + bfs.setCollisionChecker(collision_checker.get()); + } BreadthFirstSearch bfs; + std::unique_ptr costmap; + std::unique_ptr collision_checker; + BreadthFirstSearch::CoordinateVector path; +}; - bfs.setCollisionChecker(&collision_checker); +TEST_F(BFSTestFixture, free_space) +{ + initialize(10u, 10u); bfs.setStart(0u, 0u); std::vector mxs = {2u, 5u}; std::vector mys = {3u, 5u}; bfs.setGoals(mxs, mys); - - BreadthFirstSearch::CoordinateVector path; bool result = bfs.search(path); EXPECT_TRUE(result); + EXPECT_EQ(path.begin()->x, 2); + EXPECT_EQ(path.begin()->y, 3); + path.clear(); +} + +TEST_F(BFSTestFixture, wall) +{ + initialize(10u, 10u); - for (const auto pose : path) { - std::cout << pose.x << " " << pose.y << std::endl; + unsigned int mx = 3; + for(unsigned int my=0; my < costmap->getSizeInCellsY() -1; ++my) { + costmap->setCost(mx, my, LETHAL_OBSTACLE); } -} + + bfs.setStart(0u, 0u); + + std::vector mxs = {2u, 5u}; + std::vector mys = {8u, 0u}; + + bfs.setGoals(mxs, mys); + + bool result = bfs.search(path); + EXPECT_TRUE(result); + + EXPECT_EQ(path.begin()->x, 2); + EXPECT_EQ(path.begin()->y, 8); + path.clear(); +} \ No newline at end of file From 75f9f64cf54c73ea17a7d61b937f38b48be2e4c4 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 27 Apr 2023 05:45:18 -0400 Subject: [PATCH 07/36] ignore warning (#3543) * Split overlay setup into multiple steps by skipping slower to build leaf packages during preparation, then store cache and repeat setup without skipping packages * Skip restore steps after already preping overlay to avoid needlessly downloading the same overlay cache * Revert resource_class to default medium as the build resource usage seldom maxes out 4 cores nor uses more than 2GB RAM * Fix circleci config syntax by setting skip default as empty string to keep it an optional parameter * Fix circleci config syntax missing angle brackets * ignore warning * Revert "Revert resource_class to default medium" This reverts commit 44375a1c6ef6e730e47e30c7d910a15145d4ee2f. * Fix nested defaults to avoid dropping of cache after storing during test jobs by ensuring restore_overlay_workspace still sets restore: true --------- Co-authored-by: ruffsl --- .circleci/config.yml | 27 +++++++++++++++++++++++---- nav2_msgs/CMakeLists.txt | 3 +++ 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index ede7b367e94..7f66fd214ab 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_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 47093fb5e64..28ee58feb90 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" From 1e8901b4060e5dee45c94b3776f91f476ea629a9 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 27 Apr 2023 05:45:18 -0400 Subject: [PATCH 08/36] ignore warning (#3543) * Split overlay setup into multiple steps by skipping slower to build leaf packages during preparation, then store cache and repeat setup without skipping packages * Skip restore steps after already preping overlay to avoid needlessly downloading the same overlay cache * Revert resource_class to default medium as the build resource usage seldom maxes out 4 cores nor uses more than 2GB RAM * Fix circleci config syntax by setting skip default as empty string to keep it an optional parameter * Fix circleci config syntax missing angle brackets * ignore warning * Revert "Revert resource_class to default medium" This reverts commit 44375a1c6ef6e730e47e30c7d910a15145d4ee2f. * Fix nested defaults to avoid dropping of cache after storing during test jobs by ensuring restore_overlay_workspace still sets restore: true --------- Co-authored-by: ruffsl --- .circleci/config.yml | 27 +++++++++++++++++++++++---- nav2_msgs/CMakeLists.txt | 3 +++ 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index ede7b367e94..7f66fd214ab 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_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 47093fb5e64..28ee58feb90 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" From 9359ede451584c186ce41d8aff9a7a092e7ae672 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 May 2023 18:05:48 -0400 Subject: [PATCH 09/36] simplifed bfs --- nav2_route/CMakeLists.txt | 4 +- .../nav2_route/breadth_first_search.hpp | 159 ++++++------- nav2_route/include/nav2_route/node.hpp | 118 ---------- nav2_route/src/breadth_first_search.cpp | 210 +++++++++--------- nav2_route/src/node.cpp | 106 --------- nav2_route/test/CMakeLists.txt | 11 - nav2_route/test/test_breadth_first_search.cpp | 29 ++- nav2_route/test/test_node.cpp | 118 ---------- 8 files changed, 182 insertions(+), 573 deletions(-) delete mode 100644 nav2_route/include/nav2_route/node.hpp delete mode 100644 nav2_route/src/node.cpp delete mode 100644 nav2_route/test/test_node.cpp diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index 6811dfb71b6..f7defe56326 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -113,9 +113,7 @@ pluginlib_export_plugin_description_file(nav2_route plugins.xml) # Breadth first search library add_library(breadth_first_search SHARED - src/collision_checker.cpp - src/breadth_first_search.cpp - src/node.cpp) + src/breadth_first_search.cpp) ament_target_dependencies(breadth_first_search ${dependencies} diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index 7c4fd3b5946..5921ea9bfa0 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -21,109 +21,74 @@ #include #include -#include "nav2_route/node.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" namespace nav2_route { +struct Coordinates +{ + Coordinates() = default; + + Coordinates(const float & x_in, const float & y_in) + : x(x_in), y(y_in) {} + + float x, y; +}; + +struct State +{ + bool visited{false}; + bool queued{false}; +}; + class BreadthFirstSearch { -public: - typedef std::unordered_map Graph; - typedef Node::NodePtr NodePtr; - typedef Node::NodeVector NodeVector; - typedef Node::NodeVector::iterator NeighborIterator; - typedef Node::Coordinates Coordinates; - typedef Node::CoordinateVector CoordinateVector; - typedef Node::NodeGetter NodeGetter; - typedef std::queue NodeQueue; - - /** - * @brief A constructor for nav2_route::BreadthFirstSearch - */ - BreadthFirstSearch() = default; - - /** - * @brief Set the starting pose for the search - * @param mx The node X index of the start - * @param my The node Y index of the start - */ - void setStart( - const unsigned int & mx, - const unsigned int & my); - - /** - * @brief Set the goal for the search - * @param mx The node X index of the start - * @param my The node Y index of the start - */ - void setGoals( - const std::vector & mxs, - const std::vector & mys); - - /** - * @brief Create a path from the given costmap, start and goal - * @param path The output path if the search was successful - * @return True if a plan was successfully calculated - */ - bool search(CoordinateVector & path); - - void initialize(int max_iterations); - - /** - * @brief Set the collision checker - * @param collision_checker - */ - void setCollisionChecker(CollisionChecker * collision_checker); - -private: - /** - * @brief Checks if node is the goal node - * @param node The node to check - * @return True if the node is the goal node - */ - inline bool isGoal(NodePtr & node); - - /** - * @brief Adds a node to the graph - * @param index The index of the node to add - * @return The node added to the graph - */ - NodePtr addToGraph(const unsigned int & index); - - /** - * @brief Adds a node to the queue - * @param node The node to add - */ - void addToQueue(NodePtr & node); - - /** - * @brief Gets the next node in the queue - * @return The next node in the queue - */ - NodePtr getNextNode(); - - /** - * @brief Clear the queue - */ - void clearQueue(); - - /** - * @brief Clear the graph - */ - void clearGraph(); - - Graph graph_; - NodePtr start_{nullptr}; - NodeVector goals_{nullptr}; - NodeQueue queue_; - - unsigned int x_size_{0}; - unsigned int y_size_{0}; - - CollisionChecker * collision_checker_{nullptr}; - - int max_iterations_{0}; + public: + + void initMotionModel(int x_size, int y_size); + + void setCostmap(nav2_costmap_2d::Costmap2D *costmap); + + void setStart(unsigned int mx, unsigned int my); + + void setGoals(std::vector mxs, std::vector mys); + + bool search(Coordinates & closest_goal); + + private: + + inline Coordinates getCoords( + const unsigned int & index) const + { + const unsigned int & width = x_size_; + return {static_cast(index % width), static_cast(index / width)}; + } + + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & width) + { + return x + y * width; + } + + bool getNeighbors(unsigned int current, std::vector & neighbors); + + bool inCollision(unsigned int index); + + std::unordered_map states_; + std::queue queue_; + + unsigned int start_; + std::vector goals_; + + unsigned int x_size_; + unsigned int y_size_; + unsigned int max_index_; + std::vector neighbors_grid_offsets; + + nav2_costmap_2d::Costmap2D * costmap_; + }; } // namespace nav2_route diff --git a/nav2_route/include/nav2_route/node.hpp b/nav2_route/include/nav2_route/node.hpp deleted file mode 100644 index 6ab1ef8bb74..00000000000 --- a/nav2_route/include/nav2_route/node.hpp +++ /dev/null @@ -1,118 +0,0 @@ -// 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__NODE_HPP_ -#define NAV2_ROUTE__NODE_HPP_ - -#include -#include - -#include - -namespace nav2_route -{ - -class Node -{ -public: - typedef Node * NodePtr; - typedef std::vector NodeVector; - typedef std::function NodeGetter; - - struct Coordinates - { - Coordinates() {} - - Coordinates(const float & x_in, const float & y_in) - : x(x_in), y(y_in) {} - - float x, y; - }; - typedef std::vector CoordinateVector; - - explicit Node(const unsigned int index); - - ~Node(); - - bool operator==(NodePtr & rhs) const - { - return this->index_ == rhs->index_; - } - - inline bool & wasVisited() - { - return visited_; - } - - inline void visit() - { - visited_ = true; - queued_ = false; - } - - inline bool & isQueued() - { - return queued_; - } - - inline void queue() - { - queued_ = true; - } - - inline unsigned int & getIndex() - { - return index_; - } - - static inline unsigned int getIndex( - const unsigned int & x, const unsigned int & y, const unsigned int & width) - { - return x + y * width; - } - - static inline Coordinates getCoords( - const unsigned int & index) - { - // Unsigned and sign ints... bad. Get andrew to take a look - const unsigned int & width = x_size_; - return {static_cast(index % width), static_cast(index / width)}; - } - - static void initMotionModel(int x_size, int y_size); - - bool isNodeValid(CollisionChecker * collision_checker, const bool & traverse_unknown); - - void getNeighbors( - NodeGetter & node_getter, - CollisionChecker * collision_checker, - const bool & traverse_unknown, - NodeVector & neighbors); - - bool backtracePath(CoordinateVector & path); - - NodePtr parent{nullptr}; - static std::vector neighbors_grid_offsets; - -private: - unsigned int index_; - bool visited_{false}; - bool queued_{false}; - - static unsigned int x_size_; - static unsigned int max_index_; -}; - -} // namespace nav2_route -#endif // NAV2_ROUTE__NODE_HPP_ diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 475f0844fd9..911b9ef999b 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -15,154 +15,156 @@ #include "nav2_route/breadth_first_search.hpp" #include -#include "nav2_route/node.hpp" namespace nav2_route { -void BreadthFirstSearch::setStart(const unsigned int & mx, const unsigned int & my) +void BreadthFirstSearch::initMotionModel(int x_size, int y_size) { - start_ = addToGraph(Node::getIndex(mx, my, x_size_)); -} + max_index_ = x_size * y_size; + x_size_ = x_size; -void BreadthFirstSearch::setGoals( - const std::vector & mxs, - const std::vector & mys) -{ - for (unsigned int i = 0; i < mxs.size(); ++i) { - goals_.push_back(addToGraph(Node::getIndex(mxs[i], mys[i], x_size_))); - } + neighbors_grid_offsets = {-1, +1, + -x_size, +x_size, + -x_size - 1, -x_size + 1, + +x_size - 1, +x_size + 1}; + + std::cout << "offset size " << neighbors_grid_offsets.size() << std::endl; } -BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int & index) +void BreadthFirstSearch::setCostmap(nav2_costmap_2d::Costmap2D *costmap) { - auto iter = graph_.find(index); + std::cout << "Set costmap " << std::endl; + costmap_ = costmap; - if (iter != graph_.end()) { - return &(iter->second); - } + unsigned int x_size = costmap_->getSizeInCellsX(); + unsigned int y_size = costmap_->getSizeInCellsY(); - return &(graph_.emplace(index, Node(index)).first->second); + initMotionModel(static_cast(x_size), static_cast(y_size)); } -void BreadthFirstSearch::initialize(int max_iterations) -{ - // TODO(jwallace42): Add in iterations and max time parameters, needs to be completed - max_iterations_ = max_iterations; +void BreadthFirstSearch::setStart(unsigned int mx, unsigned int my) { + start_ = getIndex(mx, my, x_size_); + states_.insert(std::make_pair(start_, State())); } -void BreadthFirstSearch::setCollisionChecker(nav2_route::CollisionChecker * collision_checker) +void BreadthFirstSearch::setGoals(std::vector mxs, std::vector mys) { - collision_checker_ = collision_checker; - unsigned int x_size = collision_checker_->getCostmap()->getSizeInCellsX(); - unsigned int y_size = collision_checker->getCostmap()->getSizeInCellsY(); - - clearGraph(); - - if (x_size_ != x_size || y_size_ != y_size) { - x_size_ = x_size; - y_size_ = y_size; - Node::initMotionModel(static_cast(x_size_), static_cast(y_size_)); + goals_.clear(); + for(unsigned int i = 0; i < (mxs.size() && mys.size()); ++i) { + unsigned int index = getIndex(mxs[i], mys[i], x_size_); + goals_.push_back(index); + states_.insert(std::make_pair(index, State())); } } -bool BreadthFirstSearch::search(CoordinateVector & path) +bool BreadthFirstSearch::search(Coordinates & closest_goal) { - clearQueue(); + std::cout << "Start " << std::endl; + states_.clear(); - NodePtr current_node = nullptr; - NodePtr neighbor = nullptr; - NeighborIterator neighbor_iterator; - NodeVector neighbors; - int iterations = 0; + // clear the queue + std::queue empty_queue; + queue_.swap(empty_queue); - const unsigned int max_index = x_size_ * y_size_; - NodeGetter node_getter = - [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool - { - if (index >= max_index) { - return false; - } + //Add start to queue + queue_.push(start_); - neighbor_rtn = addToGraph(index); - return true; - }; + std::cout << "Added start " << std::endl; - addToQueue(start_); - std::cout << "Start: " << start_ << std::endl; - while (iterations < max_iterations_ && !queue_.empty()) { - iterations++; - current_node = getNextNode(); + while (!queue_.empty()) { - std::cout << "Graph Size: " << graph_.size() << std::endl; std::cout << "Queue Size: " << queue_.size() << std::endl; - current_node->visit(); - std::cout << "Current node: " << current_node << " with index " << current_node->getIndex() << - std::endl; + unsigned int current = queue_.front(); + std::cout << "grab front" << std::endl; - if (isGoal(current_node)) { - std::cout << "Found Goal " << std::endl; - return current_node->backtracePath(path); - } + Coordinates current_coord = getCoords(current); + std::cout << "Current Node: " << current_coord.x << " " << current_coord.y << std::endl; - neighbors.clear(); - neighbor = nullptr; + queue_.pop(); + states_[current].visited = true; + states_[current].queued = false; - current_node->getNeighbors(node_getter, collision_checker_, false, neighbors); + // Check goals + for(const auto &goal : goals_) { + if (current == goal) { + std::cout << "Found goal" << std::endl; + closest_goal = getCoords(current); + return true; + } + } + std::cout << "Checked goals" << std::endl; - for (neighbor_iterator = neighbors.begin(); - neighbor_iterator != neighbors.end(); ++neighbor_iterator) - { - neighbor = *neighbor_iterator; - std::cout << "neighbor: " << neighbor << " with index: " << neighbor->getIndex() - << " and parent " << current_node << std::endl; + std::vector neighbors; + getNeighbors(current, neighbors); + std::cout << "Got neigbors" << std::endl; - std::cout << "Was vistited " << neighbor->wasVisited() << " " << "was queued " << - neighbor->isQueued() << std::endl; + for(const auto neighbor : neighbors) + { + if(!states_[neighbor].queued && !states_[neighbor].visited) { + states_[neighbor].queued = true; + queue_.push(neighbor); - if (!neighbor->wasVisited() && !neighbor->isQueued()) { - std::cout << "Adding to queue" << std::endl; - neighbor->parent = current_node; - addToQueue(neighbor); + std::cout << "Added neighbor: " << neighbor << std::endl; } } - } + } return false; } -void BreadthFirstSearch::addToQueue(NodePtr & node) +bool BreadthFirstSearch::getNeighbors(unsigned int current, std::vector & neighbors) { - if (node->isQueued()) { - return; - } + const Coordinates p_coord = getCoords(current); - node->queue(); - queue_.emplace(node); -} + unsigned int index; + Coordinates c_coord{0, 0}; -BreadthFirstSearch::NodePtr BreadthFirstSearch::getNextNode() -{ - NodePtr next = queue_.front(); - queue_.pop(); - return next; -} + std::cout << "grid offset size " << neighbors_grid_offsets.size() << std::endl; + for (const int & neighbors_grid_offset : neighbors_grid_offsets) + { + std::cout << "Getting neighbors" << std::endl; + index = current + neighbors_grid_offset; -bool BreadthFirstSearch::isGoal(NodePtr & node) -{ - return std::any_of(goals_.begin(), goals_.end(), [&node](NodePtr & goal) {return goal == node;}); -} + c_coord = getCoords(index); -void BreadthFirstSearch::clearQueue() -{ - NodeQueue q; - std::swap(queue_, q); + // Check for wrap around conditions + if (std::fabs(p_coord.x - c_coord.x) > 1 || std::fabs(p_coord.y - c_coord.y) > 1) { + std::cout << "Index wraps" << std::endl; + continue; + } + + // Check for out of bounds + if (index >= max_index_) { + std::cout << "Max index hit" << std::endl; + continue; + } + + if(inCollision(index)) { + std::cout << "In collision" << std::endl; + continue; + } + + neighbors.push_back(index); + + auto it = states_.find(index); + if (it == states_.end()) { + states_.insert(std::make_pair(index, State())); + } + } + return false; } -void BreadthFirstSearch::clearGraph() -{ - Graph g; - std::swap(graph_, g); +bool BreadthFirstSearch::inCollision(unsigned int index) { + unsigned char cost = costmap_->getCost(index); + + if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + { + std::cout << "In collision" << std::endl; + return true; + } + return false; } -} // namespace nav2_route +} // namespace nav2_route \ No newline at end of file diff --git a/nav2_route/src/node.cpp b/nav2_route/src/node.cpp deleted file mode 100644 index e908d8a79c6..00000000000 --- a/nav2_route/src/node.cpp +++ /dev/null @@ -1,106 +0,0 @@ -// 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_route/node.hpp" - -namespace nav2_route -{ - -// Defining static members to be shared -std::vector Node::neighbors_grid_offsets; -unsigned int Node::max_index_; -unsigned int Node::x_size_; - -Node::Node(const unsigned int index) -: index_(index) -{} - -Node::~Node() -{ - parent = nullptr; -} - -void Node::initMotionModel(int x_size, int y_size) -{ - max_index_ = x_size * y_size; - x_size_ = x_size; - - neighbors_grid_offsets = {-1, +1, - -x_size, +x_size, - -x_size - 1, -x_size + 1, - +x_size - 1, +x_size + 1}; -} - -void Node::getNeighbors( - NodeGetter & node_getter, - CollisionChecker * collision_checker, - const bool & traverse_unknown, - NodeVector & neighbors) -{ - unsigned int index; - NodePtr neighbor; - int node_i = static_cast(getIndex()); - const Coordinates p_coord = getCoords(node_i); - Coordinates c_coord; - - for (const int & neighbors_grid_offset : neighbors_grid_offsets) { - index = node_i + neighbors_grid_offset; - // std::cout << "Index" << index << std::endl; - c_coord = getCoords(index); - - // Check for wrap around conditions - if (std::fabs(p_coord.x - c_coord.x) > 1 || std::fabs(p_coord.y - c_coord.y) > 1) { -// std::cout << "Index wraps" << std::endl; - continue; - } - - // Check for out of bounds - if (index >= max_index_) { - // std::cout << "Max index hit" << std::endl; - continue; - } - - node_getter(index, neighbor); - - if (neighbor->isNodeValid(collision_checker, traverse_unknown) ) { - neighbors.push_back(neighbor); - } - } -} - -bool Node::isNodeValid(CollisionChecker * collision_checker, const bool & traverse_unknown) -{ - return !collision_checker->inCollision(getIndex(), traverse_unknown); -} - -bool Node::backtracePath(nav2_route::Node::CoordinateVector & path) -{ - NodePtr current_node = this; - - while (current_node->parent) { - path.push_back(Node::getCoords(current_node->getIndex())); - current_node = current_node->parent; - } - - // add the start pose - path.push_back(Node::getCoords(current_node->getIndex())); - - return true; -} - -} // namespace nav2_route diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt index 2055df1fe5f..7c49473c5e2 100644 --- a/nav2_route/test/CMakeLists.txt +++ b/nav2_route/test/CMakeLists.txt @@ -143,17 +143,6 @@ # ${library_name} #) -# Test breadth first search node -ament_add_gtest(test_node - test_node.cpp -) -ament_target_dependencies(test_node - ${dependencies} -) -target_link_libraries(test_node - ${library_name} breadth_first_search -) - # Test breadth first search ament_add_gtest(test_bfs test_breadth_first_search.cpp diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index 87af941d330..c9f1861f461 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -16,7 +16,6 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/cost_values.hpp" -#include "nav2_route/collision_checker.hpp" #include "nav2_route/breadth_first_search.hpp" using namespace nav2_costmap_2d; //NOLINT @@ -28,16 +27,12 @@ class BFSTestFixture : public ::testing::Test void initialize(unsigned int x_size, unsigned int y_size) { costmap = std::make_unique(x_size, y_size, 0.0, 0.0, 1); - collision_checker = std::make_unique(costmap.get()); - bfs.initialize(1000); - bfs.setCollisionChecker(collision_checker.get()); + bfs.setCostmap(costmap.get()); } BreadthFirstSearch bfs; std::unique_ptr costmap; - std::unique_ptr collision_checker; - BreadthFirstSearch::CoordinateVector path; }; TEST_F(BFSTestFixture, free_space) @@ -45,16 +40,18 @@ TEST_F(BFSTestFixture, free_space) initialize(10u, 10u); bfs.setStart(0u, 0u); - std::vector mxs = {2u, 5u}; - std::vector mys = {3u, 5u}; + // need multiple goals + std::vector mxs = {1u}; + std::vector mys = {1u}; bfs.setGoals(mxs, mys); - bool result = bfs.search(path); + Coordinates closest_goal; + bool result = bfs.search(closest_goal); EXPECT_TRUE(result); - EXPECT_EQ(path.begin()->x, 2); - EXPECT_EQ(path.begin()->y, 3); - path.clear(); + + EXPECT_EQ(closest_goal.x, mxs.front()); + EXPECT_EQ(closest_goal.y, mys.front()); } TEST_F(BFSTestFixture, wall) @@ -73,10 +70,10 @@ TEST_F(BFSTestFixture, wall) bfs.setGoals(mxs, mys); - bool result = bfs.search(path); + Coordinates closest_goal; + bool result = bfs.search(closest_goal); EXPECT_TRUE(result); - EXPECT_EQ(path.begin()->x, 2); - EXPECT_EQ(path.begin()->y, 8); - path.clear(); + EXPECT_EQ(closest_goal.x, mxs.front()); + EXPECT_EQ(closest_goal.y, mys.front()); } \ No newline at end of file diff --git a/nav2_route/test/test_node.cpp b/nav2_route/test/test_node.cpp deleted file mode 100644 index b247f2b8fce..00000000000 --- a/nav2_route/test/test_node.cpp +++ /dev/null @@ -1,118 +0,0 @@ -// Copyright (c) 2023 Samsung Research America -// -// 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 "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/cost_values.hpp" -#include "nav2_route/node.hpp" - -class RclCppFixture -{ -public: - RclCppFixture() {rclcpp::init(0, nullptr);} - ~RclCppFixture() {rclcpp::shutdown();} -}; -RclCppFixture g_rclcppfixture; - -namespace nav2_route -{ - -TEST(test_node, node_test) { - Node::initMotionModel(10, 15); - - // Check State visiting - Node node(10); - - EXPECT_FALSE(node.wasVisited()); - EXPECT_FALSE(node.isQueued()); - - node.queue(); - EXPECT_TRUE(node.isQueued()); - EXPECT_FALSE(node.wasVisited()); - - node.visit(); - EXPECT_TRUE(node.wasVisited()); - EXPECT_FALSE(node.isQueued()); - - // Check static index functions - EXPECT_EQ(Node::getIndex(1u, 1u, 10u), 11u); - EXPECT_EQ(Node::getIndex(6u, 42, 10u), 426u); - EXPECT_EQ(Node::getCoords(10u).x, 0.0); - EXPECT_EQ(Node::getCoords(10u).y, 1.0); -} - -TEST(test_node, test_node_neighbors) -{ - int x_size = 100; - int y_size = 40; - Node::initMotionModel(x_size, y_size); - - EXPECT_EQ(Node::neighbors_grid_offsets[0], -1); - EXPECT_EQ(Node::neighbors_grid_offsets[1], 1); - EXPECT_EQ(Node::neighbors_grid_offsets[2], -x_size); - EXPECT_EQ(Node::neighbors_grid_offsets[3], x_size); - EXPECT_EQ(Node::neighbors_grid_offsets[4], -x_size - 1); - EXPECT_EQ(Node::neighbors_grid_offsets[5], -x_size + 1); - EXPECT_EQ(Node::neighbors_grid_offsets[6], +x_size - 1); - EXPECT_EQ(Node::neighbors_grid_offsets[7], +x_size + 1); - - std::unordered_map graph; - - graph.emplace(0, Node(0)); - - Node::NodeGetter neighbor_getter = - [&, this](const unsigned int & index, Node::NodePtr & neighbor_rtn) - { - auto iter = graph.find(index); - - if (iter != graph.end()) { - neighbor_rtn = &(iter->second); - } - - neighbor_rtn = &(graph.emplace(index, Node(index)).first->second); - }; - - nav2_costmap_2d::Costmap2D costmap(x_size, y_size, 0.0, 0.0, 1); - CollisionChecker collision_checker(&costmap); - - Node::NodePtr start; - neighbor_getter(0, start); - - Node::NodeVector neighbors; - - start->getNeighbors(neighbor_getter, &collision_checker, false, neighbors); - - EXPECT_EQ(neighbors.size(), 3u); - - for (const auto neighbor : neighbors) { - auto coords = Node::getCoords(neighbor->getIndex()); - - EXPECT_TRUE(coords.x >= 0.0); - EXPECT_TRUE(coords.y >= 0.0); - } -} - -TEST(test_collision_checker, collision_checker_test) -{ - // Create a dummy costmap - nav2_costmap_2d::Costmap2D costmap(10u, 10u, 1, 0, 0); - costmap.setCost(0, 0, nav2_costmap_2d::LETHAL_OBSTACLE); - - CollisionChecker collision_checker(&costmap); - EXPECT_TRUE(collision_checker.inCollision(0u, true)); -} - -} // namespace nav2_route From a2e7b4e754dbe63f0fcd9234f4a629fdeca4672b Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 May 2023 18:09:44 -0400 Subject: [PATCH 10/36] revert changes --- nav2_bringup/launch/bringup_launch.py | 4 ++-- nav2_bringup/launch/tb3_simulation_launch.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index aa64fe9443d..8fddcc32531 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -113,9 +113,9 @@ def generate_launch_description(): PushRosNamespace( condition=IfCondition(use_namespace), namespace=namespace), - PushNodeRemapping + Node( - condition=IfCondition(use_composition),: + condition=IfCondition(use_composition), name='nav2_container', package='rclcpp_components', executable='component_container_isolated', diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 36dfcff5d07..b4dfd3bd167 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -134,7 +134,7 @@ def generate_launch_description(): declare_simulator_cmd = DeclareLaunchArgument( 'headless', - default_value='False', + default_value='True', description='Whether to execute gzclient)') declare_world_cmd = DeclareLaunchArgument( From 5e3ed1df3c6e093d53f239931006e2050368c93a Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 May 2023 18:12:14 -0400 Subject: [PATCH 11/36] shape file revert --- nav2_route/graphs/scripts/export_shapefiles.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_route/graphs/scripts/export_shapefiles.py b/nav2_route/graphs/scripts/export_shapefiles.py index 47f5a682f61..4addaac6ca4 100644 --- a/nav2_route/graphs/scripts/export_shapefiles.py +++ b/nav2_route/graphs/scripts/export_shapefiles.py @@ -3,11 +3,11 @@ import sys from datetime import datetime -try: - file_prefix = sys.argv[1] +try: + file_prefix = sys.argv[1] edges = gpd.read_file(sys.argv[2]) nodes = gpd.read_file(sys.argv[3]) -except IndexError: +except: raise Exception("Incorrect arguements provided") now = datetime.now() @@ -16,4 +16,4 @@ file_name = file_prefix + "_" + now.strftime("%m_%d_%Y_%H_%M_%S") + ".geojson" -graph.to_file(file_name, driver='GeoJSON') +graph.to_file(file_name, driver='GeoJSON') \ No newline at end of file From 813c99ea03fd48ee8de42181d82239d757047a9a Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 May 2023 18:12:55 -0400 Subject: [PATCH 12/36] delete collision checker --- .../include/nav2_route/collision_checker.hpp | 37 --------------- nav2_route/src/collision_checker.cpp | 46 ------------------- 2 files changed, 83 deletions(-) delete mode 100644 nav2_route/include/nav2_route/collision_checker.hpp delete mode 100644 nav2_route/src/collision_checker.cpp diff --git a/nav2_route/include/nav2_route/collision_checker.hpp b/nav2_route/include/nav2_route/collision_checker.hpp deleted file mode 100644 index 880f03b3c2f..00000000000 --- a/nav2_route/include/nav2_route/collision_checker.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// 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__COLLISION_CHECKER_HPP_ -#define NAV2_ROUTE__COLLISION_CHECKER_HPP_ - -#include "nav2_costmap_2d/costmap_2d.hpp" - -namespace nav2_route -{ - -class CollisionChecker -{ -public: - explicit CollisionChecker(nav2_costmap_2d::Costmap2D * costmap); - - bool inCollision(const unsigned int & i, const bool traverse_unknown); - - nav2_costmap_2d::Costmap2D * getCostmap(); - -private: - nav2_costmap_2d::Costmap2D * costmap_; -}; -} // namespace nav2_route - -#endif // NAV2_ROUTE__COLLISION_CHECKER_HPP_ diff --git a/nav2_route/src/collision_checker.cpp b/nav2_route/src/collision_checker.cpp deleted file mode 100644 index d3a8a9fee17..00000000000 --- a/nav2_route/src/collision_checker.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// 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/collision_checker.hpp" - -#include - -#include "nav2_costmap_2d/cost_values.hpp" - -namespace nav2_route -{ -CollisionChecker::CollisionChecker(nav2_costmap_2d::Costmap2D * costmap) -{ - costmap_ = costmap; -} - -bool CollisionChecker::inCollision(const unsigned int & i, const bool traverse_unknown) -{ - unsigned char cost = costmap_->getCost(i); - - if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || - cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE || - (cost == nav2_costmap_2d::NO_INFORMATION && !traverse_unknown) ) - { - std::cout << "In collision" << std::endl; - return true; - } - return false; -} -nav2_costmap_2d::Costmap2D * CollisionChecker::getCostmap() -{ - return costmap_; -} - -} // namespace nav2_route From c8f99eb70bdc660ed7c044ad63969fa4c78ac4ac Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 May 2023 18:14:34 -0400 Subject: [PATCH 13/36] uncomment tests --- nav2_route/test/CMakeLists.txt | 288 ++++++++++++++++----------------- 1 file changed, 144 insertions(+), 144 deletions(-) diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt index 7c49473c5e2..550ec741ece 100644 --- a/nav2_route/test/CMakeLists.txt +++ b/nav2_route/test/CMakeLists.txt @@ -1,147 +1,147 @@ -## Route planner benchmarking script -#add_executable(performance_benchmarking performance_benchmarking.cpp) -#target_link_libraries(performance_benchmarking -# ${library_name} -#) -#ament_target_dependencies(performance_benchmarking -# ${dependencies} -#) -#install(TARGETS -# performance_benchmarking -# RUNTIME DESTINATION lib/${PROJECT_NAME} -#) -# -## Test utilities and basic types -#ament_add_gtest(test_utils_and_types -# test_utils_and_types.cpp -#) -#ament_target_dependencies(test_utils_and_types -# ${dependencies} -#) -#target_link_libraries(test_utils_and_types -# ${library_name} -#) -# -## Test edge scorer + plugins -#ament_add_gtest(test_edge_scorers -# test_edge_scorers.cpp -#) -#ament_target_dependencies(test_edge_scorers -# ${dependencies} -#) -#target_link_libraries(test_edge_scorers -# ${library_name} edge_scorers -#) -# -## Test path converter -#ament_add_gtest(test_path_converter -# test_path_converter.cpp -#) -#ament_target_dependencies(test_path_converter -# ${dependencies} -#) -#target_link_libraries(test_path_converter -# ${library_name} -#) -# -## Test node spatial tree -#ament_add_gtest(test_spatial_tree -# test_spatial_tree.cpp -#) -#ament_target_dependencies(test_spatial_tree -# ${dependencies} -#) -#target_link_libraries(test_spatial_tree -# ${library_name} -#) -# -## Test operation manager + plugins -#ament_add_gtest(test_operations -# test_operations.cpp -#) -#ament_target_dependencies(test_operations -# ${dependencies} -#) -#target_link_libraries(test_operations -# ${library_name} route_operations -#) -# -## Test graph loader -#ament_add_gtest(test_graph_loader -# test_graph_loader.cpp -#) -#ament_target_dependencies(test_graph_loader -# ${dependencies} -#) -#target_link_libraries(test_graph_loader -# ${library_name} -#) -# -## Test geojson parser -#ament_add_gtest(test_geojson_graph_file_loader -# test_geojson_graph_file_loader.cpp -#) -#ament_target_dependencies(test_geojson_graph_file_loader -# ${dependencies} -#) -#target_link_libraries(test_geojson_graph_file_loader -# ${library_name} graph_file_loaders -#) -# -## Test collision monitor seperately due to relative complexity -#ament_add_gtest(test_collision_operation -# test_collision_operation.cpp -#) -#ament_target_dependencies(test_collision_operation -# ${dependencies} -#) -#target_link_libraries(test_collision_operation -# ${library_name} route_operations -#) -# -## Test route planner -#ament_add_gtest(test_route_planner -# test_route_planner.cpp -#) -#ament_target_dependencies(test_route_planner -# ${dependencies} -#) -#target_link_libraries(test_route_planner -# ${library_name} edge_scorers -#) -# -## Test goal intent extractor -#ament_add_gtest(test_goal_intent_extractor -# test_goal_intent_extractor.cpp -#) -#ament_target_dependencies(test_goal_intent_extractor -# ${dependencies} -#) -#target_link_libraries(test_goal_intent_extractor -# ${library_name} -#) -# -## Test route tracker -#ament_add_gtest(test_route_tracker -# test_route_tracker.cpp -#) -#ament_target_dependencies(test_route_tracker -# ${dependencies} -#) -#target_link_libraries(test_route_tracker -# ${library_name} -#) -# -## Test route server -#ament_add_gtest(test_route_server -# test_route_server.cpp -#) -#ament_target_dependencies(test_route_server -# ${dependencies} -#) -#target_link_libraries(test_route_server -# ${library_name} -#) +# Route planner benchmarking script +add_executable(performance_benchmarking performance_benchmarking.cpp) +target_link_libraries(performance_benchmarking + ${library_name} +) +ament_target_dependencies(performance_benchmarking + ${dependencies} +) +install(TARGETS + performance_benchmarking + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +# Test utilities and basic types +ament_add_gtest(test_utils_and_types + test_utils_and_types.cpp +) +ament_target_dependencies(test_utils_and_types + ${dependencies} +) +target_link_libraries(test_utils_and_types + ${library_name} +) + +# Test edge scorer + plugins +ament_add_gtest(test_edge_scorers + test_edge_scorers.cpp +) +ament_target_dependencies(test_edge_scorers + ${dependencies} +) +target_link_libraries(test_edge_scorers + ${library_name} edge_scorers +) + +# Test path converter +ament_add_gtest(test_path_converter + test_path_converter.cpp +) +ament_target_dependencies(test_path_converter + ${dependencies} +) +target_link_libraries(test_path_converter + ${library_name} +) + +# Test node spatial tree +ament_add_gtest(test_spatial_tree + test_spatial_tree.cpp +) +ament_target_dependencies(test_spatial_tree + ${dependencies} +) +target_link_libraries(test_spatial_tree + ${library_name} +) + +# Test operation manager + plugins +ament_add_gtest(test_operations + test_operations.cpp +) +ament_target_dependencies(test_operations + ${dependencies} +) +target_link_libraries(test_operations + ${library_name} route_operations +) + +# Test graph loader +ament_add_gtest(test_graph_loader + test_graph_loader.cpp +) +ament_target_dependencies(test_graph_loader + ${dependencies} +) +target_link_libraries(test_graph_loader + ${library_name} +) + +# Test geojson parser +ament_add_gtest(test_geojson_graph_file_loader + test_geojson_graph_file_loader.cpp +) +ament_target_dependencies(test_geojson_graph_file_loader + ${dependencies} +) +target_link_libraries(test_geojson_graph_file_loader + ${library_name} graph_file_loaders +) + +# Test collision monitor seperately due to relative complexity +ament_add_gtest(test_collision_operation + test_collision_operation.cpp +) +ament_target_dependencies(test_collision_operation + ${dependencies} +) +target_link_libraries(test_collision_operation + ${library_name} route_operations +) + +# Test route planner +ament_add_gtest(test_route_planner + test_route_planner.cpp +) +ament_target_dependencies(test_route_planner + ${dependencies} +) +target_link_libraries(test_route_planner + ${library_name} edge_scorers +) + +# Test goal intent extractor +ament_add_gtest(test_goal_intent_extractor + test_goal_intent_extractor.cpp +) +ament_target_dependencies(test_goal_intent_extractor + ${dependencies} +) +target_link_libraries(test_goal_intent_extractor + ${library_name} +) + +# Test route tracker +ament_add_gtest(test_route_tracker + test_route_tracker.cpp +) +ament_target_dependencies(test_route_tracker + ${dependencies} +) +target_link_libraries(test_route_tracker + ${library_name} +) + +# Test route server +ament_add_gtest(test_route_server + test_route_server.cpp +) +ament_target_dependencies(test_route_server + ${dependencies} +) +target_link_libraries(test_route_server + ${library_name} +) # Test breadth first search ament_add_gtest(test_bfs From b9c8f2b2f569a144bcd00124a00b16a82c816196 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 May 2023 18:20:03 -0400 Subject: [PATCH 14/36] clean up --- nav2_route/src/breadth_first_search.cpp | 2 +- nav2_route/test/performance_benchmarking.cpp | 11 +++++------ 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 911b9ef999b..7e8f1522526 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -167,4 +167,4 @@ bool BreadthFirstSearch::inCollision(unsigned int index) { return false; } -} // namespace nav2_route \ No newline at end of file +} // namespace nav2_route diff --git a/nav2_route/test/performance_benchmarking.cpp b/nav2_route/test/performance_benchmarking.cpp index c123e2557b0..a4a9682c0b1 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++; @@ -118,8 +118,7 @@ int main(int argc, char const * argv[]) // node->get_logger(), // "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 + // 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(); kd_tree->computeTree(graph); @@ -127,8 +126,8 @@ int main(int argc, char const * argv[]) for (unsigned int i = 0; i != NUM_TESTS; i++) { unsigned int kd_tree_idx; geometry_msgs::msg::PoseStamped pose; - pose.pose.position.x = static_cast(rand_r() % DIM); - pose.pose.position.y = static_cast(rand_r() % DIM); + pose.pose.position.x = static_cast(rand() % DIM); + pose.pose.position.y = static_cast(rand() % DIM); kd_tree->findNearestGraphNodeToPose(pose, kd_tree_idx); } auto end = node->now(); From ff21bc7a43516c67c85836e57b11dce894a6066e Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 May 2023 18:20:46 -0400 Subject: [PATCH 15/36] remove space --- nav2_route/test/test_breadth_first_search.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index c9f1861f461..26b49c9e485 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -76,4 +76,4 @@ TEST_F(BFSTestFixture, wall) EXPECT_EQ(closest_goal.x, mxs.front()); EXPECT_EQ(closest_goal.y, mys.front()); -} \ No newline at end of file +} From d8ca0f5db20e07552cb582d0aea97e6c8b164f8a Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 26 May 2023 18:21:51 -0400 Subject: [PATCH 16/36] remove colcon ignore --- nav2_system_tests/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 nav2_system_tests/COLCON_IGNORE diff --git a/nav2_system_tests/COLCON_IGNORE b/nav2_system_tests/COLCON_IGNORE deleted file mode 100644 index e69de29bb2d..00000000000 From 38383641bff928edda010e6e09d5827697d113fc Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 30 May 2023 16:28:48 -0400 Subject: [PATCH 17/36] code review --- .../nav2_route/breadth_first_search.hpp | 36 ++--- nav2_route/src/breadth_first_search.cpp | 124 ++++++++------- nav2_route/test/CMakeLists.txt | 146 ------------------ 3 files changed, 79 insertions(+), 227 deletions(-) diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index 5921ea9bfa0..9b83d04aede 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -32,24 +32,24 @@ struct Coordinates Coordinates() = default; Coordinates(const float & x_in, const float & y_in) - : x(x_in), y(y_in) {} + : x(x_in), y(y_in) {} float x, y; }; -struct State +struct Node { + unsigned int index; bool visited{false}; - bool queued{false}; }; class BreadthFirstSearch { - public: +public: + typedef Node * NodePtr; + typedef std::vector NodeVector; - void initMotionModel(int x_size, int y_size); - - void setCostmap(nav2_costmap_2d::Costmap2D *costmap); + void setCostmap(nav2_costmap_2d::Costmap2D * costmap); void setStart(unsigned int mx, unsigned int my); @@ -57,38 +57,30 @@ class BreadthFirstSearch bool search(Coordinates & closest_goal); - private: - +private: inline Coordinates getCoords( - const unsigned int & index) const + const unsigned int & index) const { const unsigned int & width = x_size_; return {static_cast(index % width), static_cast(index / width)}; } - static inline unsigned int getIndex( - const unsigned int & x, const unsigned int & y, const unsigned int & width) - { - return x + y * width; - } + NodePtr addToGraph(const unsigned int index); - bool getNeighbors(unsigned int current, std::vector & neighbors); + bool getNeighbors(unsigned int current, NodeVector & neighbors); bool inCollision(unsigned int index); - std::unordered_map states_; - std::queue queue_; + std::unordered_map graph_; - unsigned int start_; - std::vector goals_; + NodePtr start_; + NodeVector goals_; unsigned int x_size_; unsigned int y_size_; unsigned int max_index_; std::vector neighbors_grid_offsets; - nav2_costmap_2d::Costmap2D * costmap_; - }; } // namespace nav2_route diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 7e8f1522526..8b8be5b3f65 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -15,105 +15,114 @@ #include "nav2_route/breadth_first_search.hpp" #include +#include +#include namespace nav2_route { -void BreadthFirstSearch::initMotionModel(int x_size, int y_size) +void BreadthFirstSearch::setCostmap(nav2_costmap_2d::Costmap2D * costmap) { + std::cout << "Set costmap " << std::endl; + 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, - -x_size - 1, -x_size + 1, - +x_size - 1, +x_size + 1}; + -x_size, +x_size, + -x_size - 1, -x_size + 1, + +x_size - 1, +x_size + 1}; std::cout << "offset size " << neighbors_grid_offsets.size() << std::endl; } -void BreadthFirstSearch::setCostmap(nav2_costmap_2d::Costmap2D *costmap) +BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int index) { - std::cout << "Set costmap " << std::endl; - costmap_ = costmap; - - unsigned int x_size = costmap_->getSizeInCellsX(); - unsigned int y_size = costmap_->getSizeInCellsY(); + auto iter = graph_.find(index); + if (iter != graph_.end()) { + return &(iter->second); + } - initMotionModel(static_cast(x_size), static_cast(y_size)); + return &(graph_.emplace(index, Node{index, false}).first->second); } -void BreadthFirstSearch::setStart(unsigned int mx, unsigned int my) { - start_ = getIndex(mx, my, x_size_); - states_.insert(std::make_pair(start_, State())); +void BreadthFirstSearch::setStart(unsigned int mx, unsigned int my) +{ + start_ = addToGraph(costmap_->getIndex(mx, my)); } void BreadthFirstSearch::setGoals(std::vector mxs, std::vector mys) { goals_.clear(); - for(unsigned int i = 0; i < (mxs.size() && mys.size()); ++i) { - unsigned int index = getIndex(mxs[i], mys[i], x_size_); - goals_.push_back(index); - states_.insert(std::make_pair(index, State())); + for (unsigned int i = 0; i < (mxs.size() && mys.size()); ++i) { + goals_.emplace_back(addToGraph(costmap_->getIndex(mxs[i], mys[i]))); } } bool BreadthFirstSearch::search(Coordinates & closest_goal) { - std::cout << "Start " << std::endl; - states_.clear(); + // clear the graph + std::unordered_map empty_graph; + std::swap(graph_, empty_graph); - // clear the queue - std::queue empty_queue; - queue_.swap(empty_queue); + std::queue queue; - //Add start to queue - queue_.push(start_); + start_->visited = true; + queue.push(start_); - std::cout << "Added start " << std::endl; + std::cout << "Start index " << start_->index << std::endl; + std::cout << "Goal index " << goals_[0]->index << std::endl; - while (!queue_.empty()) { + int iter = 0; + while (!queue.empty()) { + iter += 1; + std::cout << "Iteration " << iter << std::endl; + std::cout << "Queue Size: " << queue.size() << std::endl; - std::cout << "Queue Size: " << queue_.size() << std::endl; + auto & current = queue.front(); - unsigned int current = queue_.front(); std::cout << "grab front" << std::endl; + std::cout << std::endl; - Coordinates current_coord = getCoords(current); - std::cout << "Current Node: " << current_coord.x << " " << current_coord.y << std::endl; + Coordinates current_coord = getCoords(current->index); + std::cout << "CURRENT NODE: " << current_coord.x << " " << current_coord.y << std::endl; - queue_.pop(); - states_[current].visited = true; - states_[current].queued = false; + queue.pop(); // Check goals - for(const auto &goal : goals_) { - if (current == goal) { + for (const auto & goal : goals_) { + std::cout << "Goal index " << goal->index << std::endl; + std::cout << "current index " << current->index << std::endl; + if (current->index == goal->index) { std::cout << "Found goal" << std::endl; - closest_goal = getCoords(current); + closest_goal = getCoords(current->index); return true; } } std::cout << "Checked goals" << std::endl; - std::vector neighbors; - getNeighbors(current, neighbors); + NodeVector neighbors; + getNeighbors(current->index, neighbors); std::cout << "Got neigbors" << std::endl; - for(const auto neighbor : neighbors) - { - if(!states_[neighbor].queued && !states_[neighbor].visited) { - states_[neighbor].queued = true; - queue_.push(neighbor); + for (const auto neighbor : neighbors) { + if (!neighbor->visited) { + neighbor->visited = true; + queue.push(neighbor); - std::cout << "Added neighbor: " << neighbor << std::endl; + std::cout << "Added node: " << neighbor->index << std::endl; } } - } + } return false; } -bool BreadthFirstSearch::getNeighbors(unsigned int current, std::vector & neighbors) +bool BreadthFirstSearch::getNeighbors(unsigned int current, NodeVector & neighbors) { const Coordinates p_coord = getCoords(current); @@ -121,8 +130,7 @@ bool BreadthFirstSearch::getNeighbors(unsigned int current, std::vectorgetCost(index); if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || - cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { std::cout << "In collision" << std::endl; return true; @@ -167,4 +173,4 @@ bool BreadthFirstSearch::inCollision(unsigned int index) { return false; } -} // namespace nav2_route +} // namespace nav2_route diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt index 550ec741ece..ab076c9fc86 100644 --- a/nav2_route/test/CMakeLists.txt +++ b/nav2_route/test/CMakeLists.txt @@ -1,149 +1,3 @@ -# Route planner benchmarking script -add_executable(performance_benchmarking performance_benchmarking.cpp) -target_link_libraries(performance_benchmarking - ${library_name} -) -ament_target_dependencies(performance_benchmarking - ${dependencies} -) -install(TARGETS - performance_benchmarking - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -# Test utilities and basic types -ament_add_gtest(test_utils_and_types - test_utils_and_types.cpp -) -ament_target_dependencies(test_utils_and_types - ${dependencies} -) -target_link_libraries(test_utils_and_types - ${library_name} -) - -# Test edge scorer + plugins -ament_add_gtest(test_edge_scorers - test_edge_scorers.cpp -) -ament_target_dependencies(test_edge_scorers - ${dependencies} -) -target_link_libraries(test_edge_scorers - ${library_name} edge_scorers -) - -# Test path converter -ament_add_gtest(test_path_converter - test_path_converter.cpp -) -ament_target_dependencies(test_path_converter - ${dependencies} -) -target_link_libraries(test_path_converter - ${library_name} -) - -# Test node spatial tree -ament_add_gtest(test_spatial_tree - test_spatial_tree.cpp -) -ament_target_dependencies(test_spatial_tree - ${dependencies} -) -target_link_libraries(test_spatial_tree - ${library_name} -) - -# Test operation manager + plugins -ament_add_gtest(test_operations - test_operations.cpp -) -ament_target_dependencies(test_operations - ${dependencies} -) -target_link_libraries(test_operations - ${library_name} route_operations -) - -# Test graph loader -ament_add_gtest(test_graph_loader - test_graph_loader.cpp -) -ament_target_dependencies(test_graph_loader - ${dependencies} -) -target_link_libraries(test_graph_loader - ${library_name} -) - -# Test geojson parser -ament_add_gtest(test_geojson_graph_file_loader - test_geojson_graph_file_loader.cpp -) -ament_target_dependencies(test_geojson_graph_file_loader - ${dependencies} -) -target_link_libraries(test_geojson_graph_file_loader - ${library_name} graph_file_loaders -) - -# Test collision monitor seperately due to relative complexity -ament_add_gtest(test_collision_operation - test_collision_operation.cpp -) -ament_target_dependencies(test_collision_operation - ${dependencies} -) -target_link_libraries(test_collision_operation - ${library_name} route_operations -) - -# Test route planner -ament_add_gtest(test_route_planner - test_route_planner.cpp -) -ament_target_dependencies(test_route_planner - ${dependencies} -) -target_link_libraries(test_route_planner - ${library_name} edge_scorers -) - -# Test goal intent extractor -ament_add_gtest(test_goal_intent_extractor - test_goal_intent_extractor.cpp -) -ament_target_dependencies(test_goal_intent_extractor - ${dependencies} -) -target_link_libraries(test_goal_intent_extractor - ${library_name} -) - -# Test route tracker -ament_add_gtest(test_route_tracker - test_route_tracker.cpp -) -ament_target_dependencies(test_route_tracker - ${dependencies} -) -target_link_libraries(test_route_tracker - ${library_name} -) - -# Test route server -ament_add_gtest(test_route_server - test_route_server.cpp -) -ament_target_dependencies(test_route_server - ${dependencies} -) -target_link_libraries(test_route_server - ${library_name} -) - -# Test breadth first search ament_add_gtest(test_bfs test_breadth_first_search.cpp ) From 8ba4196b3ce9e4db7ff31f38140e84be1c7c087f Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 31 May 2023 17:20:02 -0400 Subject: [PATCH 18/36] code review --- nav2_route/CMakeLists.txt | 10 +- .../nav2_route/breadth_first_search.hpp | 4 +- nav2_route/src/breadth_first_search.cpp | 23 ++- nav2_route/test/CMakeLists.txt | 154 +++++++++++++++++- 4 files changed, 164 insertions(+), 27 deletions(-) diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index f7defe56326..338f04f0bb5 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/breadth_first_search.cpp ) ament_target_dependencies(${library_name} @@ -111,14 +112,6 @@ ament_target_dependencies(graph_file_loaders pluginlib_export_plugin_description_file(nav2_route plugins.xml) -# Breadth first search library -add_library(breadth_first_search SHARED - src/breadth_first_search.cpp) - -ament_target_dependencies(breadth_first_search - ${dependencies} -) - install(DIRECTORY include/ DESTINATION include/ ) @@ -131,7 +124,6 @@ install(TARGETS ${library_name} edge_scorers route_operations graph_file_loaders - breadth_first_search ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index 9b83d04aede..f508d0928dc 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -40,7 +40,7 @@ struct Coordinates struct Node { unsigned int index; - bool visited{false}; + bool explored{false}; }; class BreadthFirstSearch @@ -79,7 +79,7 @@ class BreadthFirstSearch unsigned int x_size_; unsigned int y_size_; unsigned int max_index_; - std::vector neighbors_grid_offsets; + std::vector neighbors_grid_offsets_; nav2_costmap_2d::Costmap2D * costmap_; }; } // namespace nav2_route diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 8b8be5b3f65..47927255ace 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -33,12 +33,12 @@ void BreadthFirstSearch::setCostmap(nav2_costmap_2d::Costmap2D * costmap) max_index_ = x_size * y_size; x_size_ = x_size; - neighbors_grid_offsets = {-1, +1, + neighbors_grid_offsets_ = {-1, +1, -x_size, +x_size, -x_size - 1, -x_size + 1, +x_size - 1, +x_size + 1}; - std::cout << "offset size " << neighbors_grid_offsets.size() << std::endl; + std::cout << "offset size " << neighbors_grid_offsets_.size() << std::endl; } BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int index) @@ -54,25 +54,23 @@ BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int in void BreadthFirstSearch::setStart(unsigned int mx, unsigned int my) { start_ = addToGraph(costmap_->getIndex(mx, my)); + std::cout << "Start index : " << costmap_->getIndex(mx, my) << std::endl; } void BreadthFirstSearch::setGoals(std::vector mxs, std::vector mys) { goals_.clear(); for (unsigned int i = 0; i < (mxs.size() && mys.size()); ++i) { + std::cout << "Goal index: " << costmap_->getIndex(mxs[i], mys[i]) << std::endl; goals_.emplace_back(addToGraph(costmap_->getIndex(mxs[i], mys[i]))); } } bool BreadthFirstSearch::search(Coordinates & closest_goal) { - // clear the graph - std::unordered_map empty_graph; - std::swap(graph_, empty_graph); - std::queue queue; - start_->visited = true; + start_->explored = true; queue.push(start_); std::cout << "Start index " << start_->index << std::endl; @@ -101,6 +99,7 @@ bool BreadthFirstSearch::search(Coordinates & closest_goal) if (current->index == goal->index) { std::cout << "Found goal" << std::endl; closest_goal = getCoords(current->index); + graph_.clear(); return true; } } @@ -111,14 +110,15 @@ bool BreadthFirstSearch::search(Coordinates & closest_goal) std::cout << "Got neigbors" << std::endl; for (const auto neighbor : neighbors) { - if (!neighbor->visited) { - neighbor->visited = true; + if (!neighbor->explored) { + neighbor->explored = true; queue.push(neighbor); - std::cout << "Added node: " << neighbor->index << std::endl; } } } + + graph_.clear(); return false; } @@ -129,8 +129,7 @@ bool BreadthFirstSearch::getNeighbors(unsigned int current, NodeVector & neighbo unsigned int index; Coordinates c_coord{0, 0}; - std::cout << "grid offset size " << neighbors_grid_offsets.size() << std::endl; - for (const int & neighbors_grid_offset : neighbors_grid_offsets) { + for (const int & neighbors_grid_offset : neighbors_grid_offsets_) { std::cout << "Getting neighbors" << std::endl; index = current + neighbors_grid_offset; diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt index ab076c9fc86..657475448c3 100644 --- a/nav2_route/test/CMakeLists.txt +++ b/nav2_route/test/CMakeLists.txt @@ -1,9 +1,155 @@ -ament_add_gtest(test_bfs - test_breadth_first_search.cpp +# Route planner benchmarking script +add_executable(performance_benchmarking performance_benchmarking.cpp) +target_link_libraries(performance_benchmarking + ${library_name} +) +ament_target_dependencies(performance_benchmarking + ${dependencies} +) +install(TARGETS + performance_benchmarking + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +# Test utilities and basic types +ament_add_gtest(test_utils_and_types + test_utils_and_types.cpp +) +ament_target_dependencies(test_utils_and_types + ${dependencies} +) +target_link_libraries(test_utils_and_types + ${library_name} +) + +# Test edge scorer + plugins +ament_add_gtest(test_edge_scorers + test_edge_scorers.cpp +) +ament_target_dependencies(test_edge_scorers + ${dependencies} +) +target_link_libraries(test_edge_scorers + ${library_name} edge_scorers +) + +# Test path converter +ament_add_gtest(test_path_converter + test_path_converter.cpp +) +ament_target_dependencies(test_path_converter + ${dependencies} ) -ament_target_dependencies(test_bfs +target_link_libraries(test_path_converter + ${library_name} +) + +# Test node spatial tree +ament_add_gtest(test_spatial_tree + test_spatial_tree.cpp +) +ament_target_dependencies(test_spatial_tree + ${dependencies} +) +target_link_libraries(test_spatial_tree + ${library_name} +) + +# Test operation manager + plugins +ament_add_gtest(test_operations + test_operations.cpp +) +ament_target_dependencies(test_operations + ${dependencies} +) +target_link_libraries(test_operations + ${library_name} route_operations +) + +# Test graph loader +ament_add_gtest(test_graph_loader + test_graph_loader.cpp +) +ament_target_dependencies(test_graph_loader ${dependencies} ) +target_link_libraries(test_graph_loader + ${library_name} +) + +# Test geojson parser +ament_add_gtest(test_geojson_graph_file_loader + test_geojson_graph_file_loader.cpp +) +ament_target_dependencies(test_geojson_graph_file_loader + ${dependencies} +) +target_link_libraries(test_geojson_graph_file_loader + ${library_name} graph_file_loaders +) + +# Test collision monitor seperately due to relative complexity +ament_add_gtest(test_collision_operation + test_collision_operation.cpp +) +ament_target_dependencies(test_collision_operation + ${dependencies} +) +target_link_libraries(test_collision_operation + ${library_name} route_operations +) + +# Test route planner +ament_add_gtest(test_route_planner + test_route_planner.cpp +) +ament_target_dependencies(test_route_planner + ${dependencies} +) +target_link_libraries(test_route_planner + ${library_name} edge_scorers +) + +# Test goal intent extractor +ament_add_gtest(test_goal_intent_extractor + test_goal_intent_extractor.cpp +) +ament_target_dependencies(test_goal_intent_extractor + ${dependencies} +) +target_link_libraries(test_goal_intent_extractor + ${library_name} +) + +# Test route tracker +ament_add_gtest(test_route_tracker + test_route_tracker.cpp +) +ament_target_dependencies(test_route_tracker + ${dependencies} +) +target_link_libraries(test_route_tracker + ${library_name} +) + +# Test route server +ament_add_gtest(test_route_server + test_route_server.cpp +) +ament_target_dependencies(test_route_server + ${dependencies} +) +target_link_libraries(test_route_server + ${library_name} +) + +# Test Breadth first search +ament_add_gtest(test_bfs + test_breadth_first_search.cpp +) +ament_target_dependencies(test_bfs + ${dependencies} +) target_link_libraries(test_bfs - ${library_name} breadth_first_search + ${library_name} ) From 8228cd645db155e953dd4226715e6beb5703a0e7 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 1 Jun 2023 17:07:02 -0400 Subject: [PATCH 19/36] clean up --- .../nav2_route/breadth_first_search.hpp | 73 +++++++++++++------ nav2_route/src/breadth_first_search.cpp | 69 ++++++------------ nav2_route/test/test_breadth_first_search.cpp | 14 ++-- 3 files changed, 80 insertions(+), 76 deletions(-) diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index f508d0928dc..a71b014b020 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -1,3 +1,4 @@ +// Copyright (c) 2020, Samsung Research America // Copyright (c) 2023 Joshua Wallace // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -15,7 +16,6 @@ #ifndef NAV2_ROUTE__BREADTH_FIRST_SEARCH_HPP_ #define NAV2_ROUTE__BREADTH_FIRST_SEARCH_HPP_ - #include #include #include @@ -27,51 +27,78 @@ namespace nav2_route { -struct Coordinates +/** + * @struct nav2_route::SimpleNode + * @brief A Node implementation for the breadth first search + */ +struct SimpleNode { - Coordinates() = default; - - Coordinates(const float & x_in, const float & y_in) - : x(x_in), y(y_in) {} - - float x, y; -}; + SimpleNode(unsigned int index) + : index(index), + explored(false) {} -struct Node -{ unsigned int index; - bool explored{false}; + bool explored; }; +/** + * @class nav2_route::BreadthFirstSearch + * @brief Preforms a breadth first search between the start and goal + */ class BreadthFirstSearch { public: - typedef Node * NodePtr; + typedef SimpleNode * NodePtr; typedef std::vector 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 mxs The array x map coordinate of the goals + * @param my The array y map coordinate of the goals + */ void setGoals(std::vector mxs, std::vector mys); - bool search(Coordinates & closest_goal); + /** + * @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); private: - inline Coordinates getCoords( - const unsigned int & index) const - { - const unsigned int & width = x_size_; - return {static_cast(index % width), static_cast(index / width)}; - } - + /** + * @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); - bool getNeighbors(unsigned int current, NodeVector & neighbors); + /** + * @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 graph_; + std::unordered_map graph_; NodePtr start_; NodeVector goals_; diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 47927255ace..64c54e929f7 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -1,3 +1,4 @@ +// Copyright (c) 2020 Samsung Research America // Copyright (c) 2023 Joshua Wallace // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -29,7 +30,6 @@ void BreadthFirstSearch::setCostmap(nav2_costmap_2d::Costmap2D * 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; @@ -37,8 +37,6 @@ void BreadthFirstSearch::setCostmap(nav2_costmap_2d::Costmap2D * costmap) -x_size, +x_size, -x_size - 1, -x_size + 1, +x_size - 1, +x_size + 1}; - - std::cout << "offset size " << neighbors_grid_offsets_.size() << std::endl; } BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int index) @@ -48,72 +46,49 @@ BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int in return &(iter->second); } - return &(graph_.emplace(index, Node{index, false}).first->second); + return &(graph_.emplace(index, SimpleNode(index)).first->second); } void BreadthFirstSearch::setStart(unsigned int mx, unsigned int my) { start_ = addToGraph(costmap_->getIndex(mx, my)); - std::cout << "Start index : " << costmap_->getIndex(mx, my) << std::endl; } void BreadthFirstSearch::setGoals(std::vector mxs, std::vector mys) { goals_.clear(); for (unsigned int i = 0; i < (mxs.size() && mys.size()); ++i) { - std::cout << "Goal index: " << costmap_->getIndex(mxs[i], mys[i]) << std::endl; goals_.emplace_back(addToGraph(costmap_->getIndex(mxs[i], mys[i]))); } } -bool BreadthFirstSearch::search(Coordinates & closest_goal) +bool BreadthFirstSearch::search(unsigned int & closest_goal_index) { std::queue queue; start_->explored = true; queue.push(start_); - std::cout << "Start index " << start_->index << std::endl; - std::cout << "Goal index " << goals_[0]->index << std::endl; - - int iter = 0; while (!queue.empty()) { - iter += 1; - std::cout << "Iteration " << iter << std::endl; - std::cout << "Queue Size: " << queue.size() << std::endl; - auto & current = queue.front(); - - std::cout << "grab front" << std::endl; - std::cout << std::endl; - - Coordinates current_coord = getCoords(current->index); - std::cout << "CURRENT NODE: " << current_coord.x << " " << current_coord.y << std::endl; - queue.pop(); // Check goals for (const auto & goal : goals_) { - std::cout << "Goal index " << goal->index << std::endl; - std::cout << "current index " << current->index << std::endl; if (current->index == goal->index) { - std::cout << "Found goal" << std::endl; - closest_goal = getCoords(current->index); + closest_goal_index = current->index; graph_.clear(); return true; } } - std::cout << "Checked goals" << std::endl; NodeVector neighbors; getNeighbors(current->index, neighbors); - std::cout << "Got neigbors" << std::endl; for (const auto neighbor : neighbors) { if (!neighbor->explored) { neighbor->explored = true; queue.push(neighbor); - std::cout << "Added node: " << neighbor->index << std::endl; } } } @@ -122,41 +97,42 @@ bool BreadthFirstSearch::search(Coordinates & closest_goal) return false; } -bool BreadthFirstSearch::getNeighbors(unsigned int current, NodeVector & neighbors) +void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & neighbors) { - const Coordinates p_coord = getCoords(current); - - unsigned int index; - Coordinates c_coord{0, 0}; + unsigned int p_mx, p_my; + costmap_->indexToCells(parent_index, p_mx, p_my); + unsigned int neigbhor_index; for (const int & neighbors_grid_offset : neighbors_grid_offsets_) { - std::cout << "Getting neighbors" << std::endl; - index = current + neighbors_grid_offset; + // Check if index is negative + int index = parent_index + neighbors_grid_offset; + if(index < 0){ + continue; + } + + neigbhor_index = static_cast(index); - c_coord = getCoords(index); + unsigned int n_mx, n_my; + costmap_->indexToCells(neigbhor_index, n_mx, n_my); // Check for wrap around conditions - if (std::fabs(p_coord.x - c_coord.x) > 1 || std::fabs(p_coord.y - c_coord.y) > 1) { - std::cout << "Index wraps" << std::endl; + 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 (index >= max_index_) { - std::cout << "Max index hit" << std::endl; + if (neigbhor_index >= max_index_) { continue; } - if (inCollision(index)) { - std::cout << "In collision" << std::endl; + if (inCollision(neigbhor_index)) { continue; } - std::cout << "Adding neighbor to vector" << std::endl; - auto neighbor = addToGraph(index); + auto neighbor = addToGraph(neigbhor_index); neighbors.push_back(neighbor); } - return false; } bool BreadthFirstSearch::inCollision(unsigned int index) @@ -166,7 +142,6 @@ bool BreadthFirstSearch::inCollision(unsigned int index) if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { - std::cout << "In collision" << std::endl; return true; } return false; diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index 26b49c9e485..010e342915d 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -46,12 +46,13 @@ TEST_F(BFSTestFixture, free_space) std::vector mys = {1u}; bfs.setGoals(mxs, mys); - Coordinates closest_goal; + unsigned int closest_goal; bool result = bfs.search(closest_goal); EXPECT_TRUE(result); - EXPECT_EQ(closest_goal.x, mxs.front()); - EXPECT_EQ(closest_goal.y, mys.front()); + auto index = costmap->getIndex(mxs.front(), mys.front()); + + EXPECT_EQ(closest_goal, index); } TEST_F(BFSTestFixture, wall) @@ -70,10 +71,11 @@ TEST_F(BFSTestFixture, wall) bfs.setGoals(mxs, mys); - Coordinates closest_goal; + unsigned int closest_goal; bool result = bfs.search(closest_goal); EXPECT_TRUE(result); - EXPECT_EQ(closest_goal.x, mxs.front()); - EXPECT_EQ(closest_goal.y, mys.front()); + auto index = costmap->getIndex(mxs.front(), mys.front()); + + EXPECT_EQ(closest_goal, index); } From 50518f74e89d89689f9b60099a382f0ffe580ba4 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 2 Jun 2023 16:58:47 -0400 Subject: [PATCH 20/36] integrated bfs into goal_intent_extractor --- .../nav2_route/breadth_first_search.hpp | 23 ++++++--- .../nav2_route/goal_intent_extractor.hpp | 17 +++++++ nav2_route/src/breadth_first_search.cpp | 49 ++++++++++++++----- nav2_route/src/goal_intent_extractor.cpp | 45 +++++++++++++++++ nav2_route/test/test_breadth_first_search.cpp | 48 ++++++++++-------- 5 files changed, 143 insertions(+), 39 deletions(-) diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index a71b014b020..fa7f90ef96f 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/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 mxs, std::vector 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 graph_; NodePtr start_; - NodeVector goals_; + NodePtr goal_; unsigned int x_size_; unsigned int y_size_; diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index fb00120bd53..6904eeeed0a 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/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 +#include #include #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" @@ -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 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 costmap_sub_; + std::unique_ptr bfs_; }; } // namespace nav2_route diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 64c54e929f7..48d11bb1cd1 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -18,6 +18,8 @@ #include #include #include +#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 mxs, std::vector 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 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(p_mx) - static_cast(n_mx)) > 1 || std::fabs(static_cast(p_my) - static_cast(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 diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index 0def6329209..03834df4984 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/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 +#include +#include #include #include #include +#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(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(node, global_costmap_topic); + bfs_ = std::make_unique(); + 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 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( const Route & input_route, const std::shared_ptr goal, diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index 010e342915d..a1683e778ed 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/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 mxs = {1u}; - std::vector 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 mxs = {2u, 5u}; - std::vector 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(); } From e2ff295780b24aac217a66965b1877096f7796a4 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 6 Jun 2023 11:30:10 -0400 Subject: [PATCH 21/36] api change --- nav2_route/CMakeLists.txt | 2 +- .../nav2_route/breadth_first_search.hpp | 10 +++--- nav2_route/package.xml | 1 + nav2_route/src/breadth_first_search.cpp | 33 +++++++++++-------- nav2_route/src/goal_intent_extractor.cpp | 9 +++-- nav2_route/test/test_breadth_first_search.cpp | 30 +++++++++++------ 6 files changed, 54 insertions(+), 31 deletions(-) diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index 338f04f0bb5..ab482abf358 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -146,5 +146,5 @@ ament_export_libraries(${library_name} edge_scorers route_operations graph_file_loaders - breadth_first_search) +) ament_package() diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index fa7f90ef96f..a7ede1b2927 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -67,16 +67,16 @@ class BreadthFirstSearch /** * @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 + * @param goals The array of goals to search for */ - void setGoal(unsigned int mx, unsigned int my); + 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 that was found by the search * @return True if the search was successful */ - bool search(); + bool search(unsigned int & goal); /** * @brief Preform a ray trace check to see if the node is directly visable @@ -112,7 +112,7 @@ class BreadthFirstSearch std::unordered_map graph_; NodePtr start_; - NodePtr goal_; + NodeVector goals_; unsigned int x_size_; unsigned int y_size_; diff --git a/nav2_route/package.xml b/nav2_route/package.xml index ef023dc604d..fe117050f93 100644 --- a/nav2_route/package.xml +++ b/nav2_route/package.xml @@ -22,6 +22,7 @@ nav2_core lib-nanoflann nlohmann-json + nav2_smac_planner ament_lint_common ament_lint_auto diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 48d11bb1cd1..6d04b39c4e9 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -18,7 +18,9 @@ #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 @@ -56,12 +58,15 @@ void BreadthFirstSearch::setStart(unsigned int mx, unsigned int my) start_ = addToGraph(costmap_->getIndex(mx, my)); } -void BreadthFirstSearch::setGoal(unsigned int mx, unsigned int my) +void BreadthFirstSearch::setGoals(std::vector & goals) { - goal_ = addToGraph(costmap_->getIndex(mx, my)); + goals_.clear(); + for (const auto & goal : goals) { + goals_.push_back(addToGraph(costmap_->getIndex(goal.x, goal.y))); + } } -bool BreadthFirstSearch::search() +bool BreadthFirstSearch::search(unsigned int & goal) { std::queue queue; @@ -75,8 +80,11 @@ bool BreadthFirstSearch::search() std::cout << "Current index: " << current->index << std::endl; // Check goals - if (current->index == goal_->index) { - return true; + for (unsigned int index = 0; index < goals_.size(); ++index) { + if (current->index == goals_[index]->index) { + goal = index; + return true; + } } NodeVector neighbors; @@ -98,7 +106,7 @@ void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & ne unsigned int p_mx, p_my; costmap_->indexToCells(parent_index, p_mx, p_my); - unsigned int neigbhor_index; + 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; @@ -108,10 +116,10 @@ void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & ne continue; } - neigbhor_index = static_cast(index); + neighbor_index = static_cast(index); unsigned int n_mx, n_my; - costmap_->indexToCells(neigbhor_index, 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 || @@ -121,16 +129,15 @@ void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & ne } // Check for out of bounds - if (neigbhor_index >= max_index_) { + if (neighbor_index >= max_index_) { continue; } - if (inCollision(neigbhor_index)) { + if (inCollision(neighbor_index)) { continue; } - auto neighbor = addToGraph(neigbhor_index); - neighbors.push_back(neighbor); + neighbors.push_back(addToGraph(neighbor_index)); } } @@ -138,7 +145,7 @@ 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); + 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.getX()); diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index 03834df4984..df0d579e929 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -19,6 +19,7 @@ #include #include +#include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_route/breadth_first_search.hpp" #include "nav2_route/goal_intent_extractor.hpp" @@ -223,18 +224,22 @@ bool GoalIntentExtractor::isNodeValid(unsigned int node_index, 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); + std::vector goals; + goals.push_back({g_mx, g_my}); bfs_->setStart(s_mx, s_my); - bfs_->setGoal(g_mx, g_my); + bfs_->setGoals(goals); if (bfs_->isNodeVisible()) { bfs_->clearGraph(); return true; } - if (bfs_->search()) { + unsigned int goal; + if (bfs_->search(goal)) { bfs_->clearGraph(); return true; } + RCLCPP_INFO_STREAM(logger_, "goal index" << goal); return false; } diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index a1683e778ed..7f697e607fe 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/cost_values.hpp" @@ -41,12 +42,15 @@ TEST_F(BFSTestFixture, free_space) bfs.setStart(0u, 0u); - unsigned int mx = 1u; - unsigned int my = 1u; - bfs.setGoal(mx, my); + std::vector goals; + goals.push_back({1u, 1u}); + bfs.setGoals(goals); - bool result = bfs.search(); + unsigned int goal; + bool result = bfs.search(goal); EXPECT_TRUE(result); + + EXPECT_EQ(costmap->getIndex(goals[goal].x, goals[goal].y), costmap->getIndex(1u, 1u)); bfs.clearGraph(); } @@ -61,13 +65,16 @@ TEST_F(BFSTestFixture, wall) bfs.setStart(0u, 0u); - unsigned int mx = 5u; - unsigned int my = 0u; + std::vector goals; + goals.push_back({5u, 0u}); + goals.push_back({0u, 4u}); + bfs.setGoals(goals); - bfs.setGoal(mx, my); + unsigned int goal; + bool result = bfs.search(goal); + EXPECT_TRUE(result); - bool result = bfs.search(); - EXPECT_FALSE(result); + EXPECT_EQ(costmap->getIndex(goals[goal].x, goals[goal].y), costmap->getIndex(0u, 4u)); bfs.clearGraph(); } @@ -76,7 +83,10 @@ TEST_F(BFSTestFixture, ray_trace) initialize(10u, 10u); bfs.setStart(0u, 0u); - bfs.setGoal(5u, 5u); + + std::vector goals; + goals.push_back({5u, 5u}); + bfs.setGoals(goals); bool result = bfs.isNodeVisible(); EXPECT_TRUE(result); From ba3cbe92d4c1986a0fd80be80e0f5c615c5f873f Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 15 Jun 2023 20:17:42 -0400 Subject: [PATCH 22/36] integrate bfs into the rough server --- nav2_msgs/action/ComputeAndTrackRoute.action | 6 + nav2_msgs/action/ComputeRoute.action | 6 + .../nav2_route/breadth_first_search.hpp | 12 +- .../nav2_route/goal_intent_extractor.hpp | 8 +- .../include/nav2_route/node_spatial_tree.hpp | 13 ++- nav2_route/src/breadth_first_search.cpp | 20 +++- nav2_route/src/goal_intent_extractor.cpp | 108 +++++++++++++----- nav2_route/src/node_spatial_tree.cpp | 23 +++- nav2_route/src/route_server.cpp | 22 ++++ nav2_route/test/performance_benchmarking.cpp | 7 +- nav2_route/test/test_breadth_first_search.cpp | 12 +- nav2_route/test/test_spatial_tree.cpp | 17 ++- 12 files changed, 188 insertions(+), 66 deletions(-) diff --git a/nav2_msgs/action/ComputeAndTrackRoute.action b/nav2_msgs/action/ComputeAndTrackRoute.action index b10629bb145..0ea7c8cd2fa 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 c702e32a47e..72587f5d801 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/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index a7ede1b2927..52efee96c2b 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -53,10 +53,10 @@ class BreadthFirstSearch typedef std::vector NodeVector; /** - * @brief Set the costmap to use + * @brief Initialize the search algorithm * @param costmap Costmap to use to check for state validity */ - void setCostmap(nav2_costmap_2d::Costmap2D * costmap); + void initialize(nav2_costmap_2d::Costmap2D * costmap, int max_iterations); /** * @brief Set the start of the breadth first search @@ -73,10 +73,11 @@ class BreadthFirstSearch /** * @brief Find the closest goal to the start given a costmap, start and goal - * @param goal The index of the goal that was found by the search - * @return True if the search was successful + * @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 */ - bool search(unsigned int & goal); + void search(unsigned int & goal); /** * @brief Preform a ray trace check to see if the node is directly visable @@ -119,6 +120,7 @@ class BreadthFirstSearch unsigned int max_index_; std::vector neighbors_grid_offsets_; nav2_costmap_2d::Costmap2D * costmap_; + int max_iterations_; }; } // namespace nav2_route diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index 6904eeeed0a..fe9908b9e44 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -120,9 +120,12 @@ class GoalIntentExtractor * @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 + * @throws nav2_core::StartOutsideMapBounds If the start index is not in the costmap + * @throws nav2_core::StartOccupied If the start is in lethal cost */ - bool isNodeValid(unsigned int node_index, const geometry_msgs::msg::PoseStamped & pose); + void findValidGraphNode(std::vector node_indices, + const geometry_msgs::msg::PoseStamped & pose, + unsigned int & best_node_index); protected: rclcpp::Logger logger_{rclcpp::get_logger("GoalIntentExtractor")}; @@ -138,6 +141,7 @@ class GoalIntentExtractor std::unique_ptr costmap_sub_; std::unique_ptr bfs_; + bool enable_search_; }; } // 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 679ee63b4ea..9088365862d 100644 --- a/nav2_route/include/nav2_route/node_spatial_tree.hpp +++ b/nav2_route/include/nav2_route/node_spatial_tree.hpp @@ -70,7 +70,9 @@ class NodeSpatialTree /** * @brief Constructor */ - NodeSpatialTree() = default; + NodeSpatialTree(int num_of_nearest_nodes); + + NodeSpatialTree() = delete; /** * @brief Destructor @@ -86,17 +88,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_; + kd_tree_t * kdtree_{nullptr}; GraphAdaptor * adaptor_; Graph * graph_; + int num_of_nearest_nodes_; }; } // namespace nav2_route diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 6d04b39c4e9..bb4a64b64f4 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -19,6 +19,7 @@ #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" @@ -26,7 +27,7 @@ namespace nav2_route { -void BreadthFirstSearch::setCostmap(nav2_costmap_2d::Costmap2D * costmap) +void BreadthFirstSearch::initialize(nav2_costmap_2d::Costmap2D * costmap, int max_iterations) { std::cout << "Set costmap " << std::endl; costmap_ = costmap; @@ -41,6 +42,8 @@ void BreadthFirstSearch::setCostmap(nav2_costmap_2d::Costmap2D * costmap) -x_size, +x_size, -x_size - 1, -x_size + 1, +x_size - 1, +x_size + 1}; + + max_iterations_ = max_iterations; } BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int index) @@ -66,24 +69,31 @@ void BreadthFirstSearch::setGoals(std::vector & go } } -bool BreadthFirstSearch::search(unsigned int & goal) +void BreadthFirstSearch::search(unsigned int & goal) { std::queue queue; start_->explored = true; queue.push(start_); + int iteration = 0; while (!queue.empty()) { auto & current = queue.front(); queue.pop(); + if (iteration > max_iterations_) { + graph_.clear(); + throw nav2_core::PlannerTimedOut("Exceeded maximum iterations"); + } + std::cout << "Current index: " << current->index << std::endl; // Check goals for (unsigned int index = 0; index < goals_.size(); ++index) { if (current->index == goals_[index]->index) { goal = index; - return true; + graph_.clear(); + return; } } @@ -98,7 +108,8 @@ bool BreadthFirstSearch::search(unsigned int & goal) } } - return false; + graph_.clear(); + throw nav2_core::NoValidPathCouldBeFound("No valid path found"); } void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & neighbors) @@ -157,6 +168,7 @@ bool BreadthFirstSearch::isNodeVisible() return false; } } + graph_.clear(); return true; } diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index df0d579e929..0c2c28e5dba 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -19,6 +19,8 @@ #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/breadth_first_search.hpp" @@ -44,8 +46,6 @@ void GoalIntentExtractor::configure( 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)); @@ -61,14 +61,34 @@ void GoalIntentExtractor::configure( node, "min_dist_from_start", rclcpp::ParameterValue(0.10)); min_dist_from_start_ = static_cast(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); + 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); - costmap_sub_ = std::make_unique(node, global_costmap_topic); - bfs_ = std::make_unique(); - bfs_->setCostmap(costmap_sub_->getCostmap().get()); + nav2_util::declare_parameter_if_not_declared( + node, "enable_search", rclcpp::ParameterValue(false)); + enable_search_ = node->get_parameter("enable_search").as_bool(); + + if (enable_search_) { + 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); + + int max_iterations; + nav2_util::declare_parameter_if_not_declared( + node, "max_iterations", rclcpp::ParameterValue(500)); + max_iterations = node->get_parameter("max_iterations").as_int(); + + costmap_sub_ = + std::make_unique(node, global_costmap_topic); + bfs_ = std::make_unique(); + bfs_->initialize(costmap_sub_->getCostmap().get(), max_iterations); + } } void GoalIntentExtractor::setGraph(Graph & graph, GraphToIDMap * id_to_graph_map) @@ -130,19 +150,22 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) // 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!"); } - if (!isNodeValid(start_route, start_)) - { - RCLCPP_INFO(logger_, "Invalid Starting Route index"); + if (enable_search_) { + unsigned int valid_start_route_id, valid_end_route_id; + findValidGraphNode(start_route_ids, start_, valid_start_route_id); + findValidGraphNode(end_route_ids, goal_, valid_end_route_id); + return {valid_end_route_id, valid_end_route_id}; } - return {start_route, end_route}; + + return {start_route_ids.front(), start_route_ids.front()}; } template @@ -213,34 +236,57 @@ Route GoalIntentExtractor::pruneStartandGoal( return pruned_route; } -bool GoalIntentExtractor::isNodeValid(unsigned int node_index, - const geometry_msgs::msg::PoseStamped & pose) +void GoalIntentExtractor::findValidGraphNode(std::vector node_indices, + const geometry_msgs::msg::PoseStamped & pose, + unsigned int & best_node_index) { 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); + if(!costmap_sub_->getCostmap()->worldToMap(pose.pose.position.x, pose.pose.position.y, + s_mx, s_my)) { + throw nav2_core::StartOutsideMapBounds( + "Start Coordinates of(" + std::to_string(pose.pose.position.x) + ", " + + std::to_string(pose.pose.position.y) + ") was outside bounds"); + } + + if (costmap_sub_->getCostmap()->getCost(s_mx, s_my) == nav2_costmap_2d::LETHAL_OBSTACLE) { + throw nav2_core::StartOccupied("Start was in lethal cost"); + } // 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); + std::vector valid_node_indices; std::vector goals; - goals.push_back({g_mx, g_my}); + for (const auto & node_index : node_indices) { + float goal_x = (*graph_)[node_index].coords.x; + float goal_y = (*graph_)[node_index].coords.y; + if (!costmap_sub_->getCostmap()->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_sub_->getCostmap()->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()) { + throw nav2_core::PlannerException("All goals were invalid"); + } bfs_->setStart(s_mx, s_my); bfs_->setGoals(goals); if (bfs_->isNodeVisible()) { - bfs_->clearGraph(); - return true; + // The visiblity check only validates the first node in goal array + best_node_index = valid_node_indices.front(); + return; } unsigned int goal; - if (bfs_->search(goal)) { - bfs_->clearGraph(); - return true; - } - RCLCPP_INFO_STREAM(logger_, "goal index" << goal); - return false; + bfs_->search(goal); + best_node_index = goal; } template Route GoalIntentExtractor::pruneStartandGoal( diff --git a/nav2_route/src/node_spatial_tree.cpp b/nav2_route/src/node_spatial_tree.cpp index 45c4a65bbd2..e4b182f497d 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_) { @@ -36,26 +41,31 @@ NodeSpatialTree::~NodeSpatialTree() void NodeSpatialTree::computeTree(Graph & graph) { - if (kdtree_) { + std::cout << "Starting" << std::endl; + if (kdtree_ != nullptr) { + std::cout << "if statement" << std::endl; delete kdtree_; + std::cout << "delete kdtree_" << std::endl; kdtree_ = nullptr; } + std::cout << "adaptor" << std::endl; if (adaptor_) { delete adaptor_; adaptor_ = nullptr; } + std::cout << " Creating " << std::endl; adaptor_ = new GraphAdaptor(graph); kdtree_ = new kd_tree_t(DIMENSION, *adaptor_, nanoflann::KDTreeSingleIndexAdaptorParams(10)); kdtree_->buildIndex(); 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 = 0; 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 +75,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 86c1a19d923..01a84210c3d 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; @@ -297,6 +298,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/performance_benchmarking.cpp b/nav2_route/test/performance_benchmarking.cpp index a4a9682c0b1..7d6468add16 100644 --- a/nav2_route/test/performance_benchmarking.cpp +++ b/nav2_route/test/performance_benchmarking.cpp @@ -119,16 +119,17 @@ 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_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index 7f697e607fe..a26cda1c167 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -29,7 +29,7 @@ class BFSTestFixture : public ::testing::Test { costmap = std::make_unique(x_size, y_size, 0.0, 0.0, 1); - bfs.setCostmap(costmap.get()); + bfs.initialize(costmap.get(), 200); } BreadthFirstSearch bfs; @@ -46,9 +46,8 @@ TEST_F(BFSTestFixture, free_space) goals.push_back({1u, 1u}); bfs.setGoals(goals); - unsigned int goal; - bool result = bfs.search(goal); - EXPECT_TRUE(result); + unsigned int goal = 0; + ASSERT_NO_THROW(bfs.search(goal)); EXPECT_EQ(costmap->getIndex(goals[goal].x, goals[goal].y), costmap->getIndex(1u, 1u)); bfs.clearGraph(); @@ -70,9 +69,8 @@ TEST_F(BFSTestFixture, wall) goals.push_back({0u, 4u}); bfs.setGoals(goals); - unsigned int goal; - bool result = bfs.search(goal); - EXPECT_TRUE(result); + unsigned int goal = 0; + EXPECT_NO_THROW(bfs.search(goal)); EXPECT_EQ(costmap->getIndex(goals[goal].x, goals[goal].y), costmap->getIndex(0u, 4u)); bfs.clearGraph(); diff --git a/nav2_route/test/test_spatial_tree.cpp b/nav2_route/test/test_spatial_tree.cpp index 8f8aa548e7c..5d6b6248a27 100644 --- a/nav2_route/test/test_spatial_tree.cpp +++ b/nav2_route/test/test_spatial_tree.cpp @@ -26,6 +26,7 @@ using namespace nav2_route; // NOLINT TEST(NodeSpatialTreeTest, test_kd_tree) { + std::cout << "HELLO" << std::endl; // Create a large graph of random nodes unsigned int seed = time(NULL); Graph graph; @@ -37,22 +38,30 @@ TEST(NodeSpatialTreeTest, test_kd_tree) graph[i].coords.y = static_cast((rand_r(&seed) % 6000000) + 1); } + std::cout << "Compute Kd tree" << std::endl; // 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); + std::cout << "Constructor" << std::endl; + kd_tree->computeTree(graph); + std::cout << "Completed tree" << std::endl; // 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)) { + std::cout << "Ran" << std::endl; + if (!kd_tree->findNearestGraphNodesToPose(pose, kd_tree_idx)) { EXPECT_TRUE(false); // Unable to find nearest neighbor! } + std::cout << "Worked" << std::endl; unsigned int closest_via_brute_force; float closest_dist = std::numeric_limits::max(); @@ -66,6 +75,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); } } From dde71abd791bd75a5bc0d3428ce522e616d6fbeb Mon Sep 17 00:00:00 2001 From: gg Date: Sun, 27 Aug 2023 10:14:46 -0400 Subject: [PATCH 23/36] completed integration --- nav2_bringup/launch/navigation_launch.py | 1 + nav2_route/CMakeLists.txt | 1 + .../nav2_route/goal_intent_extractor.hpp | 5 ++++ .../include/nav2_route/node_spatial_tree.hpp | 4 +-- nav2_route/src/goal_intent_extractor.cpp | 25 +++++++++++-------- nav2_route/src/graph_loader.cpp | 6 +++-- nav2_route/src/node_spatial_tree.cpp | 7 +----- nav2_route/src/route_server.cpp | 1 + nav2_route/test/test_breadth_first_search.cpp | 2 ++ nav2_route/test/test_spatial_tree.cpp | 6 ----- 10 files changed, 31 insertions(+), 27 deletions(-) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 3497ae11700..d23e08d1f7e 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -146,6 +146,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + prefix=['xterm -e gdb --args'], arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index ab482abf358..74b06e58103 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) find_package(nanoflann REQUIRED) find_package(nlohmann_json REQUIRED) +add_compile_options(-g) nav2_package() diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index fe9908b9e44..875ce78f86c 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -74,6 +74,11 @@ class GoalIntentExtractor const std::string & route_frame, const std::string & base_frame); + /** + * @brief Activate extractor + */ + void activate(); + /** * @brief Sets a new graph when updated * @param graph Graph to populate kD tree using diff --git a/nav2_route/include/nav2_route/node_spatial_tree.hpp b/nav2_route/include/nav2_route/node_spatial_tree.hpp index 9088365862d..a414d3c89f6 100644 --- a/nav2_route/include/nav2_route/node_spatial_tree.hpp +++ b/nav2_route/include/nav2_route/node_spatial_tree.hpp @@ -97,8 +97,8 @@ class NodeSpatialTree protected: kd_tree_t * kdtree_{nullptr}; - GraphAdaptor * adaptor_; - Graph * graph_; + GraphAdaptor * adaptor_{nullptr}; + Graph * graph_{nullptr}; int num_of_nearest_nodes_; }; diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index 0c2c28e5dba..5c0c72a1d08 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -12,9 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include #include #include #include @@ -69,7 +66,7 @@ void GoalIntentExtractor::configure( node_spatial_tree_->computeTree(graph); nav2_util::declare_parameter_if_not_declared( - node, "enable_search", rclcpp::ParameterValue(false)); + node, "enable_search", rclcpp::ParameterValue(true)); enable_search_ = node->get_parameter("enable_search").as_bool(); if (enable_search_) { @@ -79,15 +76,21 @@ void GoalIntentExtractor::configure( rclcpp::ParameterValue(std::string("global_costmap/costmap_raw"))); node->get_parameter("global_costmap_topic", global_costmap_topic); - int max_iterations; - nav2_util::declare_parameter_if_not_declared( - node, "max_iterations", rclcpp::ParameterValue(500)); - max_iterations = node->get_parameter("max_iterations").as_int(); + // int max_iterations; + // nav2_util::declare_parameter_if_not_declared( + // node, "max_iterations", rclcpp::ParameterValue(500)); + // max_iterations = node->get_parameter("max_iterations").as_int(); costmap_sub_ = std::make_unique(node, global_costmap_topic); bfs_ = std::make_unique(); - bfs_->initialize(costmap_sub_->getCostmap().get(), max_iterations); + } +} + +void GoalIntentExtractor::activate() +{ + if(enable_search_) { + bfs_->initialize(costmap_sub_->getCostmap().get(), 500); } } @@ -162,10 +165,10 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) unsigned int valid_start_route_id, valid_end_route_id; findValidGraphNode(start_route_ids, start_, valid_start_route_id); findValidGraphNode(end_route_ids, goal_, valid_end_route_id); - return {valid_end_route_id, valid_end_route_id}; + return {valid_start_route_id, valid_end_route_id}; } - return {start_route_ids.front(), start_route_ids.front()}; + return {start_route_ids.front(), end_route_ids.front()}; } template diff --git a/nav2_route/src/graph_loader.cpp b/nav2_route/src/graph_loader.cpp index 25b268bc706..fb59d859afa 100644 --- a/nav2_route/src/graph_loader.cpp +++ b/nav2_route/src/graph_loader.cpp @@ -32,7 +32,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 @@ -57,7 +59,7 @@ GraphLoader::GraphLoader( } catch (pluginlib::PluginlibException & ex) { RCLCPP_FATAL( logger_, - "Failed to create GraphFileLoader. Exception: %s", ex.what()); + "Failed to create GraphFileLoader. Exception: %s", ex.what()); throw ex; } } diff --git a/nav2_route/src/node_spatial_tree.cpp b/nav2_route/src/node_spatial_tree.cpp index e4b182f497d..fee7e6420ed 100644 --- a/nav2_route/src/node_spatial_tree.cpp +++ b/nav2_route/src/node_spatial_tree.cpp @@ -41,21 +41,16 @@ NodeSpatialTree::~NodeSpatialTree() void NodeSpatialTree::computeTree(Graph & graph) { - std::cout << "Starting" << std::endl; if (kdtree_ != nullptr) { - std::cout << "if statement" << std::endl; delete kdtree_; - std::cout << "delete kdtree_" << std::endl; kdtree_ = nullptr; } - std::cout << "adaptor" << std::endl; if (adaptor_) { delete adaptor_; adaptor_ = nullptr; } - std::cout << " Creating " << std::endl; adaptor_ = new GraphAdaptor(graph); kdtree_ = new kd_tree_t(DIMENSION, *adaptor_, nanoflann::KDTreeSingleIndexAdaptorParams(10)); kdtree_->buildIndex(); @@ -65,7 +60,7 @@ void NodeSpatialTree::computeTree(Graph & graph) bool NodeSpatialTree::findNearestGraphNodesToPose( const geometry_msgs::msg::PoseStamped & pose_in, std::vector & node_ids) { - size_t num_results = 0; + 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}; diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 01a84210c3d..e44805820e7 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -101,6 +101,7 @@ nav2_util::CallbackReturn RouteServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); + goal_intent_extractor_->activate(); compute_route_server_->activate(); compute_and_track_route_server_->activate(); graph_vis_publisher_->on_activate(); diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index a26cda1c167..0301faecbf2 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -89,6 +89,8 @@ TEST_F(BFSTestFixture, ray_trace) bool result = bfs.isNodeVisible(); EXPECT_TRUE(result); + bfs.setStart(0u, 0u); + bfs.setGoals(goals); costmap->setCost(2u, 2u, LETHAL_OBSTACLE); result = bfs.isNodeVisible(); EXPECT_FALSE(result); diff --git a/nav2_route/test/test_spatial_tree.cpp b/nav2_route/test/test_spatial_tree.cpp index 5d6b6248a27..957b63f4f78 100644 --- a/nav2_route/test/test_spatial_tree.cpp +++ b/nav2_route/test/test_spatial_tree.cpp @@ -26,7 +26,6 @@ using namespace nav2_route; // NOLINT TEST(NodeSpatialTreeTest, test_kd_tree) { - std::cout << "HELLO" << std::endl; // Create a large graph of random nodes unsigned int seed = time(NULL); Graph graph; @@ -38,15 +37,12 @@ TEST(NodeSpatialTreeTest, test_kd_tree) graph[i].coords.y = static_cast((rand_r(&seed) % 6000000) + 1); } - std::cout << "Compute Kd tree" << std::endl; // Compute our kd tree for this graph int num_of_nearest_nodes = 1; std::shared_ptr kd_tree = std::make_shared(num_of_nearest_nodes); - std::cout << "Constructor" << std::endl; kd_tree->computeTree(graph); - std::cout << "Completed tree" << std::endl; // 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 @@ -57,11 +53,9 @@ TEST(NodeSpatialTreeTest, test_kd_tree) pose.pose.position.x = static_cast((rand_r(&seed) % 6000000) + 1); pose.pose.position.y = static_cast((rand_r(&seed) % 6000000) + 1); - std::cout << "Ran" << std::endl; if (!kd_tree->findNearestGraphNodesToPose(pose, kd_tree_idx)) { EXPECT_TRUE(false); // Unable to find nearest neighbor! } - std::cout << "Worked" << std::endl; unsigned int closest_via_brute_force; float closest_dist = std::numeric_limits::max(); From d86c56520814f2af3d5e37664f0ceafd4688964e Mon Sep 17 00:00:00 2001 From: gg Date: Mon, 28 Aug 2023 20:51:31 -0400 Subject: [PATCH 24/36] completed integration of bfs search into goal intent extractor --- nav2_route/CMakeLists.txt | 1 + .../nav2_route/breadth_first_search.hpp | 34 +++--- .../nav2_route/goal_intent_extractor.hpp | 30 +++-- nav2_route/src/breadth_first_search.cpp | 36 +++--- nav2_route/src/goal_intent_extractor.cpp | 103 +++++++++++++----- nav2_route/src/graph_loader.cpp | 6 +- nav2_route/src/node_spatial_tree.cpp | 2 +- nav2_route/src/route_server.cpp | 2 + nav2_route/test/performance_benchmarking.cpp | 9 +- nav2_route/test/test_breadth_first_search.cpp | 5 +- nav2_route/test/test_spatial_tree.cpp | 2 +- 11 files changed, 150 insertions(+), 80 deletions(-) diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index 74b06e58103..25e86b9c686 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -57,6 +57,7 @@ add_library(${library_name} SHARED src/goal_intent_extractor.cpp src/breadth_first_search.cpp ) +target_compile_options(${library_name} PRIVATE -g) ament_target_dependencies(${library_name} ${dependencies} diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index 52efee96c2b..021b6cfdb9d 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -29,23 +29,23 @@ namespace nav2_route { /** - * @struct nav2_route::SimpleNode + * @struct nav2_route::SimpleNode * @brief A Node implementation for the breadth first search */ struct SimpleNode { SimpleNode(unsigned int index) - : index(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: @@ -53,7 +53,7 @@ class BreadthFirstSearch typedef std::vector NodeVector; /** - * @brief Initialize the search algorithm + * @brief Initialize the search algorithm * @param costmap Costmap to use to check for state validity */ void initialize(nav2_costmap_2d::Costmap2D * costmap, int max_iterations); @@ -61,14 +61,14 @@ class BreadthFirstSearch /** * @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 - */ + * @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); /** @@ -76,24 +76,30 @@ class BreadthFirstSearch * @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 node is directly visable * @param True if the node is visable */ - bool isNodeVisible(); + bool isNodeVisible(); + /** - * @brief clear the graph + * @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 + * @param index The index of the node * @return node A pointer to the node added into the graph */ NodePtr addToGraph(const unsigned int index); @@ -101,13 +107,13 @@ class BreadthFirstSearch /** * @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 graph_; diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index 875ce78f86c..bbce76692c8 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -29,6 +30,7 @@ #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" @@ -91,6 +93,7 @@ class GoalIntentExtractor * @param pose Pose to transform (e.g. start, goal) */ geometry_msgs::msg::PoseStamped transformPose(geometry_msgs::msg::PoseStamped & pose); + /** * @brief Main API to find the start and goal graph IDX (not IDs) for routing * @param goal Action request goal @@ -120,19 +123,26 @@ 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 + * @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 * @throws nav2_core::StartOutsideMapBounds If the start index is not in the costmap * @throws nav2_core::StartOccupied If the start is in lethal cost */ - void findValidGraphNode(std::vector node_indices, - const geometry_msgs::msg::PoseStamped & pose, - unsigned int & best_node_index); + void findValidGraphNode( + std::vector node_indices, + const geometry_msgs::msg::PoseStamped & pose, + unsigned int & best_node_index); protected: + /** + * @brief Visualize the bfs 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")}; std::shared_ptr node_spatial_tree_; GraphToIDMap * id_to_graph_map_; @@ -144,9 +154,13 @@ class GoalIntentExtractor 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 costmap_sub_; std::unique_ptr bfs_; - bool enable_search_; + rclcpp::Publisher::SharedPtr start_expansion_viz_; + rclcpp::Publisher::SharedPtr goal_expansion_viz_; }; } // namespace nav2_route diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index bb4a64b64f4..e4e26db13cd 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -29,7 +29,6 @@ namespace nav2_route void BreadthFirstSearch::initialize(nav2_costmap_2d::Costmap2D * costmap, int max_iterations) { - std::cout << "Set costmap " << std::endl; costmap_ = costmap; int x_size = static_cast(costmap_->getSizeInCellsX()); @@ -76,23 +75,20 @@ void BreadthFirstSearch::search(unsigned int & goal) start_->explored = true; queue.push(start_); - int iteration = 0; + int iteration = 0; while (!queue.empty()) { auto & current = queue.front(); queue.pop(); if (iteration > max_iterations_) { - graph_.clear(); throw nav2_core::PlannerTimedOut("Exceeded maximum iterations"); } - std::cout << "Current index: " << current->index << std::endl; // Check goals for (unsigned int index = 0; index < goals_.size(); ++index) { if (current->index == goals_[index]->index) { goal = index; - graph_.clear(); return; } } @@ -108,34 +104,31 @@ void BreadthFirstSearch::search(unsigned int & goal) } } - graph_.clear(); throw nav2_core::NoValidPathCouldBeFound("No valid path found"); } void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & neighbors) { - unsigned int p_mx, p_my; + 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; - std::cout << "Neighbor index: " << index << std::endl; - if(index < 0){ - std::cout << "Index is negative" << std::endl; + if (index < 0) { continue; } neighbor_index = static_cast(index); - unsigned int n_mx, n_my; + 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) { - std::cout << "Wraps " << std::endl; + if (std::fabs(static_cast(p_mx) - static_cast(n_mx)) > 1 || + std::fabs(static_cast(p_my) - static_cast(n_my)) > 1) + { continue; } @@ -154,16 +147,14 @@ void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & ne bool BreadthFirstSearch::isNodeVisible() { - unsigned int s_mx, s_my, g_mx, g_my; + 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.getX()); - std::cout << "cost: " << cost << std::endl; - + double cost = costmap_->getCost(line.getX(), line.getX()); if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || - cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) + cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { return false; } @@ -186,7 +177,12 @@ bool BreadthFirstSearch::inCollision(unsigned int index) void BreadthFirstSearch::clearGraph() { - graph_.clear(); + graph_.clear(); +} + +std::unordered_map * BreadthFirstSearch::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 5c0c72a1d08..059d8488dd8 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -23,6 +23,7 @@ #include "nav2_route/breadth_first_search.hpp" #include "nav2_route/goal_intent_extractor.hpp" #include "nav2_util/node_utils.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" namespace nav2_route { @@ -59,38 +60,46 @@ void GoalIntentExtractor::configure( 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)); + 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(true)); - enable_search_ = node->get_parameter("enable_search").as_bool(); + node, "enable_search", rclcpp::ParameterValue(false)); + enable_search_ = node->get_parameter("enable_search").as_bool(); if (enable_search_) { 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, "global_costmap_topic", + rclcpp::ParameterValue(std::string("global_costmap/costmap_raw"))); node->get_parameter("global_costmap_topic", global_costmap_topic); - // int max_iterations; - // nav2_util::declare_parameter_if_not_declared( - // node, "max_iterations", rclcpp::ParameterValue(500)); - // max_iterations = node->get_parameter("max_iterations").as_int(); + nav2_util::declare_parameter_if_not_declared( + node, "max_iterations", rclcpp::ParameterValue(500)); + max_iterations_ = node->get_parameter("max_iterations").as_int(); + + nav2_util::declare_parameter_if_not_declared( + node, "enable_search_viz", rclcpp::ParameterValue(false)); + enable_search_viz_ = node->get_parameter("enable_search_viz").as_bool(); - costmap_sub_ = + costmap_sub_ = std::make_unique(node, global_costmap_topic); bfs_ = std::make_unique(); + start_expansion_viz_ = node->create_publisher( + "start_expansions", + 1); + goal_expansion_viz_ = + node->create_publisher("goal_expansions", 1); } } void GoalIntentExtractor::activate() { - if(enable_search_) { - bfs_->initialize(costmap_sub_->getCostmap().get(), 500); + if (enable_search_) { + bfs_->initialize(costmap_sub_->getCostmap().get(), max_iterations_); } } @@ -163,8 +172,15 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) if (enable_search_) { unsigned int valid_start_route_id, valid_end_route_id; + findValidGraphNode(start_route_ids, start_, valid_start_route_id); + visualizeExpansions(start_expansion_viz_); + bfs_->clearGraph(); + findValidGraphNode(end_route_ids, goal_, valid_end_route_id); + visualizeExpansions(goal_expansion_viz_); + bfs_->clearGraph(); + return {valid_start_route_id, valid_end_route_id}; } @@ -239,37 +255,42 @@ Route GoalIntentExtractor::pruneStartandGoal( return pruned_route; } -void GoalIntentExtractor::findValidGraphNode(std::vector node_indices, - const geometry_msgs::msg::PoseStamped & pose, - unsigned int & best_node_index) +void GoalIntentExtractor::findValidGraphNode( + std::vector node_indices, + const geometry_msgs::msg::PoseStamped & pose, + unsigned int & best_node_index) { - unsigned int s_mx, s_my, g_mx, g_my; - if(!costmap_sub_->getCostmap()->worldToMap(pose.pose.position.x, pose.pose.position.y, - s_mx, s_my)) { + unsigned int s_mx, s_my, g_mx, g_my; + if (!costmap_sub_->getCostmap()->worldToMap( + pose.pose.position.x, pose.pose.position.y, + s_mx, s_my)) + { throw nav2_core::StartOutsideMapBounds( - "Start Coordinates of(" + std::to_string(pose.pose.position.x) + ", " + - std::to_string(pose.pose.position.y) + ") was outside bounds"); + "Start Coordinates of(" + std::to_string(pose.pose.position.x) + ", " + + std::to_string(pose.pose.position.y) + ") was outside bounds"); } if (costmap_sub_->getCostmap()->getCost(s_mx, s_my) == nav2_costmap_2d::LETHAL_OBSTACLE) { throw nav2_core::StartOccupied("Start was in lethal cost"); } - // double check the frames, probably need to move into the map frame... + //TODO(jwallace42): double check the frames, probably need to move into the map frame... std::vector valid_node_indices; - std::vector goals; + std::vector goals; for (const auto & node_index : node_indices) { float goal_x = (*graph_)[node_index].coords.x; float goal_y = (*graph_)[node_index].coords.y; if (!costmap_sub_->getCostmap()->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"); + 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_sub_->getCostmap()->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; + 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}); @@ -279,6 +300,7 @@ void GoalIntentExtractor::findValidGraphNode(std::vector node_indi throw nav2_core::PlannerException("All goals were invalid"); } + bfs_->clearGraph(); bfs_->setStart(s_mx, s_my); bfs_->setGoals(goals); if (bfs_->isNodeVisible()) { @@ -289,7 +311,34 @@ void GoalIntentExtractor::findValidGraphNode(std::vector node_indi unsigned int goal; bfs_->search(goal); - best_node_index = goal; + + best_node_index = 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_sub_.get()->getCostmap()->getSizeInCellsX(); + unsigned int height = costmap_sub_.get()->getCostmap()->getSizeInCellsY(); + occ_grid_msg.header.frame_id = "map"; + occ_grid_msg.info.resolution = costmap_sub_.get()->getCostmap()->getResolution(); + occ_grid_msg.info.width = width; + occ_grid_msg.info.height = height; + occ_grid_msg.info.origin.position.x = costmap_sub_.get()->getCostmap()->getOriginX(); + occ_grid_msg.info.origin.position.y = costmap_sub_.get()->getCostmap()->getOriginY(); + occ_grid_msg.info.origin.orientation.w = 1.0; + occ_grid_msg.data.assign(width * height, 0); + + auto graph = bfs_->getGraph(); + + for (const auto & node : *graph) { + occ_grid_msg.data[node.first] = 100; + } + occ_grid_pub->publish(occ_grid_msg); } template Route GoalIntentExtractor::pruneStartandGoal( diff --git a/nav2_route/src/graph_loader.cpp b/nav2_route/src/graph_loader.cpp index fb59d859afa..8a60c2628c5 100644 --- a/nav2_route/src/graph_loader.cpp +++ b/nav2_route/src/graph_loader.cpp @@ -33,8 +33,8 @@ GraphLoader::GraphLoader( nav2_util::declare_parameter_if_not_declared( node, "graph_filepath", rclcpp::ParameterValue( - ament_index_cpp::get_package_share_directory("nav2_route") + - "/graphs/aws_graph.geojson")); + 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 @@ -59,7 +59,7 @@ GraphLoader::GraphLoader( } catch (pluginlib::PluginlibException & ex) { RCLCPP_FATAL( logger_, - "Failed to create GraphFileLoader. Exception: %s", ex.what()); + "Failed to create GraphFileLoader. Exception: %s", ex.what()); throw ex; } } diff --git a/nav2_route/src/node_spatial_tree.cpp b/nav2_route/src/node_spatial_tree.cpp index fee7e6420ed..02b47504f14 100644 --- a/nav2_route/src/node_spatial_tree.cpp +++ b/nav2_route/src/node_spatial_tree.cpp @@ -71,7 +71,7 @@ bool NodeSpatialTree::findNearestGraphNodesToPose( } for (int i = 0; i < num_of_nearest_nodes_ && i < static_cast(ret_index.size()); ++i) { - node_ids.push_back(ret_index[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 e44805820e7..f4efb38dded 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -204,6 +204,7 @@ Route RouteServer::findRoute( { // Find the search boundaries auto [start_route, end_route] = goal_intent_extractor_->findStartandGoal(goal); + std::cout << "found start and goal: " << start_route << " " << end_route << std::endl; if (rerouting_info.rerouting_start_id != std::numeric_limits::max()) { start_route = id_to_graph_map_.at(rerouting_info.rerouting_start_id); @@ -220,6 +221,7 @@ Route RouteServer::findRoute( route = route_planner_->findRoute(graph_, start_route, end_route, rerouting_info.blocked_ids); } + std::cout << "exiting" << std::endl; return goal_intent_extractor_->pruneStartandGoal(route, goal, rerouting_info); } diff --git a/nav2_route/test/performance_benchmarking.cpp b/nav2_route/test/performance_benchmarking.cpp index 7d6468add16..c527e4e386c 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,8 +119,9 @@ 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 - int num_of_nearest_nodes = 1; - std::shared_ptr kd_tree = std::make_shared(num_of_nearest_nodes); + 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(); diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index 0301faecbf2..4a7e248346c 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -42,7 +42,7 @@ TEST_F(BFSTestFixture, free_space) bfs.setStart(0u, 0u); - std::vector goals; + std::vector goals; goals.push_back({1u, 1u}); bfs.setGoals(goals); @@ -58,7 +58,7 @@ TEST_F(BFSTestFixture, wall) initialize(10u, 10u); unsigned int mx_obs = 3; - for(unsigned int my_obs=0; my_obs < costmap->getSizeInCellsY(); ++my_obs) { + for (unsigned int my_obs = 0; my_obs < costmap->getSizeInCellsY(); ++my_obs) { costmap->setCost(mx_obs, my_obs, LETHAL_OBSTACLE); } @@ -88,6 +88,7 @@ TEST_F(BFSTestFixture, ray_trace) bool result = bfs.isNodeVisible(); EXPECT_TRUE(result); + bfs.clearGraph(); bfs.setStart(0u, 0u); bfs.setGoals(goals); diff --git a/nav2_route/test/test_spatial_tree.cpp b/nav2_route/test/test_spatial_tree.cpp index 957b63f4f78..d9363026acd 100644 --- a/nav2_route/test/test_spatial_tree.cpp +++ b/nav2_route/test/test_spatial_tree.cpp @@ -39,7 +39,7 @@ TEST(NodeSpatialTreeTest, test_kd_tree) // Compute our kd tree for this graph int num_of_nearest_nodes = 1; - std::shared_ptr kd_tree = + std::shared_ptr kd_tree = std::make_shared(num_of_nearest_nodes); kd_tree->computeTree(graph); From d09d644b260156fbb10bfd8efba835de47a506ea Mon Sep 17 00:00:00 2001 From: gg Date: Wed, 30 Aug 2023 23:13:28 -0400 Subject: [PATCH 25/36] cleanup --- nav2_bringup/launch/navigation_launch.py | 1 - nav2_route/CMakeLists.txt | 2 -- .../include/nav2_route/goal_intent_extractor.hpp | 11 +++++------ nav2_route/src/goal_intent_extractor.cpp | 16 ++++++---------- 4 files changed, 11 insertions(+), 19 deletions(-) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index d23e08d1f7e..3497ae11700 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -146,7 +146,6 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], - prefix=['xterm -e gdb --args'], arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index 25e86b9c686..ab482abf358 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) find_package(nanoflann REQUIRED) find_package(nlohmann_json REQUIRED) -add_compile_options(-g) nav2_package() @@ -57,7 +56,6 @@ add_library(${library_name} SHARED src/goal_intent_extractor.cpp src/breadth_first_search.cpp ) -target_compile_options(${library_name} PRIVATE -g) ament_target_dependencies(${library_name} ${dependencies} diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index bbce76692c8..f2556f209c8 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -16,7 +16,6 @@ #define NAV2_ROUTE__GOAL_INTENT_EXTRACTOR_HPP_ #include -#include #include #include #include @@ -123,19 +122,19 @@ class GoalIntentExtractor */ void setStart(const geometry_msgs::msg::PoseStamped & start_pose); +protected: /** * @brief Checks if there is connection between a node and a given pose - * @param node_index The index of the node + * @param node_indices A list of graph node indices * @param pose The pose + * @return The best node * @throws nav2_core::StartOutsideMapBounds If the start index is not in the costmap * @throws nav2_core::StartOccupied If the start is in lethal cost */ - void findValidGraphNode( + unsigned int associatePoseWithGraphNode( std::vector node_indices, - const geometry_msgs::msg::PoseStamped & pose, - unsigned int & best_node_index); + const geometry_msgs::msg::PoseStamped & pose); -protected: /** * @brief Visualize the bfs search expansions * @param occ_grid_pub a occupancy grid publisher to view the expansions diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index 059d8488dd8..7a18a3ec5e6 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -171,13 +171,11 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) } if (enable_search_) { - unsigned int valid_start_route_id, valid_end_route_id; - - findValidGraphNode(start_route_ids, start_, valid_start_route_id); + unsigned int valid_start_route_id = associatePoseWithGraphNode(start_route_ids, start_); visualizeExpansions(start_expansion_viz_); bfs_->clearGraph(); - findValidGraphNode(end_route_ids, goal_, valid_end_route_id); + unsigned int valid_end_route_id = associatePoseWithGraphNode(end_route_ids, goal_); visualizeExpansions(goal_expansion_viz_); bfs_->clearGraph(); @@ -255,10 +253,9 @@ Route GoalIntentExtractor::pruneStartandGoal( return pruned_route; } -void GoalIntentExtractor::findValidGraphNode( +unsigned int GoalIntentExtractor::associatePoseWithGraphNode( std::vector node_indices, - const geometry_msgs::msg::PoseStamped & pose, - unsigned int & best_node_index) + const geometry_msgs::msg::PoseStamped & pose) { unsigned int s_mx, s_my, g_mx, g_my; if (!costmap_sub_->getCostmap()->worldToMap( @@ -305,14 +302,13 @@ void GoalIntentExtractor::findValidGraphNode( bfs_->setGoals(goals); if (bfs_->isNodeVisible()) { // The visiblity check only validates the first node in goal array - best_node_index = valid_node_indices.front(); - return; + return valid_node_indices.front(); } unsigned int goal; bfs_->search(goal); - best_node_index = valid_node_indices[goal]; + return valid_node_indices[goal]; } void GoalIntentExtractor::visualizeExpansions( From 5754baf26248d94d4b909100b5d46e450ffda620 Mon Sep 17 00:00:00 2001 From: gg Date: Wed, 30 Aug 2023 23:28:58 -0400 Subject: [PATCH 26/36] clean up --- .../include/nav2_route/goal_intent_extractor.hpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index f2556f209c8..eae50ec0a3d 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -15,14 +15,11 @@ #ifndef NAV2_ROUTE__GOAL_INTENT_EXTRACTOR_HPP_ #define NAV2_ROUTE__GOAL_INTENT_EXTRACTOR_HPP_ -#include -#include #include #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" @@ -124,10 +121,11 @@ class GoalIntentExtractor protected: /** - * @brief Checks if there is connection between a node and a given pose - * @param node_indices A list of graph node indices - * @param pose The pose - * @return The best node + * @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 */ @@ -136,8 +134,8 @@ class GoalIntentExtractor const geometry_msgs::msg::PoseStamped & pose); /** - * @brief Visualize the bfs search expansions - * @param occ_grid_pub a occupancy grid publisher to view the expansions + * @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); From f8dc30f339319bf958a1397668e1b87bf0bf8983 Mon Sep 17 00:00:00 2001 From: gg Date: Wed, 30 Aug 2023 23:40:50 -0400 Subject: [PATCH 27/36] cleanup --- nav2_route/include/nav2_route/node_spatial_tree.hpp | 5 +++++ nav2_route/package.xml | 1 - nav2_route/src/breadth_first_search.cpp | 6 +++--- nav2_route/src/route_server.cpp | 1 - 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/nav2_route/include/nav2_route/node_spatial_tree.hpp b/nav2_route/include/nav2_route/node_spatial_tree.hpp index a414d3c89f6..5d33fb6ed1e 100644 --- a/nav2_route/include/nav2_route/node_spatial_tree.hpp +++ b/nav2_route/include/nav2_route/node_spatial_tree.hpp @@ -69,9 +69,14 @@ 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(int num_of_nearest_nodes); + /** + * @brief Delete the default constructor + */ NodeSpatialTree() = delete; /** diff --git a/nav2_route/package.xml b/nav2_route/package.xml index fe117050f93..ef023dc604d 100644 --- a/nav2_route/package.xml +++ b/nav2_route/package.xml @@ -22,7 +22,6 @@ nav2_core lib-nanoflann nlohmann-json - nav2_smac_planner ament_lint_common ament_lint_auto diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index e4e26db13cd..f35e3830e32 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -86,9 +86,9 @@ void BreadthFirstSearch::search(unsigned int & goal) // Check goals - for (unsigned int index = 0; index < goals_.size(); ++index) { - if (current->index == goals_[index]->index) { - goal = index; + for (unsigned int i = 0; i < goals_.size(); ++i) { + if (current->index == goals_[i]->index) { + goal = i; return; } } diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index f4efb38dded..53ccdfa1cbb 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -204,7 +204,6 @@ Route RouteServer::findRoute( { // Find the search boundaries auto [start_route, end_route] = goal_intent_extractor_->findStartandGoal(goal); - std::cout << "found start and goal: " << start_route << " " << end_route << std::endl; if (rerouting_info.rerouting_start_id != std::numeric_limits::max()) { start_route = id_to_graph_map_.at(rerouting_info.rerouting_start_id); From d240ec0ceba3b2c0e785da0cf431e6db7d4cf9f5 Mon Sep 17 00:00:00 2001 From: gg Date: Wed, 30 Aug 2023 23:52:09 -0400 Subject: [PATCH 28/36] remove print --- nav2_route/src/route_server.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 53ccdfa1cbb..e44805820e7 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -220,7 +220,6 @@ Route RouteServer::findRoute( route = route_planner_->findRoute(graph_, start_route, end_route, rerouting_info.blocked_ids); } - std::cout << "exiting" << std::endl; return goal_intent_extractor_->pruneStartandGoal(route, goal, rerouting_info); } From fa97e4a96a934bc5c84e8cd50a0cd39c60f832e0 Mon Sep 17 00:00:00 2001 From: gg Date: Fri, 8 Sep 2023 15:08:13 -0400 Subject: [PATCH 29/36] small fixes --- nav2_bringup/launch/navigation_launch.py | 2 + nav2_bt_navigator/CMakeLists.txt | 1 + .../nav2_route/breadth_first_search.hpp | 4 +- nav2_route/src/breadth_first_search.cpp | 5 +-- nav2_route/src/goal_intent_extractor.cpp | 39 ++++++++++--------- nav2_route/src/node_spatial_tree.cpp | 2 +- nav2_route/test/test_breadth_first_search.cpp | 4 +- 7 files changed, 31 insertions(+), 26 deletions(-) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 3497ae11700..cfc9e141bcc 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -147,6 +147,7 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], + prefix=['xterm -e gdb --args'], remappings=remappings), Node( package='nav2_behaviors', @@ -167,6 +168,7 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], + # prefix=['xterm -e gdb -ex run --args'], remappings=remappings), Node( package='nav2_waypoint_follower', diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index e36fb0de03b..423e0e8533f 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(nav2_core REQUIRED) find_package(tf2_ros REQUIRED) nav2_package() +add_compile_options(-g) include_directories( include diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index 021b6cfdb9d..8c26c02a21d 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -80,10 +80,10 @@ class BreadthFirstSearch void search(unsigned int & goal); /** - * @brief Preform a ray trace check to see if the node is directly visable + * @brief Preform a ray trace check to see if the first node is visable * @param True if the node is visable */ - bool isNodeVisible(); + bool isFirstGoalVisible(); /** diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index f35e3830e32..a7949e5a12c 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -84,7 +84,6 @@ void BreadthFirstSearch::search(unsigned int & goal) throw nav2_core::PlannerTimedOut("Exceeded maximum iterations"); } - // Check goals for (unsigned int i = 0; i < goals_.size(); ++i) { if (current->index == goals_[i]->index) { @@ -145,14 +144,14 @@ void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & ne } } -bool BreadthFirstSearch::isNodeVisible() +bool BreadthFirstSearch::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.getX()); + double cost = costmap_->getCost(line.getX(), line.getY()); if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index 7a18a3ec5e6..f6b52ba585b 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -67,26 +67,26 @@ void GoalIntentExtractor::configure( node_spatial_tree_->computeTree(graph); nav2_util::declare_parameter_if_not_declared( - node, "enable_search", rclcpp::ParameterValue(false)); + node, "enable_search", rclcpp::ParameterValue(true)); enable_search_ = node->get_parameter("enable_search").as_bool(); if (enable_search_) { - std::string global_costmap_topic; + std::string costmap_topic; nav2_util::declare_parameter_if_not_declared( - node, "global_costmap_topic", + node, "costmap_topic", rclcpp::ParameterValue(std::string("global_costmap/costmap_raw"))); - node->get_parameter("global_costmap_topic", global_costmap_topic); + node->get_parameter("costmap_topic", costmap_topic); nav2_util::declare_parameter_if_not_declared( - node, "max_iterations", rclcpp::ParameterValue(500)); + 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(false)); + node, "enable_search_viz", rclcpp::ParameterValue(true)); enable_search_viz_ = node->get_parameter("enable_search_viz").as_bool(); costmap_sub_ = - std::make_unique(node, global_costmap_topic); + std::make_unique(node, costmap_topic); bfs_ = std::make_unique(); start_expansion_viz_ = node->create_publisher( "start_expansions", @@ -256,7 +256,7 @@ Route GoalIntentExtractor::pruneStartandGoal( unsigned int GoalIntentExtractor::associatePoseWithGraphNode( std::vector node_indices, const geometry_msgs::msg::PoseStamped & pose) -{ +{ unsigned int s_mx, s_my, g_mx, g_my; if (!costmap_sub_->getCostmap()->worldToMap( pose.pose.position.x, pose.pose.position.y, @@ -287,7 +287,8 @@ unsigned int GoalIntentExtractor::associatePoseWithGraphNode( if (costmap_sub_->getCostmap()->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; + 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}); @@ -300,10 +301,10 @@ unsigned int GoalIntentExtractor::associatePoseWithGraphNode( bfs_->clearGraph(); bfs_->setStart(s_mx, s_my); bfs_->setGoals(goals); - if (bfs_->isNodeVisible()) { - // The visiblity check only validates the first node in goal array - return valid_node_indices.front(); - } + // if (bfs_->isFirstGoalVisible()) { + // // The visiblity check only validates the first node in goal array + // return valid_node_indices.front(); + // } unsigned int goal; bfs_->search(goal); @@ -317,15 +318,17 @@ void GoalIntentExtractor::visualizeExpansions( if (!enable_search_viz_) { return; } + + auto costmap = costmap_sub_->getCostmap(); nav_msgs::msg::OccupancyGrid occ_grid_msg; - unsigned int width = costmap_sub_.get()->getCostmap()->getSizeInCellsX(); - unsigned int height = costmap_sub_.get()->getCostmap()->getSizeInCellsY(); + unsigned int width = costmap->getSizeInCellsX(); + unsigned int height = costmap->getSizeInCellsY(); occ_grid_msg.header.frame_id = "map"; - occ_grid_msg.info.resolution = costmap_sub_.get()->getCostmap()->getResolution(); + 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_sub_.get()->getCostmap()->getOriginX(); - occ_grid_msg.info.origin.position.y = costmap_sub_.get()->getCostmap()->getOriginY(); + 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); diff --git a/nav2_route/src/node_spatial_tree.cpp b/nav2_route/src/node_spatial_tree.cpp index 02b47504f14..c1f81d6102b 100644 --- a/nav2_route/src/node_spatial_tree.cpp +++ b/nav2_route/src/node_spatial_tree.cpp @@ -41,7 +41,7 @@ NodeSpatialTree::~NodeSpatialTree() void NodeSpatialTree::computeTree(Graph & graph) { - if (kdtree_ != nullptr) { + if (kdtree_) { delete kdtree_; kdtree_ = nullptr; } diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index 4a7e248346c..fc88e7c8532 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -86,14 +86,14 @@ TEST_F(BFSTestFixture, ray_trace) goals.push_back({5u, 5u}); bfs.setGoals(goals); - bool result = bfs.isNodeVisible(); + bool result = bfs.isFirstGoalVisible(); EXPECT_TRUE(result); bfs.clearGraph(); bfs.setStart(0u, 0u); bfs.setGoals(goals); costmap->setCost(2u, 2u, LETHAL_OBSTACLE); - result = bfs.isNodeVisible(); + result = bfs.isFirstGoalVisible(); EXPECT_FALSE(result); bfs.clearGraph(); From 4ca27fded9cc36ae2cfc08f01eaf019359242852 Mon Sep 17 00:00:00 2001 From: gg Date: Fri, 8 Sep 2023 16:48:20 -0400 Subject: [PATCH 30/36] transform poses into costmap frame --- .../nav2_costmap_2d/costmap_subscriber.hpp | 5 +++ nav2_costmap_2d/src/costmap_subscriber.cpp | 10 ++++- .../nav2_route/breadth_first_search.hpp | 4 +- .../nav2_route/goal_intent_extractor.hpp | 15 ++++--- nav2_route/src/breadth_first_search.cpp | 2 +- nav2_route/src/goal_intent_extractor.cpp | 42 ++++++++++++------- nav2_route/src/route_server.cpp | 2 +- nav2_route/test/test_breadth_first_search.cpp | 4 +- .../test/test_goal_intent_extractor.cpp | 6 +-- 9 files changed, 59 insertions(+), 31 deletions(-) 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 e4ea745015d..112d96679bb 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 d56ac942e98..5ea9f1d8bf3 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_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index 8c26c02a21d..8d13e8a03d8 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -56,7 +56,7 @@ class BreadthFirstSearch * @brief Initialize the search algorithm * @param costmap Costmap to use to check for state validity */ - void initialize(nav2_costmap_2d::Costmap2D * costmap, int max_iterations); + void initialize(std::shared_ptr costmap, int max_iterations); /** * @brief Set the start of the breadth first search @@ -125,7 +125,7 @@ class BreadthFirstSearch unsigned int y_size_; unsigned int max_index_; std::vector neighbors_grid_offsets_; - nav2_costmap_2d::Costmap2D * costmap_; + std::shared_ptr costmap_; int max_iterations_; }; } // namespace nav2_route diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index eae50ec0a3d..ad1a8a2e34e 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -73,9 +73,9 @@ class GoalIntentExtractor const std::string & base_frame); /** - * @brief Activate extractor + * @brief Initialize extractor */ - void activate(); + void initialize(); /** * @brief Sets a new graph when updated @@ -85,10 +85,13 @@ 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 @@ -131,7 +134,7 @@ class GoalIntentExtractor */ unsigned int associatePoseWithGraphNode( std::vector node_indices, - const geometry_msgs::msg::PoseStamped & pose); + geometry_msgs::msg::PoseStamped & pose); /** * @brief Visualize the search expansions @@ -141,12 +144,14 @@ class GoalIntentExtractor 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_; diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index a7949e5a12c..2967a2dc18a 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -27,7 +27,7 @@ namespace nav2_route { -void BreadthFirstSearch::initialize(nav2_costmap_2d::Costmap2D * costmap, int max_iterations) +void BreadthFirstSearch::initialize(std::shared_ptr costmap, int max_iterations) { costmap_ = costmap; diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index f6b52ba585b..c687df3255e 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -39,6 +39,7 @@ 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; @@ -96,10 +97,11 @@ void GoalIntentExtractor::configure( } } -void GoalIntentExtractor::activate() +void GoalIntentExtractor::initialize() { if (enable_search_) { - bfs_->initialize(costmap_sub_->getCostmap().get(), max_iterations_); + bfs_->initialize(costmap_sub_->getCostmap(), max_iterations_); + costmap_frame_ = costmap_sub_->getFrameID(); } } @@ -110,15 +112,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; @@ -156,9 +159,8 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) } } - // transform to route_frame - start_ = transformPose(start_pose); - goal_ = transformPose(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 @@ -255,28 +257,36 @@ Route GoalIntentExtractor::pruneStartandGoal( unsigned int GoalIntentExtractor::associatePoseWithGraphNode( std::vector node_indices, - const geometry_msgs::msg::PoseStamped & pose) + geometry_msgs::msg::PoseStamped & pose) { + auto start = transformPose(pose, costmap_frame_); unsigned int s_mx, s_my, g_mx, g_my; if (!costmap_sub_->getCostmap()->worldToMap( - pose.pose.position.x, pose.pose.position.y, + start.pose.position.x, start.pose.position.y, s_mx, s_my)) { throw nav2_core::StartOutsideMapBounds( - "Start Coordinates of(" + std::to_string(pose.pose.position.x) + ", " + - std::to_string(pose.pose.position.y) + ") was outside bounds"); + "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + + std::to_string(start.pose.position.y) + ") was outside bounds"); } if (costmap_sub_->getCostmap()->getCost(s_mx, s_my) == nav2_costmap_2d::LETHAL_OBSTACLE) { throw nav2_core::StartOccupied("Start was in lethal cost"); } - //TODO(jwallace42): double check the frames, probably need to move into the map frame... std::vector valid_node_indices; std::vector goals; for (const auto & node_index : node_indices) { - float goal_x = (*graph_)[node_index].coords.x; - float goal_y = (*graph_)[node_index].coords.y; + 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_sub_->getCostmap()->worldToMap(goal_x, goal_y, g_mx, g_my)) { RCLCPP_WARN_STREAM( logger_, "Goal coordinate of(" + std::to_string(goal_x) + ", " + diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index e44805820e7..ba6209ffa08 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -101,7 +101,7 @@ nav2_util::CallbackReturn RouteServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - goal_intent_extractor_->activate(); + goal_intent_extractor_->initialize(); compute_route_server_->activate(); compute_and_track_route_server_->activate(); graph_vis_publisher_->on_activate(); diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index fc88e7c8532..932e71822c9 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -29,11 +29,11 @@ class BFSTestFixture : public ::testing::Test { costmap = std::make_unique(x_size, y_size, 0.0, 0.0, 1); - bfs.initialize(costmap.get(), 200); + bfs.initialize(costmap, 200); } BreadthFirstSearch bfs; - std::unique_ptr costmap; + std::shared_ptr costmap; }; TEST_F(BFSTestFixture, free_space) diff --git a/nav2_route/test/test_goal_intent_extractor.cpp b/nav2_route/test/test_goal_intent_extractor.cpp index 5435764b8ed..92ed00e3467 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) From 459f55c398d162f9c30f95943371bff486364ead Mon Sep 17 00:00:00 2001 From: gg Date: Sun, 10 Sep 2023 13:29:37 -0400 Subject: [PATCH 31/36] remove changes used for testing --- nav2_bringup/launch/navigation_launch.py | 2 -- nav2_bt_navigator/CMakeLists.txt | 1 - nav2_route/src/goal_intent_extractor.cpp | 4 ++-- nav2_route/src/graph_loader.cpp | 5 +---- 4 files changed, 3 insertions(+), 9 deletions(-) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index cfc9e141bcc..3497ae11700 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -147,7 +147,6 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - prefix=['xterm -e gdb --args'], remappings=remappings), Node( package='nav2_behaviors', @@ -168,7 +167,6 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - # prefix=['xterm -e gdb -ex run --args'], remappings=remappings), Node( package='nav2_waypoint_follower', diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index 423e0e8533f..e36fb0de03b 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -19,7 +19,6 @@ find_package(nav2_core REQUIRED) find_package(tf2_ros REQUIRED) nav2_package() -add_compile_options(-g) include_directories( include diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index c687df3255e..9784bfb0467 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -68,7 +68,7 @@ void GoalIntentExtractor::configure( node_spatial_tree_->computeTree(graph); nav2_util::declare_parameter_if_not_declared( - node, "enable_search", rclcpp::ParameterValue(true)); + node, "enable_search", rclcpp::ParameterValue(false)); enable_search_ = node->get_parameter("enable_search").as_bool(); if (enable_search_) { @@ -83,7 +83,7 @@ void GoalIntentExtractor::configure( max_iterations_ = node->get_parameter("max_iterations").as_int(); nav2_util::declare_parameter_if_not_declared( - node, "enable_search_viz", rclcpp::ParameterValue(true)); + node, "enable_search_viz", rclcpp::ParameterValue(false)); enable_search_viz_ = node->get_parameter("enable_search_viz").as_bool(); costmap_sub_ = diff --git a/nav2_route/src/graph_loader.cpp b/nav2_route/src/graph_loader.cpp index 8a60c2628c5..1de8a677535 100644 --- a/nav2_route/src/graph_loader.cpp +++ b/nav2_route/src/graph_loader.cpp @@ -15,7 +15,6 @@ #include #include "nav2_route/graph_loader.hpp" -#include "ament_index_cpp/get_package_share_directory.hpp" namespace nav2_route { @@ -32,9 +31,7 @@ GraphLoader::GraphLoader( route_frame_ = frame; nav2_util::declare_parameter_if_not_declared( - node, "graph_filepath", rclcpp::ParameterValue( - ament_index_cpp::get_package_share_directory("nav2_route") + - "/graphs/aws_graph.geojson")); + node, "graph_filepath", rclcpp::ParameterValue(std::string(""))); graph_filepath_ = node->get_parameter("graph_filepath").as_string(); // Default Graph Parser From 90735c970efd64c093cd7ab25e6be24570ae0d04 Mon Sep 17 00:00:00 2001 From: gg Date: Sun, 17 Sep 2023 13:33:47 -0400 Subject: [PATCH 32/36] handle route to costmap transform, cleanup --- .../nav2_route/goal_intent_extractor.hpp | 1 + nav2_route/src/goal_intent_extractor.cpp | 30 +++++++++---------- 2 files changed, 16 insertions(+), 15 deletions(-) diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index ad1a8a2e34e..9a4e9e84309 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -160,6 +160,7 @@ class GoalIntentExtractor bool enable_search_viz_; int max_iterations_; std::unique_ptr costmap_sub_; + std::shared_ptr costmap_; std::unique_ptr bfs_; rclcpp::Publisher::SharedPtr start_expansion_viz_; rclcpp::Publisher::SharedPtr goal_expansion_viz_; diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index 9784bfb0467..5c47fa54baf 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -100,7 +100,8 @@ void GoalIntentExtractor::configure( void GoalIntentExtractor::initialize() { if (enable_search_) { - bfs_->initialize(costmap_sub_->getCostmap(), max_iterations_); + costmap_ = costmap_sub_->getCostmap(); + bfs_->initialize(costmap_, max_iterations_); costmap_frame_ = costmap_sub_->getFrameID(); } } @@ -259,18 +260,18 @@ unsigned int GoalIntentExtractor::associatePoseWithGraphNode( std::vector node_indices, geometry_msgs::msg::PoseStamped & pose) { - auto start = transformPose(pose, costmap_frame_); + auto pose_transformed = transformPose(pose, costmap_frame_); unsigned int s_mx, s_my, g_mx, g_my; - if (!costmap_sub_->getCostmap()->worldToMap( - start.pose.position.x, start.pose.position.y, + 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(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was outside bounds"); + "Start Coordinates of(" + std::to_string(pose_transformed.pose.position.x) + ", " + + std::to_string(pose_transformed.pose.position.y) + ") was outside bounds"); } - if (costmap_sub_->getCostmap()->getCost(s_mx, s_my) == nav2_costmap_2d::LETHAL_OBSTACLE) { + if (costmap_->getCost(s_mx, s_my) == nav2_costmap_2d::LETHAL_OBSTACLE) { throw nav2_core::StartOccupied("Start was in lethal cost"); } @@ -287,14 +288,14 @@ unsigned int GoalIntentExtractor::associatePoseWithGraphNode( float goal_x = goal.pose.position.x; float goal_y = goal.pose.position.y; - if (!costmap_sub_->getCostmap()->worldToMap(goal_x, goal_y, g_mx, g_my)) { + 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_sub_->getCostmap()->getCost(g_mx, g_my) == nav2_costmap_2d::LETHAL_OBSTACLE) { + 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."); @@ -329,16 +330,15 @@ void GoalIntentExtractor::visualizeExpansions( return; } - auto costmap = costmap_sub_->getCostmap(); nav_msgs::msg::OccupancyGrid occ_grid_msg; - unsigned int width = costmap->getSizeInCellsX(); - unsigned int height = costmap->getSizeInCellsY(); + 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.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.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); From 0ca9cd44dfe51e190b174c64ceb5cd4cb837a73e Mon Sep 17 00:00:00 2001 From: gg Date: Tue, 26 Sep 2023 15:00:53 -0400 Subject: [PATCH 33/36] octogon issue --- nav2_bringup/launch/navigation_launch.py | 1 + nav2_bringup/rviz/nav2_default_view.rviz | 46 ++++++ nav2_route/CMakeLists.txt | 1 + .../nav2_route/breadth_first_search.hpp | 21 ++- .../nav2_route/goal_intent_extractor.hpp | 3 + nav2_route/src/breadth_first_search.cpp | 50 +++++-- nav2_route/src/goal_intent_extractor.cpp | 25 +++- nav2_route/src/graph_loader.cpp | 6 +- nav2_route/src/route_server.cpp | 5 + nav2_route/test/test_breadth_first_search.cpp | 103 +++++++------- .../nav2_simple_commander/example_route.py | 132 +++++++++--------- 11 files changed, 265 insertions(+), 128 deletions(-) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 3497ae11700..785d8726859 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -147,6 +147,7 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], + prefix=['xterm -e gdb --args'], remappings=remappings), Node( package='nav2_behaviors', diff --git a/nav2_bringup/rviz/nav2_default_view.rviz b/nav2_bringup/rviz/nav2_default_view.rviz index c08384f53d3..dad07c29993 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_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index ab482abf358..74b06e58103 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) find_package(nanoflann REQUIRED) find_package(nlohmann_json REQUIRED) +add_compile_options(-g) nav2_package() diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index 8d13e8a03d8..2e19d3a7774 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" @@ -36,10 +37,20 @@ struct SimpleNode { SimpleNode(unsigned int index) : index(index), - explored(false) {} + queued(false), + visited(false), + cost(std::numeric_limits::max()){} unsigned int index; - bool explored; + bool queued; + bool visited; + float cost; +}; + +struct CompareNodeCost { + bool operator()(const SimpleNode * a, const SimpleNode * b) const { + return a->cost > b->cost; + } }; /** @@ -116,6 +127,11 @@ class BreadthFirstSearch */ 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_; @@ -124,6 +140,7 @@ class BreadthFirstSearch 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_; diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index 9a4e9e84309..9b6dc29b53f 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -164,6 +164,9 @@ class GoalIntentExtractor std::unique_ptr bfs_; 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/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 2967a2dc18a..8d8e512a12e 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -37,10 +37,13 @@ void BreadthFirstSearch::initialize(std::shared_ptr max_index_ = x_size * y_size; x_size_ = x_size; - neighbors_grid_offsets_ = {-1, +1, - -x_size, +x_size, - -x_size - 1, -x_size + 1, - +x_size - 1, +x_size + 1}; + 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; } @@ -70,15 +73,20 @@ void BreadthFirstSearch::setGoals(std::vector & go void BreadthFirstSearch::search(unsigned int & goal) { - std::queue queue; + std::priority_queue, CompareNodeCost> queue; - start_->explored = true; + start_->queued = true; + start_->cost = 0.0f; queue.push(start_); int iteration = 0; while (!queue.empty()) { - auto & current = queue.front(); + auto * current = queue.top(); queue.pop(); + current->queued = false; + std::cout << "Current index: " << current->index << std::endl; + std::cout << "Current cost: " << current->cost << std::endl; + std::cout << std::endl; if (iteration > max_iterations_) { throw nav2_core::PlannerTimedOut("Exceeded maximum iterations"); @@ -96,11 +104,20 @@ void BreadthFirstSearch::search(unsigned int & goal) getNeighbors(current->index, neighbors); for (const auto neighbor : neighbors) { - if (!neighbor->explored) { - neighbor->explored = true; - queue.push(neighbor); + if (!neighbor->visited) { + float updated_cost = current->cost + calculateCost(current->index, neighbor->index); + if (updated_cost < neighbor->cost) { + neighbor->cost = updated_cost; + } + + if(!neighbor->queued) { + neighbor->queued = true; + queue.push(neighbor); + } } } + std::cout << std::endl; + current->visited = true; } throw nav2_core::NoValidPathCouldBeFound("No valid path found"); @@ -174,6 +191,19 @@ bool BreadthFirstSearch::inCollision(unsigned int index) return false; } + +float BreadthFirstSearch::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) { + // SquareRoot of 2 + return 1.41421356237f; + } + } + return 1.0f; +} + void BreadthFirstSearch::clearGraph() { graph_.clear(); diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index 5c47fa54baf..ba0db80bd72 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -68,9 +69,17 @@ void GoalIntentExtractor::configure( node_spatial_tree_->computeTree(graph); nav2_util::declare_parameter_if_not_declared( - node, "enable_search", rclcpp::ParameterValue(false)); + node, "enable_search", rclcpp::ParameterValue(true)); 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_) { std::string costmap_topic; nav2_util::declare_parameter_if_not_declared( @@ -83,7 +92,7 @@ void GoalIntentExtractor::configure( max_iterations_ = node->get_parameter("max_iterations").as_int(); nav2_util::declare_parameter_if_not_declared( - node, "enable_search_viz", rclcpp::ParameterValue(false)); + node, "enable_search_viz", rclcpp::ParameterValue(true)); enable_search_viz_ = node->get_parameter("enable_search_viz").as_bool(); costmap_sub_ = @@ -160,6 +169,9 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) } } + 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_); @@ -175,10 +187,12 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) if (enable_search_) { unsigned int valid_start_route_id = associatePoseWithGraphNode(start_route_ids, start_); + std::cout << "Start ID: " << valid_start_route_id << std::endl; visualizeExpansions(start_expansion_viz_); bfs_->clearGraph(); unsigned int valid_end_route_id = associatePoseWithGraphNode(end_route_ids, goal_); + std::cout << "End ID: " << valid_end_route_id << std::endl; visualizeExpansions(goal_expansion_viz_); bfs_->clearGraph(); @@ -271,9 +285,11 @@ unsigned int GoalIntentExtractor::associatePoseWithGraphNode( std::to_string(pose_transformed.pose.position.y) + ") was outside bounds"); } - if (costmap_->getCost(s_mx, s_my) == nav2_costmap_2d::LETHAL_OBSTACLE) { + 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; @@ -316,6 +332,7 @@ unsigned int GoalIntentExtractor::associatePoseWithGraphNode( // // The visiblity check only validates the first node in goal array // return valid_node_indices.front(); // } + RCLCPP_INFO(logger_, "Visability Check failed"); unsigned int goal; bfs_->search(goal); diff --git a/nav2_route/src/graph_loader.cpp b/nav2_route/src/graph_loader.cpp index 1de8a677535..6bb460ba092 100644 --- a/nav2_route/src/graph_loader.cpp +++ b/nav2_route/src/graph_loader.cpp @@ -12,9 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include "nav2_route/graph_loader.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" namespace nav2_route { @@ -31,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/route_server.cpp b/nav2_route/src/route_server.cpp index ba6209ffa08..2fdde19f535 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -43,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), @@ -204,6 +205,8 @@ Route RouteServer::findRoute( { // Find the search boundaries auto [start_route, end_route] = goal_intent_extractor_->findStartandGoal(goal); + std::cout << "Start ID: " << start_route << std::endl; + std::cout << "End ID: " << end_route << std::endl; if (rerouting_info.rerouting_start_id != std::numeric_limits::max()) { start_route = id_to_graph_map_.at(rerouting_info.rerouting_start_id); @@ -217,6 +220,8 @@ Route RouteServer::findRoute( route.start_node = &graph_.at(start_route); } else { // Compute the route via graph-search, returns a node-edge sequence + std::cout << "Start ID: " << start_route << std::endl; + std::cout << "End ID: " << end_route << std::endl; route = route_planner_->findRoute(graph_, start_route, end_route, rerouting_info.blocked_ids); } diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index 932e71822c9..08f1045ec34 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -38,63 +38,72 @@ class BFSTestFixture : public ::testing::Test TEST_F(BFSTestFixture, free_space) { - initialize(10u, 10u); + initialize(100u, 100u); bfs.setStart(0u, 0u); + std::cout << "Set start" << std::endl; std::vector goals; - goals.push_back({1u, 1u}); + goals.push_back({100u, 100u}); bfs.setGoals(goals); + std::cout << "set goal" << std::endl; unsigned int goal = 0; + std::cout << "start search" << std::endl; ASSERT_NO_THROW(bfs.search(goal)); - - EXPECT_EQ(costmap->getIndex(goals[goal].x, goals[goal].y), costmap->getIndex(1u, 1u)); - bfs.clearGraph(); -} - -TEST_F(BFSTestFixture, 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); + std::cout << "search" << std::endl; + auto graph = bfs.getGraph(); + for (const auto node : *graph) { + std::cout << "Index: " << node.first << std::endl; + std::cout << "Cost: " << node.second.cost << std::endl; } - bfs.setStart(0u, 0u); - - std::vector goals; - goals.push_back({5u, 0u}); - goals.push_back({0u, 4u}); - bfs.setGoals(goals); - - unsigned int goal = 0; - EXPECT_NO_THROW(bfs.search(goal)); - - EXPECT_EQ(costmap->getIndex(goals[goal].x, goals[goal].y), costmap->getIndex(0u, 4u)); + // EXPECT_EQ(costmap->getIndex(goals[goal].x, goals[goal].y), costmap->getIndex(0u, 0u)); bfs.clearGraph(); } -TEST_F(BFSTestFixture, ray_trace) -{ - initialize(10u, 10u); - - bfs.setStart(0u, 0u); - - std::vector goals; - goals.push_back({5u, 5u}); - bfs.setGoals(goals); - - bool result = bfs.isFirstGoalVisible(); - EXPECT_TRUE(result); - bfs.clearGraph(); - - bfs.setStart(0u, 0u); - bfs.setGoals(goals); - costmap->setCost(2u, 2u, LETHAL_OBSTACLE); - result = bfs.isFirstGoalVisible(); - EXPECT_FALSE(result); - - bfs.clearGraph(); -} +// TEST_F(BFSTestFixture, 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); +// } +// +// bfs.setStart(0u, 0u); +// +// std::vector goals; +// goals.push_back({5u, 0u}); +// goals.push_back({0u, 4u}); +// bfs.setGoals(goals); +// +// unsigned int goal = 0; +// EXPECT_NO_THROW(bfs.search(goal)); +// +// EXPECT_EQ(costmap->getIndex(goals[goal].x, goals[goal].y), costmap->getIndex(0u, 4u)); +// bfs.clearGraph(); +// } +// +// TEST_F(BFSTestFixture, ray_trace) +// { +// initialize(10u, 10u); +// +// bfs.setStart(0u, 0u); +// +// std::vector goals; +// goals.push_back({5u, 5u}); +// bfs.setGoals(goals); +// +// bool result = bfs.isFirstGoalVisible(); +// EXPECT_TRUE(result); +// bfs.clearGraph(); +// +// bfs.setStart(0u, 0u); +// bfs.setGoals(goals); +// costmap->setCost(2u, 2u, LETHAL_OBSTACLE); +// result = bfs.isFirstGoalVisible(); +// EXPECT_FALSE(result); +// +// bfs.clearGraph(); +// } diff --git a/nav2_simple_commander/nav2_simple_commander/example_route.py b/nav2_simple_commander/nav2_simple_commander/example_route.py index f2f61263c9d..11d37885081 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,70 +69,73 @@ def main(): goal_pose.pose.position.y = -5.42 goal_pose.pose.orientation.w = 1.0 - # 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) - - # May also use NodeIDs on the graph if they are known by passing them instead as `int` - navigator.getAndTrackRoute(initial_pose, goal_pose) - - # Note for the route server, we have a special route argument in the API b/c it may be - # providing feedback messages simultaneously to others (e.g. controller or WPF as below) - isTrackingRoute = True - task_canceled = False - last_feedback = None - while not navigator.isTaskComplete(trackingRoute=isTrackingRoute): - ################################################ - # - # Implement some code here for your application! - # - ################################################ - - # Do something with the feedback, which contains the route / path if tracking - feedback = navigator.getFeedback(trackingRoute=isTrackingRoute) - while feedback is not None: - if not last_feedback or \ - (feedback.last_node_id != last_feedback.last_node_id or \ - feedback.next_node_id != last_feedback.next_node_id): - print('Passed node ' + str(feedback.last_node_id) + - ' to next node ' + str(feedback.next_node_id) + - ' along edge ' + str(feedback.current_edge_id) + '.') - - last_feedback = feedback - - if feedback.rerouted: - # Follow the path from the route server using the controller server - print('Passing new route to controller!') - navigator.followPath(feedback.path) - - # May instead use the waypoint follower (or nav through poses) and use the route's sparse nodes! - # print("Passing route to waypoint follower!") - # nodes = [toPoseStamped(x.position, feedback.route.header) for x in feedback.route.nodes] - # navigator.followWaypoints(nodes) - - feedback = navigator.getFeedback(trackingRoute=isTrackingRoute) - - # Check if followPath or WPF task is done (or failed), will cancel all current tasks, including route - if navigator.isTaskComplete(): - print("Controller or waypoint follower server completed its task!") - navigator.cancelTask() - task_canceled = True - - # Route server will return completed status before the controller / WPF server - # so wait for the actual robot task processing server to complete - while not navigator.isTaskComplete() and not task_canceled: - pass - - # Do something depending on the return code - result = navigator.getResult() - if result == TaskResult.SUCCEEDED: - print('Goal succeeded!') - elif result == TaskResult.CANCELED: - print('Goal was canceled!') - elif result == TaskResult.FAILED: - print('Goal failed!') - else: - print('Goal has an invalid return status!') + while(True): + time.sleep(5) + + # # 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) + # + # # May also use NodeIDs on the graph if they are known by passing them instead as `int` + # navigator.getAndTrackRoute(initial_pose, goal_pose) + # + # # Note for the route server, we have a special route argument in the API b/c it may be + # # providing feedback messages simultaneously to others (e.g. controller or WPF as below) + # isTrackingRoute = True + # task_canceled = False + # last_feedback = None + # while not navigator.isTaskComplete(trackingRoute=isTrackingRoute): + # ################################################ + # # + # # Implement some code here for your application! + # # + # ################################################ + # + # # Do something with the feedback, which contains the route / path if tracking + # feedback = navigator.getFeedback(trackingRoute=isTrackingRoute) + # while feedback is not None: + # if not last_feedback or \ + # (feedback.last_node_id != last_feedback.last_node_id or \ + # feedback.next_node_id != last_feedback.next_node_id): + # print('Passed node ' + str(feedback.last_node_id) + + # ' to next node ' + str(feedback.next_node_id) + + # ' along edge ' + str(feedback.current_edge_id) + '.') + # + # last_feedback = feedback + # + # if feedback.rerouted: + # # Follow the path from the route server using the controller server + # print('Passing new route to controller!') + # navigator.followPath(feedback.path) + # + # # May instead use the waypoint follower (or nav through poses) and use the route's sparse nodes! + # # print("Passing route to waypoint follower!") + # # nodes = [toPoseStamped(x.position, feedback.route.header) for x in feedback.route.nodes] + # # navigator.followWaypoints(nodes) + # + # feedback = navigator.getFeedback(trackingRoute=isTrackingRoute) + # + # # Check if followPath or WPF task is done (or failed), will cancel all current tasks, including route + # if navigator.isTaskComplete(): + # print("Controller or waypoint follower server completed its task!") + # navigator.cancelTask() + # task_canceled = True + # + # # Route server will return completed status before the controller / WPF server + # # so wait for the actual robot task processing server to complete + # while not navigator.isTaskComplete() and not task_canceled: + # pass + # + # # Do something depending on the return code + # result = navigator.getResult() + # if result == TaskResult.SUCCEEDED: + # print('Goal succeeded!') + # elif result == TaskResult.CANCELED: + # print('Goal was canceled!') + # elif result == TaskResult.FAILED: + # print('Goal failed!') + # else: + # print('Goal has an invalid return status!') navigator.lifecycleShutdown() From 65908886e5824b11ce72659e3993f7da7c44a644 Mon Sep 17 00:00:00 2001 From: gg Date: Tue, 17 Oct 2023 20:13:21 -0400 Subject: [PATCH 34/36] dijkstra implementation/other fixes --- nav2_bringup/launch/navigation_launch.py | 1 - nav2_route/CMakeLists.txt | 1 - .../nav2_route/breadth_first_search.hpp | 51 ++++--- .../nav2_route/goal_intent_extractor.hpp | 2 +- nav2_route/src/breadth_first_search.cpp | 49 +++---- nav2_route/src/goal_intent_extractor.cpp | 34 +++-- nav2_route/src/route_server.cpp | 4 - nav2_route/test/test_breadth_first_search.cpp | 117 ++++++++-------- .../nav2_simple_commander/example_route.py | 131 +++++++++--------- 9 files changed, 179 insertions(+), 211 deletions(-) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 785d8726859..3497ae11700 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -147,7 +147,6 @@ def generate_launch_description(): respawn_delay=2.0, parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], - prefix=['xterm -e gdb --args'], remappings=remappings), Node( package='nav2_behaviors', diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index 74b06e58103..ab482abf358 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) find_package(nanoflann REQUIRED) find_package(nlohmann_json REQUIRED) -add_compile_options(-g) nav2_package() diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/breadth_first_search.hpp index 2e19d3a7774..f9da222a7e8 100644 --- a/nav2_route/include/nav2_route/breadth_first_search.hpp +++ b/nav2_route/include/nav2_route/breadth_first_search.hpp @@ -1,5 +1,5 @@ // Copyright (c) 2020, Samsung Research America -// Copyright (c) 2023 Joshua Wallace +// 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. @@ -29,39 +29,38 @@ namespace nav2_route { -/** - * @struct nav2_route::SimpleNode - * @brief A Node implementation for the breadth first search - */ -struct SimpleNode -{ - SimpleNode(unsigned int index) - : index(index), - queued(false), - visited(false), - cost(std::numeric_limits::max()){} - - unsigned int index; - bool queued; - bool visited; - float cost; -}; - -struct CompareNodeCost { - bool operator()(const SimpleNode * a, const SimpleNode * b) const { - return a->cost > b->cost; - } -}; - /** * @class nav2_route::BreadthFirstSearch * @brief Preforms a breadth first search between the start and goal */ -class BreadthFirstSearch +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 diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index 9b6dc29b53f..f29d57fdf73 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -161,7 +161,7 @@ class GoalIntentExtractor int max_iterations_; std::unique_ptr costmap_sub_; std::shared_ptr costmap_; - std::unique_ptr bfs_; + std::unique_ptr ds_; rclcpp::Publisher::SharedPtr start_expansion_viz_; rclcpp::Publisher::SharedPtr goal_expansion_viz_; diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/breadth_first_search.cpp index 8d8e512a12e..3835f174c4b 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/breadth_first_search.cpp @@ -15,7 +15,6 @@ #include "nav2_route/breadth_first_search.hpp" -#include #include #include #include @@ -27,7 +26,7 @@ namespace nav2_route { -void BreadthFirstSearch::initialize(std::shared_ptr costmap, int max_iterations) +void DijkstraSearch::initialize(std::shared_ptr costmap, int max_iterations) { costmap_ = costmap; @@ -48,7 +47,7 @@ void BreadthFirstSearch::initialize(std::shared_ptr max_iterations_ = max_iterations; } -BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int index) +DijkstraSearch::NodePtr DijkstraSearch::addToGraph(const unsigned int index) { auto iter = graph_.find(index); if (iter != graph_.end()) { @@ -58,12 +57,12 @@ BreadthFirstSearch::NodePtr BreadthFirstSearch::addToGraph(const unsigned int in return &(graph_.emplace(index, SimpleNode(index)).first->second); } -void BreadthFirstSearch::setStart(unsigned int mx, unsigned int my) +void DijkstraSearch::setStart(unsigned int mx, unsigned int my) { start_ = addToGraph(costmap_->getIndex(mx, my)); } -void BreadthFirstSearch::setGoals(std::vector & goals) +void DijkstraSearch::setGoals(std::vector & goals) { goals_.clear(); for (const auto & goal : goals) { @@ -71,28 +70,22 @@ void BreadthFirstSearch::setGoals(std::vector & go } } -void BreadthFirstSearch::search(unsigned int & goal) +void DijkstraSearch::search(unsigned int & goal) { - std::priority_queue, CompareNodeCost> queue; - - start_->queued = true; + NodeQueue queue; start_->cost = 0.0f; - queue.push(start_); + queue.push(std::make_pair(start_->cost, start_)); int iteration = 0; while (!queue.empty()) { - auto * current = queue.top(); + + auto current = queue.top().second; queue.pop(); - current->queued = false; - std::cout << "Current index: " << current->index << std::endl; - std::cout << "Current cost: " << current->cost << std::endl; - std::cout << std::endl; if (iteration > max_iterations_) { throw nav2_core::PlannerTimedOut("Exceeded maximum iterations"); } - // Check goals for (unsigned int i = 0; i < goals_.size(); ++i) { if (current->index == goals_[i]->index) { goal = i; @@ -108,22 +101,17 @@ void BreadthFirstSearch::search(unsigned int & goal) float updated_cost = current->cost + calculateCost(current->index, neighbor->index); if (updated_cost < neighbor->cost) { neighbor->cost = updated_cost; - } - - if(!neighbor->queued) { - neighbor->queued = true; - queue.push(neighbor); + queue.push(std::make_pair(neighbor->cost, neighbor)); } } } - std::cout << std::endl; current->visited = true; } throw nav2_core::NoValidPathCouldBeFound("No valid path found"); } -void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & neighbors) +void DijkstraSearch::getNeighbors(unsigned int parent_index, NodeVector & neighbors) { unsigned int p_mx, p_my; costmap_->indexToCells(parent_index, p_mx, p_my); @@ -161,7 +149,7 @@ void BreadthFirstSearch::getNeighbors(unsigned int parent_index, NodeVector & ne } } -bool BreadthFirstSearch::isFirstGoalVisible() +bool DijkstraSearch::isFirstGoalVisible() { unsigned int s_mx, s_my, g_mx, g_my; costmap_->indexToCells(start_->index, s_mx, s_my); @@ -179,7 +167,7 @@ bool BreadthFirstSearch::isFirstGoalVisible() return true; } -bool BreadthFirstSearch::inCollision(unsigned int index) +bool DijkstraSearch::inCollision(unsigned int index) { unsigned char cost = costmap_->getCost(index); @@ -192,24 +180,23 @@ bool BreadthFirstSearch::inCollision(unsigned int index) } -float BreadthFirstSearch::calculateCost(unsigned int current_index, unsigned int neighbor_index) +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) { - // SquareRoot of 2 - return 1.41421356237f; + return 14.0f; } } - return 1.0f; + return 10.0f; } -void BreadthFirstSearch::clearGraph() +void DijkstraSearch::clearGraph() { graph_.clear(); } -std::unordered_map * BreadthFirstSearch::getGraph() +std::unordered_map * DijkstraSearch::getGraph() { return &graph_; } diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index ba0db80bd72..a710b6d507a 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -69,7 +69,7 @@ void GoalIntentExtractor::configure( node_spatial_tree_->computeTree(graph); nav2_util::declare_parameter_if_not_declared( - node, "enable_search", rclcpp::ParameterValue(true)); + node, "enable_search", rclcpp::ParameterValue(false)); enable_search_ = node->get_parameter("enable_search").as_bool(); route_start_pose_pub_ = @@ -92,12 +92,12 @@ void GoalIntentExtractor::configure( max_iterations_ = node->get_parameter("max_iterations").as_int(); nav2_util::declare_parameter_if_not_declared( - node, "enable_search_viz", rclcpp::ParameterValue(true)); + node, "enable_search_viz", rclcpp::ParameterValue(false)); enable_search_viz_ = node->get_parameter("enable_search_viz").as_bool(); costmap_sub_ = std::make_unique(node, costmap_topic); - bfs_ = std::make_unique(); + ds_ = std::make_unique(); start_expansion_viz_ = node->create_publisher( "start_expansions", 1); @@ -110,7 +110,7 @@ void GoalIntentExtractor::initialize() { if (enable_search_) { costmap_ = costmap_sub_->getCostmap(); - bfs_->initialize(costmap_, max_iterations_); + ds_->initialize(costmap_, max_iterations_); costmap_frame_ = costmap_sub_->getFrameID(); } } @@ -187,14 +187,12 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) if (enable_search_) { unsigned int valid_start_route_id = associatePoseWithGraphNode(start_route_ids, start_); - std::cout << "Start ID: " << valid_start_route_id << std::endl; visualizeExpansions(start_expansion_viz_); - bfs_->clearGraph(); + ds_->clearGraph(); unsigned int valid_end_route_id = associatePoseWithGraphNode(end_route_ids, goal_); - std::cout << "End ID: " << valid_end_route_id << std::endl; visualizeExpansions(goal_expansion_viz_); - bfs_->clearGraph(); + ds_->clearGraph(); return {valid_start_route_id, valid_end_route_id}; } @@ -325,17 +323,17 @@ unsigned int GoalIntentExtractor::associatePoseWithGraphNode( throw nav2_core::PlannerException("All goals were invalid"); } - bfs_->clearGraph(); - bfs_->setStart(s_mx, s_my); - bfs_->setGoals(goals); - // if (bfs_->isFirstGoalVisible()) { - // // The visiblity check only validates the first node in goal array - // return valid_node_indices.front(); - // } - RCLCPP_INFO(logger_, "Visability Check failed"); + 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; - bfs_->search(goal); + ds_->search(goal); return valid_node_indices[goal]; } @@ -359,7 +357,7 @@ void GoalIntentExtractor::visualizeExpansions( occ_grid_msg.info.origin.orientation.w = 1.0; occ_grid_msg.data.assign(width * height, 0); - auto graph = bfs_->getGraph(); + auto graph = ds_->getGraph(); for (const auto & node : *graph) { occ_grid_msg.data[node.first] = 100; diff --git a/nav2_route/src/route_server.cpp b/nav2_route/src/route_server.cpp index 2fdde19f535..c5679a1c559 100644 --- a/nav2_route/src/route_server.cpp +++ b/nav2_route/src/route_server.cpp @@ -205,8 +205,6 @@ Route RouteServer::findRoute( { // Find the search boundaries auto [start_route, end_route] = goal_intent_extractor_->findStartandGoal(goal); - std::cout << "Start ID: " << start_route << std::endl; - std::cout << "End ID: " << end_route << std::endl; if (rerouting_info.rerouting_start_id != std::numeric_limits::max()) { start_route = id_to_graph_map_.at(rerouting_info.rerouting_start_id); @@ -220,8 +218,6 @@ Route RouteServer::findRoute( route.start_node = &graph_.at(start_route); } else { // Compute the route via graph-search, returns a node-edge sequence - std::cout << "Start ID: " << start_route << std::endl; - std::cout << "End ID: " << end_route << std::endl; route = route_planner_->findRoute(graph_, start_route, end_route, rerouting_info.blocked_ids); } diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_breadth_first_search.cpp index 08f1045ec34..c93bbc66d80 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_breadth_first_search.cpp @@ -22,88 +22,79 @@ using namespace nav2_costmap_2d; //NOLINT using namespace nav2_route; //NOLINT -class BFSTestFixture : public ::testing::Test +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); - bfs.initialize(costmap, 200); + ds.initialize(costmap, 200); } - BreadthFirstSearch bfs; + DijkstraSearch ds; std::shared_ptr costmap; }; -TEST_F(BFSTestFixture, free_space) +TEST_F(DSTestFixture, free_space) { initialize(100u, 100u); - bfs.setStart(0u, 0u); - std::cout << "Set start" << std::endl; + ds.setStart(0u, 0u); std::vector goals; - goals.push_back({100u, 100u}); - bfs.setGoals(goals); - std::cout << "set goal" << std::endl; + goals.push_back({50u, 50u}); + ds.setGoals(goals); unsigned int goal = 0; - std::cout << "start search" << std::endl; - ASSERT_NO_THROW(bfs.search(goal)); - std::cout << "search" << std::endl; - auto graph = bfs.getGraph(); - for (const auto node : *graph) { - std::cout << "Index: " << node.first << std::endl; - std::cout << "Cost: " << node.second.cost << std::endl; + 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); } - // EXPECT_EQ(costmap->getIndex(goals[goal].x, goals[goal].y), costmap->getIndex(0u, 0u)); - bfs.clearGraph(); + 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(BFSTestFixture, 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); -// } -// -// bfs.setStart(0u, 0u); -// -// std::vector goals; -// goals.push_back({5u, 0u}); -// goals.push_back({0u, 4u}); -// bfs.setGoals(goals); -// -// unsigned int goal = 0; -// EXPECT_NO_THROW(bfs.search(goal)); -// -// EXPECT_EQ(costmap->getIndex(goals[goal].x, goals[goal].y), costmap->getIndex(0u, 4u)); -// bfs.clearGraph(); -// } -// -// TEST_F(BFSTestFixture, ray_trace) -// { -// initialize(10u, 10u); -// -// bfs.setStart(0u, 0u); -// -// std::vector goals; -// goals.push_back({5u, 5u}); -// bfs.setGoals(goals); -// -// bool result = bfs.isFirstGoalVisible(); -// EXPECT_TRUE(result); -// bfs.clearGraph(); -// -// bfs.setStart(0u, 0u); -// bfs.setGoals(goals); -// costmap->setCost(2u, 2u, LETHAL_OBSTACLE); -// result = bfs.isFirstGoalVisible(); -// EXPECT_FALSE(result); -// -// bfs.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_simple_commander/nav2_simple_commander/example_route.py b/nav2_simple_commander/nav2_simple_commander/example_route.py index 11d37885081..d5de59485cc 100644 --- a/nav2_simple_commander/nav2_simple_commander/example_route.py +++ b/nav2_simple_commander/nav2_simple_commander/example_route.py @@ -70,72 +70,71 @@ def main(): goal_pose.pose.orientation.w = 1.0 while(True): - time.sleep(5) - - # # 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) - # - # # May also use NodeIDs on the graph if they are known by passing them instead as `int` - # navigator.getAndTrackRoute(initial_pose, goal_pose) - # - # # Note for the route server, we have a special route argument in the API b/c it may be - # # providing feedback messages simultaneously to others (e.g. controller or WPF as below) - # isTrackingRoute = True - # task_canceled = False - # last_feedback = None - # while not navigator.isTaskComplete(trackingRoute=isTrackingRoute): - # ################################################ - # # - # # Implement some code here for your application! - # # - # ################################################ - # - # # Do something with the feedback, which contains the route / path if tracking - # feedback = navigator.getFeedback(trackingRoute=isTrackingRoute) - # while feedback is not None: - # if not last_feedback or \ - # (feedback.last_node_id != last_feedback.last_node_id or \ - # feedback.next_node_id != last_feedback.next_node_id): - # print('Passed node ' + str(feedback.last_node_id) + - # ' to next node ' + str(feedback.next_node_id) + - # ' along edge ' + str(feedback.current_edge_id) + '.') - # - # last_feedback = feedback - # - # if feedback.rerouted: - # # Follow the path from the route server using the controller server - # print('Passing new route to controller!') - # navigator.followPath(feedback.path) - # - # # May instead use the waypoint follower (or nav through poses) and use the route's sparse nodes! - # # print("Passing route to waypoint follower!") - # # nodes = [toPoseStamped(x.position, feedback.route.header) for x in feedback.route.nodes] - # # navigator.followWaypoints(nodes) - # - # feedback = navigator.getFeedback(trackingRoute=isTrackingRoute) - # - # # Check if followPath or WPF task is done (or failed), will cancel all current tasks, including route - # if navigator.isTaskComplete(): - # print("Controller or waypoint follower server completed its task!") - # navigator.cancelTask() - # task_canceled = True - # - # # Route server will return completed status before the controller / WPF server - # # so wait for the actual robot task processing server to complete - # while not navigator.isTaskComplete() and not task_canceled: - # pass - # - # # Do something depending on the return code - # result = navigator.getResult() - # if result == TaskResult.SUCCEEDED: - # print('Goal succeeded!') - # elif result == TaskResult.CANCELED: - # print('Goal was canceled!') - # elif result == TaskResult.FAILED: - # print('Goal failed!') - # else: - # print('Goal has an invalid return status!') + + # 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) + + # May also use NodeIDs on the graph if they are known by passing them instead as `int` + navigator.getAndTrackRoute(initial_pose, goal_pose) + + # Note for the route server, we have a special route argument in the API b/c it may be + # providing feedback messages simultaneously to others (e.g. controller or WPF as below) + isTrackingRoute = True + task_canceled = False + last_feedback = None + while not navigator.isTaskComplete(trackingRoute=isTrackingRoute): + ################################################ + # + # Implement some code here for your application! + # + ################################################ + + # Do something with the feedback, which contains the route / path if tracking + feedback = navigator.getFeedback(trackingRoute=isTrackingRoute) + while feedback is not None: + if not last_feedback or \ + (feedback.last_node_id != last_feedback.last_node_id or \ + feedback.next_node_id != last_feedback.next_node_id): + print('Passed node ' + str(feedback.last_node_id) + + ' to next node ' + str(feedback.next_node_id) + + ' along edge ' + str(feedback.current_edge_id) + '.') + + last_feedback = feedback + + if feedback.rerouted: + # Follow the path from the route server using the controller server + print('Passing new route to controller!') + navigator.followPath(feedback.path) + + # May instead use the waypoint follower (or nav through poses) and use the route's sparse nodes! + # print("Passing route to waypoint follower!") + # nodes = [toPoseStamped(x.position, feedback.route.header) for x in feedback.route.nodes] + # navigator.followWaypoints(nodes) + + feedback = navigator.getFeedback(trackingRoute=isTrackingRoute) + + # Check if followPath or WPF task is done (or failed), will cancel all current tasks, including route + if navigator.isTaskComplete(): + print("Controller or waypoint follower server completed its task!") + navigator.cancelTask() + task_canceled = True + + # Route server will return completed status before the controller / WPF server + # so wait for the actual robot task processing server to complete + while not navigator.isTaskComplete() and not task_canceled: + pass + + # Do something depending on the return code + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + print('Goal succeeded!') + elif result == TaskResult.CANCELED: + print('Goal was canceled!') + elif result == TaskResult.FAILED: + print('Goal failed!') + else: + print('Goal has an invalid return status!') navigator.lifecycleShutdown() From 6839d7d381187b5d9e8fd6ed937328ac03869a75 Mon Sep 17 00:00:00 2001 From: gg Date: Tue, 17 Oct 2023 20:22:31 -0400 Subject: [PATCH 35/36] rename --- nav2_route/CMakeLists.txt | 2 +- .../{breadth_first_search.hpp => dijkstra_search.hpp} | 0 nav2_route/include/nav2_route/goal_intent_extractor.hpp | 2 +- .../src/{breadth_first_search.cpp => dijkstra_search.cpp} | 2 +- nav2_route/src/goal_intent_extractor.cpp | 1 - nav2_route/test/CMakeLists.txt | 8 ++++---- ..._breadth_first_search.cpp => test_dijkstra_search.cpp} | 2 +- 7 files changed, 8 insertions(+), 9 deletions(-) rename nav2_route/include/nav2_route/{breadth_first_search.hpp => dijkstra_search.hpp} (100%) rename nav2_route/src/{breadth_first_search.cpp => dijkstra_search.cpp} (99%) rename nav2_route/test/{test_breadth_first_search.cpp => test_dijkstra_search.cpp} (98%) diff --git a/nav2_route/CMakeLists.txt b/nav2_route/CMakeLists.txt index ab482abf358..911be39ba10 100644 --- a/nav2_route/CMakeLists.txt +++ b/nav2_route/CMakeLists.txt @@ -54,7 +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 + src/dijkstra_search.cpp ) ament_target_dependencies(${library_name} diff --git a/nav2_route/include/nav2_route/breadth_first_search.hpp b/nav2_route/include/nav2_route/dijkstra_search.hpp similarity index 100% rename from nav2_route/include/nav2_route/breadth_first_search.hpp rename to nav2_route/include/nav2_route/dijkstra_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 f29d57fdf73..9d2ddafd99c 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -31,7 +31,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" +#include "nav2_route/dijkstra_search.hpp" namespace nav2_route { diff --git a/nav2_route/src/breadth_first_search.cpp b/nav2_route/src/dijkstra_search.cpp similarity index 99% rename from nav2_route/src/breadth_first_search.cpp rename to nav2_route/src/dijkstra_search.cpp index 3835f174c4b..c84ed17f500 100644 --- a/nav2_route/src/breadth_first_search.cpp +++ b/nav2_route/src/dijkstra_search.cpp @@ -13,7 +13,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_route/breadth_first_search.hpp" +#include "nav2_route/dijkstra_search.hpp" #include #include diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index a710b6d507a..7636a5d563f 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -21,7 +21,6 @@ #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #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" #include "nav_msgs/msg/occupancy_grid.hpp" diff --git a/nav2_route/test/CMakeLists.txt b/nav2_route/test/CMakeLists.txt index 657475448c3..d6109ef2dc1 100644 --- a/nav2_route/test/CMakeLists.txt +++ b/nav2_route/test/CMakeLists.txt @@ -144,12 +144,12 @@ target_link_libraries(test_route_server ) # Test Breadth first search -ament_add_gtest(test_bfs - test_breadth_first_search.cpp +ament_add_gtest(test_ds + test_dijkstra_search.cpp ) -ament_target_dependencies(test_bfs +ament_target_dependencies(test_ds ${dependencies} ) -target_link_libraries(test_bfs +target_link_libraries(test_ds ${library_name} ) diff --git a/nav2_route/test/test_breadth_first_search.cpp b/nav2_route/test/test_dijkstra_search.cpp similarity index 98% rename from nav2_route/test/test_breadth_first_search.cpp rename to nav2_route/test/test_dijkstra_search.cpp index c93bbc66d80..a1c662054b1 100644 --- a/nav2_route/test/test_breadth_first_search.cpp +++ b/nav2_route/test/test_dijkstra_search.cpp @@ -17,7 +17,7 @@ #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/cost_values.hpp" -#include "nav2_route/breadth_first_search.hpp" +#include "nav2_route/dijkstra_search.hpp" using namespace nav2_costmap_2d; //NOLINT using namespace nav2_route; //NOLINT From f6804c51ed70326ecb115c4039c6627d97a14b26 Mon Sep 17 00:00:00 2001 From: gg Date: Sun, 29 Oct 2023 12:08:20 -0400 Subject: [PATCH 36/36] rolling costmap work for search, small big fixes --- .../nav2_route/goal_intent_extractor.hpp | 1 + nav2_route/src/dijkstra_search.cpp | 1 + nav2_route/src/goal_intent_extractor.cpp | 32 +++++++++++++++++-- 3 files changed, 31 insertions(+), 3 deletions(-) diff --git a/nav2_route/include/nav2_route/goal_intent_extractor.hpp b/nav2_route/include/nav2_route/goal_intent_extractor.hpp index 9d2ddafd99c..790a62820e2 100644 --- a/nav2_route/include/nav2_route/goal_intent_extractor.hpp +++ b/nav2_route/include/nav2_route/goal_intent_extractor.hpp @@ -157,6 +157,7 @@ class GoalIntentExtractor 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_; diff --git a/nav2_route/src/dijkstra_search.cpp b/nav2_route/src/dijkstra_search.cpp index c84ed17f500..3d63d7f563c 100644 --- a/nav2_route/src/dijkstra_search.cpp +++ b/nav2_route/src/dijkstra_search.cpp @@ -106,6 +106,7 @@ void DijkstraSearch::search(unsigned int & goal) } } current->visited = true; + iteration++; } throw nav2_core::NoValidPathCouldBeFound("No valid path found"); diff --git a/nav2_route/src/goal_intent_extractor.cpp b/nav2_route/src/goal_intent_extractor.cpp index 7636a5d563f..147adb24d2e 100644 --- a/nav2_route/src/goal_intent_extractor.cpp +++ b/nav2_route/src/goal_intent_extractor.cpp @@ -80,6 +80,10 @@ void GoalIntentExtractor::configure( "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", @@ -91,7 +95,7 @@ void GoalIntentExtractor::configure( max_iterations_ = node->get_parameter("max_iterations").as_int(); nav2_util::declare_parameter_if_not_declared( - node, "enable_search_viz", rclcpp::ParameterValue(false)); + node, "enable_search_viz", rclcpp::ParameterValue(true)); enable_search_viz_ = node->get_parameter("enable_search_viz").as_bool(); costmap_sub_ = @@ -185,11 +189,32 @@ GoalIntentExtractor::findStartandGoal(const std::shared_ptr goal) } if (enable_search_) { - unsigned int valid_start_route_id = associatePoseWithGraphNode(start_route_ids, start_); + 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 = associatePoseWithGraphNode(end_route_ids, goal_); + 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(); @@ -319,6 +344,7 @@ unsigned int GoalIntentExtractor::associatePoseWithGraphNode( } if (goals.empty()) { + RCLCPP_WARN(logger_, "All goals for association were invalid"); throw nav2_core::PlannerException("All goals were invalid"); }