From f815e8bd8e3826603e97749bb31a3145268de023 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Alejandro=20Gonz=C3=A1lez?= <71234974+pepisg@users.noreply.github.com> Date: Mon, 8 Apr 2024 13:28:52 -0500 Subject: [PATCH 01/35] Scale cost critic's weight when dynamically updated (#4246) * Scale cost critic's weight when dynamically updated Signed-off-by: pepisg * sign off Signed-off-by: pepisg --------- Signed-off-by: pepisg --- nav2_mppi_controller/src/critics/cost_critic.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 7a848fe7abb..f348d7780a1 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -34,6 +34,12 @@ void CostCritic::initialize() // Normalized by cost value to put in same regime as other weights weight_ /= 254.0f; + // Normalize weight when parameter is changed dynamically as well + auto weightDynamicCb = [&](const rclcpp::Parameter & weight) { + weight_ = weight.as_double() / 254.0f; + }; + parameters_handler_->addDynamicParamCallback(name_ + ".cost_weight", weightDynamicCb); + collision_checker_.setCostmap(costmap_); possible_collision_cost_ = findCircumscribedCost(costmap_ros_); From 8af70abb35e6f93a0e6e6827c7bc3ad364b33d4b Mon Sep 17 00:00:00 2001 From: Tim Clephas Date: Wed, 10 Apr 2024 18:51:24 +0200 Subject: [PATCH 02/35] =?UTF-8?q?Fix=20compiler=20warning=20=E2=80=98error?= =?UTF-8?q?=5Flevel=E2=80=99=20may=20be=20used=20uninitialized=20(#4253)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Tim Clephas --- nav2_lifecycle_manager/src/lifecycle_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index cea3a182602..fdee32f9fc1 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -177,7 +177,7 @@ LifecycleManager::CreateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & error_level = diagnostic_msgs::msg::DiagnosticStatus::WARN; message = "Managed nodes have been shut down"; break; - case NodeState::UNKNOWN: + default: // NodeState::UNKNOWN error_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; message = "An error has occurred during a node state transition"; break; From 61a774be41fc236738ed7c103ba806be07b0914e Mon Sep 17 00:00:00 2001 From: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Date: Thu, 11 Apr 2024 23:46:03 +0200 Subject: [PATCH 03/35] Add expanding the ~/ to the full home dir of user in the path to the map yaml. (#4258) * Add user home expander of home sequence Signed-off-by: Wiktor Bajor * Add passing home dir as string instead of const char* Signed-off-by: Wiktor Bajor * Add docs Signed-off-by: Wiktor Bajor * Fix function declaration Signed-off-by: Wiktor Bajor * Fix linter issues Signed-off-by: Wiktor Bajor * Uncrustify linter Signed-off-by: Wiktor Bajor * Uncrustify linter Signed-off-by: Wiktor Bajor * Uncrustify linter: remove remove whitespace Signed-off-by: Wiktor Bajor --------- Signed-off-by: Wiktor Bajor --- .../include/nav2_map_server/map_io.hpp | 11 ++++++++ nav2_map_server/src/map_io.cpp | 28 +++++++++++++++++-- nav2_map_server/test/unit/test_map_io.cpp | 27 ++++++++++++++++++ 3 files changed, 64 insertions(+), 2 deletions(-) diff --git a/nav2_map_server/include/nav2_map_server/map_io.hpp b/nav2_map_server/include/nav2_map_server/map_io.hpp index bee7df73132..97dc81821c9 100644 --- a/nav2_map_server/include/nav2_map_server/map_io.hpp +++ b/nav2_map_server/include/nav2_map_server/map_io.hpp @@ -98,6 +98,17 @@ bool saveMapToFile( const nav_msgs::msg::OccupancyGrid & map, const SaveParameters & save_parameters); +/** + * @brief Expand ~/ to home user dir. + * @param yaml_filename Name of input YAML file. + * @param home_dir Expanded `~/`home dir or empty string if HOME not set + * + * @return Expanded string or input string if `~/` not expanded + */ +std::string expand_user_home_dir_if_needed( + std::string yaml_filename, + std::string home_dir); + } // namespace nav2_map_server #endif // NAV2_MAP_SERVER__MAP_IO_HPP_ diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 4803b6d3f64..85428490edb 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include "Magick++.h" #include "nav2_util/geometry_utils.hpp" @@ -115,9 +116,33 @@ T yaml_get_value(const YAML::Node & node, const std::string & key) } } +std::string get_home_dir() +{ + if (const char * home_dir = std::getenv("HOME")) { + return std::string{home_dir}; + } + return std::string{}; +} + +std::string expand_user_home_dir_if_needed( + std::string yaml_filename, + std::string home_variable_value) +{ + if (yaml_filename.size() < 2 || !(yaml_filename[0] == '~' && yaml_filename[1] == '/')) { + return yaml_filename; + } + if (home_variable_value.empty()) { + std::cout << "[INFO] [map_io]: Map yaml file name starts with '~/' but no HOME variable set. \n" + << "[INFO] [map_io] User home dir will be not expanded \n"; + return yaml_filename; + } + const std::string prefix{home_variable_value}; + return yaml_filename.replace(0, 1, prefix); +} + LoadParameters loadMapYaml(const std::string & yaml_filename) { - YAML::Node doc = YAML::LoadFile(yaml_filename); + YAML::Node doc = YAML::LoadFile(expand_user_home_dir_if_needed(yaml_filename, get_home_dir())); LoadParameters load_parameters; auto image_file_name = yaml_get_value(doc, "image"); @@ -295,7 +320,6 @@ LOAD_MAP_STATUS loadMapFromYaml( " for reason: " << e.what() << std::endl; return INVALID_MAP_METADATA; } - try { loadMapFromFile(load_parameters, map); } catch (std::exception & e) { diff --git a/nav2_map_server/test/unit/test_map_io.cpp b/nav2_map_server/test/unit/test_map_io.cpp index acc8d9debd4..31269cf2d47 100644 --- a/nav2_map_server/test/unit/test_map_io.cpp +++ b/nav2_map_server/test/unit/test_map_io.cpp @@ -311,3 +311,30 @@ TEST_F(MapIOTester, loadInvalidYAML) LoadParameters loadParameters; ASSERT_ANY_THROW(loadParameters = loadMapYaml(path(TEST_DIR) / path("invalid_file.yaml"))); } + +TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldNotChangeInputStringWhenShorterThanTwo) +{ + const std::string emptyFileName{}; + ASSERT_EQ(emptyFileName, expand_user_home_dir_if_needed(emptyFileName, "/home/user")); +} + +TEST( + HomeUserExpanderTestSuite, + homeUserExpanderShouldNotChangeInputStringWhenInputStringDoesNotStartWithHomeSequence) +{ + const std::string fileName{"valid_file.yaml"}; + ASSERT_EQ(fileName, expand_user_home_dir_if_needed(fileName, "/home/user")); +} + +TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldNotChangeInputStringWhenHomeVariableNotFound) +{ + const std::string fileName{"~/valid_file.yaml"}; + ASSERT_EQ(fileName, expand_user_home_dir_if_needed(fileName, "")); +} + +TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldExpandHomeSequenceWhenHomeVariableSet) +{ + const std::string fileName{"~/valid_file.yaml"}; + const std::string expectedOutputFileName{"/home/user/valid_file.yaml"}; + ASSERT_EQ(expectedOutputFileName, expand_user_home_dir_if_needed(fileName, "/home/user")); +} From c09708845e2327dfee1dac7d0a874fd9bf7f2598 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 11 Apr 2024 15:45:47 -0700 Subject: [PATCH 04/35] Smac Planner Optimizations; 9.1% improvement in speed (#4257) * optimizations * Update test_nodehybrid.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski --- nav2_smac_planner/CMakeLists.txt | 13 +--- .../include/nav2_smac_planner/node_hybrid.hpp | 4 +- nav2_smac_planner/src/a_star.cpp | 2 +- nav2_smac_planner/src/analytic_expansion.cpp | 2 +- nav2_smac_planner/src/collision_checker.cpp | 6 +- nav2_smac_planner/src/node_hybrid.cpp | 71 +++++++++++++------ nav2_smac_planner/test/test_nodehybrid.cpp | 2 +- 7 files changed, 59 insertions(+), 41 deletions(-) diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index 4bbe5c55ca7..d9da9fd1474 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -23,14 +23,11 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(angles REQUIRED) find_package(ompl REQUIRED) -find_package(OpenMP REQUIRED) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") - if(MSVC) add_compile_definitions(_USE_MATH_DEFINES) else() @@ -40,16 +37,8 @@ endif() include_directories( include ${OMPL_INCLUDE_DIRS} - ${OpenMP_INCLUDE_DIRS} ) -find_package(OpenMP) -if(OPENMP_FOUND) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") - set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") -endif() - set(library_name nav2_smac_planner) set(dependencies @@ -85,7 +74,7 @@ add_library(${library_name} SHARED src/node_basic.cpp ) -target_link_libraries(${library_name} ${OMPL_LIBRARIES} ${OpenMP_LIBRARIES} OpenMP::OpenMP_CXX) +target_link_libraries(${library_name} ${OMPL_LIBRARIES}) target_include_directories(${library_name} PUBLIC ${Eigen3_INCLUDE_DIRS}) ament_target_dependencies(${library_name} diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index c46a9471869..b6a33a444ae 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -463,16 +463,14 @@ class NodeHybrid Coordinates pose; // Constants required across all nodes but don't want to allocate more than once - static double travel_distance_cost; + static float travel_distance_cost; static HybridMotionTable motion_table; // Wavefront lookup and queue for continuing to expand as needed static LookupTable obstacle_heuristic_lookup_table; static ObstacleHeuristicQueue obstacle_heuristic_queue; - static nav2_costmap_2d::Costmap2D * sampled_costmap; static std::shared_ptr costmap_ros; static std::shared_ptr inflation_layer; - static CostmapDownsampler downsampler; // Dubin / Reeds-Shepp lookup and size for dereferencing static LookupTable dist_heuristic_lookup_table; static float size_lookup; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 1f0d7207631..0470bfd3029 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -365,7 +365,7 @@ bool AStarAlgorithm::createPath( } if (_best_heuristic_node.first < getToleranceHeuristic()) { - // If we run out of serach options, return the path that is closest, if within tolerance. + // If we run out of search options, return the path that is closest, if within tolerance. return _graph.at(_best_heuristic_node.second).backtracePath(path); } diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index e14033d0f1c..5f5f97d9ec7 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -180,7 +180,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansiondistance(from(), to()); // A move of sqrt(2) is guaranteed to be in a new cell - static const float sqrt_2 = std::sqrt(2.0f); + static const float sqrt_2 = sqrtf(2.0f); // If the length is too far, exit. This prevents unsafe shortcutting of paths // into higher cost areas far out from the goal itself, let search to the work of getting diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index c9ce030fab5..7e0396bddcf 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -112,10 +112,10 @@ bool GridCollisionChecker::inCollision( // if footprint, then we check for the footprint's points, but first see // if the robot is even potentially in an inscribed collision footprint_cost_ = static_cast(costmap_->getCost( - static_cast(x + 0.5), static_cast(y + 0.5))); + static_cast(x + 0.5f), static_cast(y + 0.5f))); if (footprint_cost_ < possible_collision_cost_) { - if (possible_collision_cost_ > 0) { + if (possible_collision_cost_ > 0.0f) { return false; } else { RCLCPP_ERROR_THROTTLE( @@ -162,7 +162,7 @@ bool GridCollisionChecker::inCollision( } else { // if radius, then we can check the center of the cost assuming inflation is used footprint_cost_ = static_cast(costmap_->getCost( - static_cast(x + 0.5), static_cast(y + 0.5))); + static_cast(x + 0.5f), static_cast(y + 0.5f))); if (footprint_cost_ == UNKNOWN && traverse_unknown) { return false; diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index aa1773e682d..63f121de3dd 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -36,15 +36,13 @@ namespace nav2_smac_planner // defining static member for all instance to share LookupTable NodeHybrid::obstacle_heuristic_lookup_table; -double NodeHybrid::travel_distance_cost = sqrt(2); +float NodeHybrid::travel_distance_cost = sqrtf(2.0f); HybridMotionTable NodeHybrid::motion_table; float NodeHybrid::size_lookup = 25; LookupTable NodeHybrid::dist_heuristic_lookup_table; -nav2_costmap_2d::Costmap2D * NodeHybrid::sampled_costmap = nullptr; std::shared_ptr NodeHybrid::costmap_ros = nullptr; std::shared_ptr NodeHybrid::inflation_layer = nullptr; -CostmapDownsampler NodeHybrid::downsampler; ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue; // Each of these tables are the projected motion models through @@ -489,35 +487,37 @@ void NodeHybrid::resetObstacleHeuristic( // than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality costmap_ros = costmap_ros_i; inflation_layer = nav2_costmap_2d::InflationLayer::getInflationLayer(costmap_ros); - sampled_costmap = costmap_ros->getCostmap(); + auto costmap = costmap_ros->getCostmap(); + + // Clear lookup table + unsigned int size = 0u; + unsigned int size_x = 0u; if (motion_table.downsample_obstacle_heuristic) { - std::weak_ptr ptr; - downsampler.on_configure(ptr, "fake_frame", "fake_topic", sampled_costmap, 2.0, true); - downsampler.on_activate(); - sampled_costmap = downsampler.downsample(2.0); + size_x = ceil(static_cast(costmap->getSizeInCellsX()) / 2.0f); + size = size_x * + ceil(static_cast(costmap->getSizeInCellsY()) / 2.0f); + } else { + size_x = costmap->getSizeInCellsX(); + size = size_x * costmap->getSizeInCellsY(); } - // Clear lookup table - unsigned int size = sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY(); if (obstacle_heuristic_lookup_table.size() == size) { // must reset all values std::fill( obstacle_heuristic_lookup_table.begin(), - obstacle_heuristic_lookup_table.end(), 0.0); + obstacle_heuristic_lookup_table.end(), 0.0f); } else { unsigned int obstacle_size = obstacle_heuristic_lookup_table.size(); - obstacle_heuristic_lookup_table.resize(size, 0.0); + obstacle_heuristic_lookup_table.resize(size, 0.0f); // must reset values for non-constructed indices std::fill_n( - obstacle_heuristic_lookup_table.begin(), obstacle_size, 0.0); + obstacle_heuristic_lookup_table.begin(), obstacle_size, 0.0f); } obstacle_heuristic_queue.clear(); - obstacle_heuristic_queue.reserve( - sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY()); + obstacle_heuristic_queue.reserve(size); // Set initial goal point to queue from. Divided by 2 due to downsampled costmap. - const unsigned int size_x = sampled_costmap->getSizeInCellsX(); unsigned int goal_index; if (motion_table.downsample_obstacle_heuristic) { goal_index = floor(goal_y / 2.0f) * size_x + floor(goal_x / 2.0f); @@ -562,8 +562,17 @@ float NodeHybrid::getObstacleHeuristic( const float & cost_penalty) { // If already expanded, return the cost - const unsigned int size_x = sampled_costmap->getSizeInCellsX(); + auto costmap = costmap_ros->getCostmap(); const bool is_circular = costmap_ros->getUseRadius(); + unsigned int size_x = 0u; + unsigned int size_y = 0u; + if (motion_table.downsample_obstacle_heuristic) { + size_x = ceil(static_cast(costmap->getSizeInCellsX()) / 2.0f); + size_y = ceil(static_cast(costmap->getSizeInCellsY()) / 2.0f); + } else { + size_x = costmap->getSizeInCellsX(); + size_y = costmap->getSizeInCellsY(); + } // Divided by 2 due to downsampled costmap. unsigned int start_y, start_x; @@ -600,8 +609,7 @@ float NodeHybrid::getObstacleHeuristic( ObstacleHeuristicComparator{}); const int size_x_int = static_cast(size_x); - const unsigned int size_y = sampled_costmap->getSizeInCellsY(); - const float sqrt2 = sqrt(2.0f); + const float sqrt2 = sqrtf(2.0f); float c_cost, cost, travel_cost, new_cost, existing_cost; unsigned int idx, mx, my; unsigned int new_idx = 0; @@ -632,7 +640,30 @@ float NodeHybrid::getObstacleHeuristic( // if neighbor path is better and non-lethal, set new cost and add to queue if (new_idx < size_x * size_y) { - cost = static_cast(sampled_costmap->getCost(new_idx)); + if (downsample_H) { + // Get costmap values as if downsampled + unsigned int y_offset = (new_idx / size_x) * 2; + unsigned int x_offset = (new_idx - ((new_idx / size_x) * size_x)) * 2; + cost = costmap->getCost(x_offset, y_offset); + for (unsigned int i = 0; i < 2u; ++i) { + unsigned int mxd = x_offset + i; + if (mxd >= costmap->getSizeInCellsX()) { + continue; + } + for (unsigned int j = 0; j < 2u; ++j) { + unsigned int myd = y_offset + j; + if (myd >= costmap->getSizeInCellsY()) { + continue; + } + if (i == 0 && j == 0) { + continue; + } + cost = std::min(cost, static_cast(costmap->getCost(mxd, myd))); + } + } + } else { + cost = static_cast(costmap->getCost(new_idx)); + } if (!is_circular) { // Adjust cost value if using SE2 footprint checks diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 1c127c47990..f6d917c08f1 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -50,7 +50,7 @@ TEST(NodeHybridTest, test_node_hybrid) // Check defaulted constants nav2_smac_planner::NodeHybrid testA(49); - EXPECT_EQ(testA.travel_distance_cost, sqrt(2)); + EXPECT_EQ(testA.travel_distance_cost, sqrtf(2)); nav2_smac_planner::NodeHybrid::initMotionModel( nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info); From cc2b696e5b512ca3b1e0e175bdd1cbccb265ff91 Mon Sep 17 00:00:00 2001 From: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Date: Fri, 12 Apr 2024 18:47:04 +0200 Subject: [PATCH 05/35] Set start and goal as float (#4255) * Set start and goal as float Signed-off-by: Brice * fix worldToMapContinuous type Signed-off-by: Brice * add static_cast Signed-off-by: Brice * fix linting Signed-off-by: Brice * floor float to check start = goal Signed-off-by: Brice --------- Signed-off-by: Brice --- .../include/nav2_costmap_2d/costmap_2d.hpp | 10 ++++ nav2_costmap_2d/src/costmap_2d.cpp | 15 ++++++ .../include/nav2_smac_planner/a_star.hpp | 8 +-- nav2_smac_planner/src/a_star.cpp | 51 +++++++++++-------- nav2_smac_planner/src/smac_planner_2d.cpp | 18 +++++-- nav2_smac_planner/src/smac_planner_hybrid.cpp | 6 +-- .../src/smac_planner_lattice.cpp | 6 +-- 7 files changed, 79 insertions(+), 35 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index 04b9f4daa43..005dd205d80 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -183,6 +183,16 @@ class Costmap2D */ bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const; + /** + * @brief Convert from world coordinates to map coordinates + * @param wx The x world coordinate + * @param wy The y world coordinate + * @param mx Will be set to the associated map x coordinate + * @param my Will be set to the associated map y coordinate + * @return True if the conversion was successful (legal bounds) false otherwise + */ + bool worldToMapContinuous(double wx, double wy, float & mx, float & my) const; + /** * @brief Convert from world coordinates to map coordinates without checking for legal bounds * @param wx The x world coordinate diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 8914166a5e1..9cb7ce19816 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -296,6 +296,21 @@ bool Costmap2D::worldToMap(double wx, double wy, unsigned int & mx, unsigned int return false; } +bool Costmap2D::worldToMapContinuous(double wx, double wy, float & mx, float & my) const +{ + if (wx < origin_x_ || wy < origin_y_) { + return false; + } + + mx = static_cast((wx - origin_x_) / resolution_) + 0.5f; + my = static_cast((wy - origin_y_) / resolution_) + 0.5f; + + if (mx < size_x_ && my < size_y_) { + return true; + } + return false; +} + void Costmap2D::worldToMapNoBounds(double wx, double wy, int & mx, int & my) const { mx = static_cast((wx - origin_x_) / resolution_); diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 668aff5e4d8..b59fa9d66b6 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -129,8 +129,8 @@ class AStarAlgorithm * @param dim_3 The node dim_3 index of the goal */ void setGoal( - const unsigned int & mx, - const unsigned int & my, + const float & mx, + const float & my, const unsigned int & dim_3); /** @@ -140,8 +140,8 @@ class AStarAlgorithm * @param dim_3 The node dim_3 index of the goal */ void setStart( - const unsigned int & mx, - const unsigned int & my, + const float & mx, + const float & my, const unsigned int & dim_3); /** diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 0470bfd3029..92a89230e92 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -132,28 +132,32 @@ typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( template<> void AStarAlgorithm::setStart( - const unsigned int & mx, - const unsigned int & my, + const float & mx, + const float & my, const unsigned int & dim_3) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero starting dim 3."); } - _start = addToGraph(Node2D::getIndex(mx, my, getSizeX())); + _start = addToGraph( + Node2D::getIndex( + static_cast(mx), + static_cast(my), + getSizeX())); } template void AStarAlgorithm::setStart( - const unsigned int & mx, - const unsigned int & my, + const float & mx, + const float & my, const unsigned int & dim_3) { - _start = addToGraph(NodeT::getIndex(mx, my, dim_3)); - _start->setPose( - Coordinates( - static_cast(mx), - static_cast(my), - static_cast(dim_3))); + _start = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + _start->setPose(Coordinates(mx, my, dim_3)); } template<> @@ -182,30 +186,35 @@ void AStarAlgorithm::populateExpansionsLog( template<> void AStarAlgorithm::setGoal( - const unsigned int & mx, - const unsigned int & my, + const float & mx, + const float & my, const unsigned int & dim_3) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } - _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX())); + _goal = addToGraph( + Node2D::getIndex( + static_cast(mx), + static_cast(my), + getSizeX())); _goal_coordinates = Node2D::Coordinates(mx, my); } template void AStarAlgorithm::setGoal( - const unsigned int & mx, - const unsigned int & my, + const float & mx, + const float & my, const unsigned int & dim_3) { - _goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); + _goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + dim_3)); - typename NodeT::Coordinates goal_coords( - static_cast(mx), - static_cast(my), - static_cast(dim_3)); + typename NodeT::Coordinates goal_coords(mx, my, dim_3); if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { if (!_start) { diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 38fd12b9e17..00cc07b464b 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -215,8 +215,13 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( _a_star->setCollisionChecker(&_collision_checker); // Set starting point - unsigned int mx_start, my_start, mx_goal, my_goal; - if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) { + float mx_start, my_start, mx_goal, my_goal; + if (!costmap->worldToMapContinuous( + start.pose.position.x, + start.pose.position.y, + mx_start, + my_start)) + { throw nav2_core::StartOutsideMapBounds( "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + std::to_string(start.pose.position.y) + ") was outside bounds"); @@ -224,7 +229,12 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( _a_star->setStart(mx_start, my_start, 0); // Set goal point - if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) { + if (!costmap->worldToMapContinuous( + goal.pose.position.x, + goal.pose.position.y, + mx_goal, + my_goal)) + { throw nav2_core::GoalOutsideMapBounds( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds"); @@ -244,7 +254,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( pose.pose.orientation.w = 1.0; // Corner case of start and goal beeing on the same cell - if (mx_start == mx_goal && my_start == my_goal) { + if (std::floor(mx_start) == std::floor(mx_goal) && std::floor(my_start) == std::floor(my_goal)) { pose.pose = start.pose; // if we have a different start and goal orientation, set the unique path pose to the goal // orientation, unless use_final_approach_orientation=true where we need it to be the start diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index a41e794b597..0e0c8829222 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -355,8 +355,8 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates - unsigned int mx, my; - if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { + float mx, my; + if (!costmap->worldToMapContinuous(start.pose.position.x, start.pose.position.y, mx, 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"); @@ -374,7 +374,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( _a_star->setStart(mx, my, orientation_bin_id); // Set goal point, in A* bin search coordinates - if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { + if (!costmap->worldToMapContinuous(goal.pose.position.x, goal.pose.position.y, mx, my)) { throw nav2_core::GoalOutsideMapBounds( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds"); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 87f2e365639..4f2dba45e43 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -281,8 +281,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( _a_star->setCollisionChecker(&_collision_checker); // Set starting point, in A* bin search coordinates - unsigned int mx, my; - if (!_costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) { + float mx, my; + if (!_costmap->worldToMapContinuous(start.pose.position.x, start.pose.position.y, mx, 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"); @@ -292,7 +292,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation))); // Set goal point, in A* bin search coordinates - if (!_costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) { + if (!_costmap->worldToMapContinuous(goal.pose.position.x, goal.pose.position.y, mx, my)) { throw nav2_core::GoalOutsideMapBounds( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + std::to_string(goal.pose.position.y) + ") was outside bounds"); From 12c786f8b8c533b0c4adc1a38c08021c90bfcba3 Mon Sep 17 00:00:00 2001 From: Sokolov Denis <52282102+perchess@users.noreply.github.com> Date: Sat, 13 Apr 2024 02:04:15 +0500 Subject: [PATCH 06/35] Implement Critic for Velocity Deadband Hardware Constraints (#4256) * Adding new velocity deadband critic. - add some tests - cast double to float - add new features from "main" branch - fix formating - add cost test - fix linting issue - add README Signed-off-by: Denis Sokolov * Remove velocity deadband critic from defaults Signed-off-by: Denis Sokolov * remove old weight Signed-off-by: Denis Sokolov * fix velocity deadband critic tests Signed-off-by: Denis Sokolov --------- Signed-off-by: Denis Sokolov --- nav2_mppi_controller/CMakeLists.txt | 1 + nav2_mppi_controller/README.md | 14 +++ nav2_mppi_controller/critics.xml | 4 + .../critics/velocity_deadband_critic.hpp | 54 +++++++++ .../src/critics/velocity_deadband_critic.cpp | 104 ++++++++++++++++++ nav2_mppi_controller/test/critics_tests.cpp | 50 +++++++++ 6 files changed, 227 insertions(+) create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp create mode 100644 nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 02590490199..95eabf6c411 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -88,6 +88,7 @@ add_library(mppi_critics SHARED src/critics/prefer_forward_critic.cpp src/critics/twirling_critic.cpp src/critics/constraint_critic.cpp + src/critics/velocity_deadband_critic.cpp ) set(libraries mppi_controller mppi_critics) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 893ed426a56..d061958135c 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -171,6 +171,15 @@ Uses inflated costmap cost directly to avoid obstacles | cost_weight | double | Default 10.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | + +#### Velocity Deadband Critic + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | cost_weight | double | Default 35.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | + | deadband_velocities | double[] | Default [0.0, 0.0, 0.0]. The array of deadband velocities [vx, vz, wz]. A zero array indicates that the critic will take no action. | + + ### XML configuration example ``` controller_server: @@ -262,6 +271,11 @@ controller_server: threshold_to_consider: 0.5 max_angle_to_furthest: 1.0 forward_preference: true + # VelocityDeadbandCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 35.0 + # deadband_velocities: [0.05, 0.05, 0.05] # TwirlingCritic: # enabled: true # twirling_cost_power: 1 diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index 2e7d110338b..deae2c11cb9 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -41,5 +41,9 @@ mppi critic for incentivizing moving within kinematic and dynamic bounds + + mppi critic for restricting command velocities in deadband range + + diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp new file mode 100644 index 00000000000..76e1dbd670f --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp @@ -0,0 +1,54 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ + +#include + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::VelocityDeadbandCritic + * @brief Critic objective function for enforcing feasible constraints + */ +class VelocityDeadbandCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to goal following + * + * @param costs [out] add reference cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + unsigned int power_{0}; + float weight_{0}; + std::vector deadband_velocities_{0.0f, 0.0f, 0.0f}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ diff --git a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp new file mode 100644 index 00000000000..84d3aba303b --- /dev/null +++ b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp @@ -0,0 +1,104 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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_mppi_controller/critics/velocity_deadband_critic.hpp" + +namespace mppi::critics +{ + +void VelocityDeadbandCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 35.0); + + // Recast double to float + std::vector deadband_velocities{0.0, 0.0, 0.0}; + getParam(deadband_velocities, "deadband_velocities", std::vector{0.0, 0.0, 0.0}); + std::transform( + deadband_velocities.begin(), deadband_velocities.end(), deadband_velocities_.begin(), + [](double d) {return static_cast(d);}); + + RCLCPP_INFO_STREAM( + logger_, "VelocityDeadbandCritic instantiated with " + << power_ << " power, " << weight_ << " weight, deadband_velocity [" + << deadband_velocities_.at(0) << "," << deadband_velocities_.at(1) << "," + << deadband_velocities_.at(2) << "]"); +} + +void VelocityDeadbandCritic::score(CriticData & data) +{ + using xt::evaluation_strategy::immediate; + + if (!enabled_) { + return; + } + + auto & vx = data.state.vx; + auto & wz = data.state.wz; + + if (data.motion_model->isHolonomic()) { + auto & vy = data.state.vy; + if (power_ > 1u) { + data.costs += xt::pow( + xt::sum( + std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) * + data.model_dt, + {1}, immediate) * + weight_, + power_); + } else { + data.costs += xt::sum( + (std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) * + data.model_dt, + {1}, immediate) * + weight_; + } + return; + } + + if (power_ > 1u) { + data.costs += xt::pow( + xt::sum( + std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) * + data.model_dt, + {1}, immediate) * + weight_, + power_); + } else { + data.costs += xt::sum( + (std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) * + data.model_dt, + {1}, immediate) * + weight_; + } + return; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS(mppi::critics::VelocityDeadbandCritic, mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index edc1c8dfb27..ef85ab293de 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -30,6 +30,7 @@ #include "nav2_mppi_controller/critics/path_follow_critic.hpp" #include "nav2_mppi_controller/critics/prefer_forward_critic.hpp" #include "nav2_mppi_controller/critics/twirling_critic.hpp" +#include "nav2_mppi_controller/critics/velocity_deadband_critic.hpp" #include "utils_test.cpp" // NOLINT // Tests the various critic plugin functions @@ -607,3 +608,52 @@ TEST(CriticTests, PathAlignCritic) critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); } + +TEST(CriticTests, VelocityDeadbandCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + ParametersHandler param_handler(node); + auto getParam = param_handler.getParamGetter("critic"); + std::vector deadband_velocities_; + getParam(deadband_velocities_, "deadband_velocities", std::vector{0.08, 0.08, 0.08}); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + models::Path path; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + + // Initialization testing + + // Make sure initializes correctly and that defaults are reasonable + VelocityDeadbandCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide velocities out of deadband bounds, should not have any costs + state.vx = 0.80 * xt::ones({1000, 30}); + state.vy = 0.60 * xt::ones({1000, 30}); + state.wz = 0.80 * xt::ones({1000, 30}); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); + + // Test cost value + state.vx = 0.01 * xt::ones({1000, 30}); + state.vy = 0.02 * xt::ones({1000, 30}); + state.wz = 0.021 * xt::ones({1000, 30}); + critic.score(data); + // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7 + EXPECT_NEAR(costs(1), 19.845, 0.01); +} From 677e6accb3aea9380678da7d623b3387578d3549 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 22 Apr 2024 18:02:42 -0700 Subject: [PATCH 07/35] Completing #4259 (#4273) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * changed slam launch file according to the comments for the PR #4211 Signed-off-by: Ibrahim Özcan * Fixing litning problems --------- Signed-off-by: Ibrahim Özcan Co-authored-by: Ibrahim Özcan --- nav2_bringup/launch/slam_launch.py | 37 ++++++++++++++++++------------ 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index a806d260ce5..f9294b711d2 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -21,7 +21,7 @@ from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node, SetParameter +from launch_ros.actions import Node, SetParameter, SetRemap from launch_ros.descriptions import ParameterFile from nav2_common.launch import HasNodeParams, RewrittenYaml @@ -88,7 +88,6 @@ def generate_launch_description(): ) # Nodes launching commands - start_map_server = GroupAction( actions=[ SetParameter('use_sim_time', use_sim_time), @@ -119,19 +118,28 @@ def generate_launch_description(): source_file=params_file, node_name='slam_toolbox' ) - start_slam_toolbox_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(slam_launch_file), - launch_arguments={'use_sim_time': use_sim_time}.items(), - condition=UnlessCondition(has_slam_toolbox_params), - ) + start_slam_toolbox_cmd = GroupAction( + + actions=[ + # Remapping required to have a slam session subscribe & publish in optional namespaces + SetRemap(src='/scan', dst='scan'), + SetRemap(src='/tf', dst='tf'), + SetRemap(src='/tf_static', dst='tf_static'), + SetRemap(src='/map', dst='map'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={'use_sim_time': use_sim_time}.items(), + condition=UnlessCondition(has_slam_toolbox_params), + ), - start_slam_toolbox_cmd_with_params = IncludeLaunchDescription( - PythonLaunchDescriptionSource(slam_launch_file), - launch_arguments={ - 'use_sim_time': use_sim_time, - 'slam_params_file': params_file, - }.items(), - condition=IfCondition(has_slam_toolbox_params), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={'use_sim_time': use_sim_time, + 'slam_params_file': params_file}.items(), + condition=IfCondition(has_slam_toolbox_params), + ) + ] ) ld = LaunchDescription() @@ -149,6 +157,5 @@ def generate_launch_description(): # Running SLAM Toolbox (Only one of them will be run) ld.add_action(start_slam_toolbox_cmd) - ld.add_action(start_slam_toolbox_cmd_with_params) return ld From 13e20057f7e515713d4424267f88d5ce29bdc4d7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 23 Apr 2024 14:49:52 -0700 Subject: [PATCH 08/35] Revert "Revert "adding base footprint publisher to nav2_util (#3860)" (#3994)" (#3995) This reverts commit ef85df2f0419db5f2aba68c685c16d034a2aa6b6. --- .../nav2_util/base_footprint_publisher.hpp | 129 ++++++++++++++++++ nav2_util/src/CMakeLists.txt | 5 + nav2_util/src/base_footprint_publisher.cpp | 27 ++++ nav2_util/test/CMakeLists.txt | 4 + .../test/test_base_footprint_publisher.cpp | 72 ++++++++++ 5 files changed, 237 insertions(+) create mode 100644 nav2_util/include/nav2_util/base_footprint_publisher.hpp create mode 100644 nav2_util/src/base_footprint_publisher.cpp create mode 100644 nav2_util/test/test_base_footprint_publisher.cpp diff --git a/nav2_util/include/nav2_util/base_footprint_publisher.hpp b/nav2_util/include/nav2_util/base_footprint_publisher.hpp new file mode 100644 index 00000000000..52bcdb53eb0 --- /dev/null +++ b/nav2_util/include/nav2_util/base_footprint_publisher.hpp @@ -0,0 +1,129 @@ +// Copyright (c) 2023 Open Navigation LLC +// +// 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_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ +#define NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" + +namespace nav2_util +{ + +/** + * @brief A TF2 listener that overrides the subscription callback + * to inject base footprint publisher removing Z, Pitch, and Roll for + * 3D state estimation but desiring a 2D frame for navigation, visualization, or other reasons + */ +class BaseFootprintPublisherListener : public tf2_ros::TransformListener +{ +public: + BaseFootprintPublisherListener(tf2::BufferCore & buffer, bool spin_thread, rclcpp::Node & node) + : tf2_ros::TransformListener(buffer, spin_thread) + { + node.declare_parameter( + "base_link_frame", rclcpp::ParameterValue(std::string("base_link"))); + node.declare_parameter( + "base_footprint_frame", rclcpp::ParameterValue(std::string("base_footprint"))); + base_link_frame_ = node.get_parameter("base_link_frame").as_string(); + base_footprint_frame_ = node.get_parameter("base_footprint_frame").as_string(); + tf_broadcaster_ = std::make_shared(node); + } + + /** + * @brief Overrides TF2 subscription callback to inject base footprint publisher + */ + void subscription_callback(tf2_msgs::msg::TFMessage::ConstSharedPtr msg, bool is_static) override + { + TransformListener::subscription_callback(msg, is_static); + + if (is_static) { + return; + } + + for (unsigned int i = 0; i != msg->transforms.size(); i++) { + auto & t = msg->transforms[i]; + if (t.child_frame_id == base_link_frame_) { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = t.header.stamp; + transform.header.frame_id = base_link_frame_; + transform.child_frame_id = base_footprint_frame_; + + // Project to Z-zero + transform.transform.translation = t.transform.translation; + transform.transform.translation.z = 0.0; + + // Remove Roll and Pitch + tf2::Quaternion q; + q.setRPY(0, 0, tf2::getYaw(t.transform.rotation)); + q.normalize(); + transform.transform.rotation.x = q.x(); + transform.transform.rotation.y = q.y(); + transform.transform.rotation.z = q.z(); + transform.transform.rotation.w = q.w(); + + tf_broadcaster_->sendTransform(transform); + return; + } + } + } + +protected: + std::shared_ptr tf_broadcaster_; + std::string base_link_frame_, base_footprint_frame_; +}; + +/** + * @class nav2_util::BaseFootprintPublisher + * @brief Republishes the ``base_link`` frame as ``base_footprint`` + * stripping away the Z, Roll, and Pitch of the full 3D state to provide + * a 2D projection for navigation when state estimation is full 3D + */ +class BaseFootprintPublisher : public rclcpp::Node +{ +public: + /** + * @brief A constructor + */ + explicit BaseFootprintPublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("base_footprint_publisher", options) + { + RCLCPP_INFO(get_logger(), "Creating base footprint publisher"); + tf_buffer_ = std::make_shared(get_clock()); + auto timer_interface = std::make_shared( + get_node_base_interface(), + get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + listener_publisher_ = std::make_shared( + *tf_buffer_, true, *this); + } + +protected: + std::shared_ptr tf_buffer_; + std::shared_ptr listener_publisher_; +}; + +} // end namespace nav2_util + +#endif // NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 104966e2198..6ddcdc6d32f 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -29,6 +29,11 @@ add_executable(lifecycle_bringup ) target_link_libraries(lifecycle_bringup ${library_name}) +add_executable(base_footprint_publisher + base_footprint_publisher.cpp +) +target_link_libraries(base_footprint_publisher ${library_name}) + find_package(Boost REQUIRED COMPONENTS program_options) install(TARGETS diff --git a/nav2_util/src/base_footprint_publisher.cpp b/nav2_util/src/base_footprint_publisher.cpp new file mode 100644 index 00000000000..f3b6791db44 --- /dev/null +++ b/nav2_util/src/base_footprint_publisher.cpp @@ -0,0 +1,27 @@ +// Copyright (c) 2023 Open Navigation LLC +// +// 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_util/base_footprint_publisher.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index fb4b6c107f2..14e1a361d3b 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -42,6 +42,10 @@ ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) +ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp) +ament_target_dependencies(test_base_footprint_publisher geometry_msgs) +target_link_libraries(test_base_footprint_publisher ${library_name}) + ament_add_gtest(test_array_parser test_array_parser.cpp) target_link_libraries(test_array_parser ${library_name}) diff --git a/nav2_util/test/test_base_footprint_publisher.cpp b/nav2_util/test/test_base_footprint_publisher.cpp new file mode 100644 index 00000000000..47dc83c7f31 --- /dev/null +++ b/nav2_util/test/test_base_footprint_publisher.cpp @@ -0,0 +1,72 @@ +// Copyright (c) 2023 Open Navigation LLC +// +// 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_util/base_footprint_publisher.hpp" +#include "gtest/gtest.h" +#include "tf2/exceptions.h" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(TestBaseFootprintPublisher, TestBaseFootprintPublisher) +{ + auto node = std::make_shared(); + rclcpp::spin_some(node->get_node_base_interface()); + + auto tf_broadcaster = std::make_shared(node); + auto buffer = std::make_shared(node->get_clock()); + auto timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); + buffer->setCreateTimerInterface(timer_interface); + auto listener = std::make_shared(*buffer, true); + + std::string base_link = "base_link"; + std::string base_footprint = "base_footprint"; + + // Publish something to TF, should fail, doesn't exist + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node->now(); + transform.header.frame_id = "test1_1"; + transform.child_frame_id = "test1"; + tf_broadcaster->sendTransform(transform); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_THROW( + buffer->lookupTransform(base_link, base_footprint, tf2::TimePointZero), + tf2::TransformException); + + // This is valid, should work now and communicate the Z-removed info + transform.header.stamp = node->now(); + transform.header.frame_id = "odom"; + transform.child_frame_id = "base_link"; + transform.transform.translation.x = 1.0; + transform.transform.translation.y = 1.0; + transform.transform.translation.z = 1.0; + tf_broadcaster->sendTransform(transform); + rclcpp::Rate r(1.0); + r.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + auto t = buffer->lookupTransform(base_link, base_footprint, tf2::TimePointZero); + EXPECT_EQ(t.transform.translation.x, 1.0); + EXPECT_EQ(t.transform.translation.y, 1.0); + EXPECT_EQ(t.transform.translation.z, 0.0); +} From f8563843d350fcb2238a3dfbd2d7cc3f05c3624a Mon Sep 17 00:00:00 2001 From: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Date: Wed, 24 Apr 2024 18:49:18 +0200 Subject: [PATCH 09/35] [Collision monitor] Dynamic radius for circle polygon (#4226) * Collision monitor: add a radius topic sub for dynamic circle polygon Signed-off-by: asarazin * add test on circle radius update Signed-off-by: asarazin --------- Signed-off-by: asarazin Co-authored-by: asarazin --- nav2_collision_monitor/CMakeLists.txt | 2 + .../include/nav2_collision_monitor/circle.hpp | 35 ++++- .../nav2_collision_monitor/polygon.hpp | 25 +++- nav2_collision_monitor/package.xml | 1 + nav2_collision_monitor/src/circle.cpp | 95 +++++++++++-- nav2_collision_monitor/src/polygon.cpp | 130 ++++++++++-------- .../src/velocity_polygon.cpp | 7 +- nav2_collision_monitor/test/polygons_test.cpp | 74 ++++++++-- 8 files changed, 275 insertions(+), 94 deletions(-) diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 8cef0f609fc..d812df7d654 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -32,6 +33,7 @@ set(dependencies rclcpp_components sensor_msgs geometry_msgs + std_msgs tf2 tf2_ros tf2_geometry_msgs diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index c4400327013..a468be7c880 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -19,6 +19,8 @@ #include #include +#include "std_msgs/msg/float32.hpp" + #include "nav2_collision_monitor/polygon.hpp" namespace nav2_collision_monitor @@ -67,15 +69,16 @@ class Circle : public Polygon int getPointsInside(const std::vector & points) const override; /** - * @brief Specifies that the shape is always set for a circle object + * @brief Returns true if circle radius is set. + * Otherwise, prints a warning and returns false. */ - bool isShapeSet() override {return true;} + bool isShapeSet() override; protected: /** * @brief Supporting routine obtaining polygon-specific ROS-parameters - * @brief polygon_sub_topic Input name of polygon subscription topic - * @param polygon_pub_topic Output name of polygon publishing topic + * @param polygon_sub_topic Input name of polygon subscription topic + * @param polygon_pub_topic Output name of polygon or radius publishing topic * @param footprint_topic Output name of footprint topic. For Circle returns empty string, * there is no footprint subscription in this class. * @return True if all parameters were obtained or false in failure case @@ -85,12 +88,34 @@ class Circle : public Polygon std::string & polygon_pub_topic, std::string & footprint_topic) override; + /** + * @brief Creates polygon or radius topic subscription + * @param polygon_sub_topic Output name of polygon or radius subscription topic. + * Empty, if no polygon subscription. + */ + void createSubscription(std::string & polygon_sub_topic) override; + + /** + * @brief Updates polygon from radius value + * @param radius New circle radius to update polygon + */ + void updatePolygon(double radius); + + /** + * @brief Dynamic circle radius callback + * @param msg Shared pointer to the radius value message + */ + void radiusCallback(std_msgs::msg::Float32::ConstSharedPtr msg); + + // ----- Variables ----- /// @brief Radius of the circle double radius_; /// @brief (radius * radius) value. Stored for optimization. - double radius_squared_; + double radius_squared_ = -1.0; + /// @brief Radius subscription + rclcpp::Subscription::SharedPtr radius_sub_; }; // class Circle } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 1e6a72d1590..fdc9e07b7a3 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -128,7 +128,7 @@ class Polygon /** * @brief Returns true if polygon points were set. - * Othewise, prints a warning and returns false. + * Otherwise, prints a warning and returns false. */ virtual bool isShapeSet(); @@ -165,14 +165,24 @@ class Polygon protected: /** * @brief Supporting routine obtaining ROS-parameters common for all shapes - * @param polygon_pub_topic Output name of polygon publishing topic + * @param polygon_pub_topic Output name of polygon or radius subscription topic. + * Empty, if no polygon subscription. + * @param polygon_sub_topic Output name of polygon publishing topic + * @param footprint_topic Output name of footprint topic. + * Empty, if no footprint subscription. + * @param use_dynamic_sub If false, the parameter polygon_sub_topic or footprint_topic + * will not be declared * @return True if all parameters were obtained or false in failure case */ - bool getCommonParameters(std::string & polygon_pub_topic); + bool getCommonParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic, + bool use_dynamic_sub = false); /** * @brief Supporting routine obtaining polygon-specific ROS-parameters - * @brief polygon_sub_topic Output name of polygon subscription topic. + * @param polygon_sub_topic Output name of polygon or radius subscription topic. * Empty, if no polygon subscription. * @param polygon_pub_topic Output name of polygon publishing topic * @param footprint_topic Output name of footprint topic. @@ -184,6 +194,13 @@ class Polygon std::string & polygon_pub_topic, std::string & footprint_topic); + /** + * @brief Creates polygon or radius topic subscription + * @param polygon_sub_topic Output name of polygon or radius subscription topic. + * Empty, if no polygon subscription. + */ + virtual void createSubscription(std::string & polygon_sub_topic); + /** * @brief Updates polygon from geometry_msgs::msg::PolygonStamped message * @param msg Message to update polygon from diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 4889ea77f16..fd663526e86 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -17,6 +17,7 @@ tf2_geometry_msgs sensor_msgs geometry_msgs + std_msgs nav2_common nav2_util nav2_costmap_2d diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index fa0fc33a4f8..353db72dc07 100644 --- a/nav2_collision_monitor/src/circle.cpp +++ b/nav2_collision_monitor/src/circle.cpp @@ -70,6 +70,15 @@ int Circle::getPointsInside(const std::vector & points) const return num; } +bool Circle::isShapeSet() +{ + if (radius_squared_ == -1.0) { + RCLCPP_WARN(logger_, "[%s]: Circle radius is not set yet", polygon_name_.c_str()); + return false; + } + return true; +} + bool Circle::getParameters( std::string & polygon_sub_topic, std::string & polygon_pub_topic, @@ -80,29 +89,91 @@ bool Circle::getParameters( throw std::runtime_error{"Failed to lock node"}; } - if (!getCommonParameters(polygon_pub_topic)) { - return false; - } - - // There is no polygon or footprint subscription for the Circle. Thus, set strings as empty. + // Clear the polygon subscription topic. It will be set later, if necessary. polygon_sub_topic.clear(); - footprint_topic.clear(); + bool use_dynamic_sub = true; // if getting parameter radius fails, use dynamic subscription try { // Leave it not initialized: the will cause an error if it will not set nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".radius", rclcpp::PARAMETER_DOUBLE); radius_ = node->get_parameter(polygon_name_ + ".radius").as_double(); radius_squared_ = radius_ * radius_; - } catch (const std::exception & ex) { - RCLCPP_ERROR( + use_dynamic_sub = false; + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { + RCLCPP_INFO( logger_, - "[%s]: Error while getting circle parameters: %s", - polygon_name_.c_str(), ex.what()); - return false; + "[%s]: Polygon circle radius is not defined. Using dynamic subscription instead.", + polygon_name_.c_str()); } - return true; + bool ret = true; + if (!getCommonParameters( + polygon_sub_topic, polygon_pub_topic, footprint_topic, use_dynamic_sub)) + { + if (use_dynamic_sub && polygon_sub_topic.empty()) { + RCLCPP_ERROR( + logger_, + "[%s]: Error while getting circle parameters: static radius and sub topic both not defined", + polygon_name_.c_str()); + } + ret = false; + } + + // There is no footprint subscription for the Circle. Thus, set string as empty. + footprint_topic.clear(); + + return ret; +} + +void Circle::createSubscription(std::string & polygon_sub_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!polygon_sub_topic.empty()) { + RCLCPP_INFO( + logger_, + "[%s]: Subscribing on %s topic for polygon", + polygon_name_.c_str(), polygon_sub_topic.c_str()); + rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default + if (polygon_subscribe_transient_local_) { + polygon_qos.transient_local(); + } + radius_sub_ = node->create_subscription( + polygon_sub_topic, polygon_qos, + std::bind(&Circle::radiusCallback, this, std::placeholders::_1)); + } +} + +void Circle::updatePolygon(double radius) +{ + // Update circle radius + radius_ = radius; + radius_squared_ = radius_ * radius_; + + // Create a polygon from radius and store it + std::vector poly; + getPolygon(poly); + polygon_.polygon.points.clear(); // clear polygon points + for (const Point & p : poly) { + geometry_msgs::msg::Point32 p_s; + p_s.x = p.x; + p_s.y = p.y; + // p_s.z will remain 0.0 + polygon_.polygon.points.push_back(p_s); // add new points + } +} + +void Circle::radiusCallback(std_msgs::msg::Float32::ConstSharedPtr msg) +{ + RCLCPP_INFO( + logger_, + "[%s]: Polygon circle radius update has been arrived", + polygon_name_.c_str()); + updatePolygon(msg->data); } } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 6a0801d1d26..7e0a69b84f4 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -65,19 +65,7 @@ bool Polygon::configure() return false; } - if (!polygon_sub_topic.empty()) { - RCLCPP_INFO( - logger_, - "[%s]: Subscribing on %s topic for polygon", - polygon_name_.c_str(), polygon_sub_topic.c_str()); - rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default - if (polygon_subscribe_transient_local_) { - polygon_qos.transient_local(); - } - polygon_sub_ = node->create_subscription( - polygon_sub_topic, polygon_qos, - std::bind(&Polygon::polygonCallback, this, std::placeholders::_1)); - } + createSubscription(polygon_sub_topic); if (!footprint_topic.empty()) { RCLCPP_INFO( @@ -287,7 +275,11 @@ void Polygon::publish() polygon_pub_->publish(std::move(msg)); } -bool Polygon::getCommonParameters(std::string & polygon_pub_topic) +bool Polygon::getCommonParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic, + bool use_dynamic_sub_topic) { auto node = node_.lock(); if (!node) { @@ -372,6 +364,28 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) node, polygon_name_ + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name_)); polygon_pub_topic = node->get_parameter(polygon_name_ + ".polygon_pub_topic").as_string(); } + + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".polygon_subscribe_transient_local", rclcpp::ParameterValue(false)); + polygon_subscribe_transient_local_ = + node->get_parameter(polygon_name_ + ".polygon_subscribe_transient_local").as_bool(); + + if (use_dynamic_sub_topic) { + if (action_type_ != APPROACH) { + // Get polygon sub topic + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING); + polygon_sub_topic = + node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string(); + } else { + // Obtain the footprint topic to make a footprint subscription for approach polygon + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".footprint_topic", + rclcpp::ParameterValue("local_costmap/published_footprint")); + footprint_topic = + node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); + } + } } catch (const std::exception & ex) { RCLCPP_ERROR( logger_, @@ -393,63 +407,64 @@ bool Polygon::getParameters( throw std::runtime_error{"Failed to lock node"}; } - if (!getCommonParameters(polygon_pub_topic)) { - return false; - } - // Clear the subscription topics. They will be set later, if necessary. polygon_sub_topic.clear(); footprint_topic.clear(); + bool use_dynamic_sub = true; // if getting parameter points fails, use dynamic subscription try { - try { - // Leave it uninitialized: it will throw an inner exception if the parameter is not set - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".points", rclcpp::PARAMETER_STRING); - std::string poly_string = - node->get_parameter(polygon_name_ + ".points").as_string(); - - // Do not need to proceed further, if "points" parameter is defined. - // Static polygon will be used. - return getPolygonFromString(poly_string, poly_); - } catch (const rclcpp::exceptions::ParameterUninitializedException &) { - RCLCPP_INFO( + // Leave it uninitialized: it will throw an inner exception if the parameter is not set + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".points", rclcpp::PARAMETER_STRING); + std::string poly_string = + node->get_parameter(polygon_name_ + ".points").as_string(); + + use_dynamic_sub = !getPolygonFromString(poly_string, poly_); + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { + RCLCPP_INFO( + logger_, + "[%s]: Polygon points are not defined. Using dynamic subscription instead.", + polygon_name_.c_str()); + } + + if (!getCommonParameters( + polygon_sub_topic, polygon_pub_topic, footprint_topic, use_dynamic_sub)) + { + if (use_dynamic_sub && polygon_sub_topic.empty() && footprint_topic.empty()) { + RCLCPP_ERROR( logger_, - "[%s]: Polygon points are not defined. Using dynamic subscription instead.", + "[%s]: Error while getting polygon parameters:" + " static points and sub topic both not defined", polygon_name_.c_str()); } - - if (action_type_ == STOP || action_type_ == SLOWDOWN || action_type_ == LIMIT || - action_type_ == DO_NOTHING) - { - // Dynamic polygon will be used - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING); - polygon_sub_topic = - node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string(); - } else if (action_type_ == APPROACH) { - // Obtain the footprint topic to make a footprint subscription for approach polygon - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".footprint_topic", - rclcpp::ParameterValue("local_costmap/published_footprint")); - footprint_topic = - node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); - } - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".polygon_subscribe_transient_local", rclcpp::ParameterValue(false)); - polygon_subscribe_transient_local_ = - node->get_parameter(polygon_name_ + ".polygon_subscribe_transient_local").as_bool(); - } catch (const std::exception & ex) { - RCLCPP_ERROR( - logger_, - "[%s]: Error while getting polygon parameters: %s", - polygon_name_.c_str(), ex.what()); return false; } return true; } +void Polygon::createSubscription(std::string & polygon_sub_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + if (!polygon_sub_topic.empty()) { + RCLCPP_INFO( + logger_, + "[%s]: Subscribing on %s topic for polygon", + polygon_name_.c_str(), polygon_sub_topic.c_str()); + rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default + if (polygon_subscribe_transient_local_) { + polygon_qos.transient_local(); + } + polygon_sub_ = node->create_subscription( + polygon_sub_topic, polygon_qos, + std::bind(&Polygon::polygonCallback, this, std::placeholders::_1)); + } +} + void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) { std::size_t new_size = msg->polygon.points.size(); @@ -582,6 +597,7 @@ bool Polygon::getPolygonFromString( "Points in the polygon specification must be pairs of numbers" "Found a point with %d numbers.", static_cast(vvf[i].size())); + polygon.clear(); return false; } } diff --git a/nav2_collision_monitor/src/velocity_polygon.cpp b/nav2_collision_monitor/src/velocity_polygon.cpp index 84779a1a2d4..602a018c2bb 100644 --- a/nav2_collision_monitor/src/velocity_polygon.cpp +++ b/nav2_collision_monitor/src/velocity_polygon.cpp @@ -34,8 +34,9 @@ VelocityPolygon::~VelocityPolygon() } bool VelocityPolygon::getParameters( - std::string & /*polygon_sub_topic*/, std::string & polygon_pub_topic, - std::string & /*footprint_topic*/) + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic) { auto node = node_.lock(); if (!node) { @@ -43,7 +44,7 @@ bool VelocityPolygon::getParameters( } clock_ = node->get_clock(); - if (!getCommonParameters(polygon_pub_topic)) { + if (!getCommonParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic, false)) { return false; } diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 849b6278686..c703e12557b 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -117,6 +117,17 @@ class TestNode : public nav2_util::LifecycleNode polygon_pub_->publish(std::move(msg)); } + void publishRadius() + { + radius_pub_ = this->create_publisher( + POLYGON_SUB_TOPIC, rclcpp::SystemDefaultsQoS()); + + std::unique_ptr msg = std::make_unique(); + msg->data = CIRCLE_RADIUS; + + radius_pub_->publish(std::move(msg)); + } + void publishFootprint() { footprint_pub_ = this->create_publisher( @@ -159,6 +170,7 @@ class TestNode : public nav2_util::LifecycleNode private: rclcpp::Publisher::SharedPtr polygon_pub_; + rclcpp::Publisher::SharedPtr radius_pub_; rclcpp::Publisher::SharedPtr footprint_pub_; rclcpp::Subscription::SharedPtr polygon_sub_; @@ -227,11 +239,11 @@ class Tester : public ::testing::Test void setPolygonParameters( const char * points, const bool is_static); - void setCircleParameters(const double radius); + void setCircleParameters(const double radius, const bool is_static); bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param); // Creating routines void createPolygon(const std::string & action_type, const bool is_static); - void createCircle(const std::string & action_type); + void createCircle(const std::string & action_type, const bool is_static); void sendTransforms(double shift); @@ -240,6 +252,9 @@ class Tester : public ::testing::Test const std::chrono::nanoseconds & timeout, std::vector & poly); + // Wait until circle polygon radius will be received + bool waitRadius(const std::chrono::nanoseconds & timeout); + // Wait until footprint will be received bool waitFootprint( const std::chrono::nanoseconds & timeout, @@ -344,12 +359,19 @@ void Tester::setPolygonParameters( } } -void Tester::setCircleParameters(const double radius) +void Tester::setCircleParameters(const double radius, const bool is_static) { - test_node_->declare_parameter( - std::string(CIRCLE_NAME) + ".radius", rclcpp::ParameterValue(radius)); - test_node_->set_parameter( - rclcpp::Parameter(std::string(CIRCLE_NAME) + ".radius", radius)); + if (is_static) { + test_node_->declare_parameter( + std::string(CIRCLE_NAME) + ".radius", rclcpp::ParameterValue(radius)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".radius", radius)); + } else { + test_node_->declare_parameter( + std::string(CIRCLE_NAME) + ".polygon_sub_topic", rclcpp::ParameterValue(POLYGON_SUB_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(CIRCLE_NAME) + ".polygon_sub_topic", POLYGON_SUB_TOPIC)); + } } bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const std::string & param) @@ -382,10 +404,10 @@ void Tester::createPolygon(const std::string & action_type, const bool is_static polygon_->activate(); } -void Tester::createCircle(const std::string & action_type) +void Tester::createCircle(const std::string & action_type, const bool is_static) { setCommonParameters(CIRCLE_NAME, action_type); - setCircleParameters(CIRCLE_RADIUS); + setCircleParameters(CIRCLE_RADIUS, is_static); circle_ = std::make_shared( test_node_, CIRCLE_NAME, @@ -433,6 +455,19 @@ bool Tester::waitPolygon( return false; } +bool Tester::waitRadius(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (circle_->isShapeSet()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + bool Tester::waitFootprint( const std::chrono::nanoseconds & timeout, std::vector & footprint) @@ -518,7 +553,7 @@ TEST_F(Tester, testPolygonGetApproachParameters) TEST_F(Tester, testCircleGetParameters) { - createCircle("approach"); + createCircle("approach", true); // Check that common parameters set correctly EXPECT_EQ(circle_->getName(), CIRCLE_NAME); @@ -628,8 +663,9 @@ TEST_F(Tester, testCircleUndeclaredRadius) tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_FALSE(circle_->configure()); - // Check that "radius" parameter is not set after configuring + // Check that "radius" and "polygon_sub_topic" parameters are not set after configuring ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "radius")); + ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "polygon_sub_topic")); } TEST_F(Tester, testPolygonTopicUpdate) @@ -646,7 +682,7 @@ TEST_F(Tester, testPolygonTopicUpdate) ASSERT_FALSE(waitPolygon(100ms, poly)); ASSERT_FALSE(polygon_->isShapeSet()); - // Publush correct polygon and make shure that it was set correctly + // Publish correct polygon and make sure that it was set correctly test_node_->publishPolygon(BASE_FRAME_ID, true); ASSERT_TRUE(waitPolygon(500ms, poly)); ASSERT_TRUE(polygon_->isShapeSet()); @@ -661,6 +697,18 @@ TEST_F(Tester, testPolygonTopicUpdate) EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7], EPSILON); } +TEST_F(Tester, testCircleTopicUpdate) +{ + createCircle("stop", false); + ASSERT_FALSE(circle_->isShapeSet()); + + // Publish radius and make sure that it was set correctly + test_node_->publishRadius(); + ASSERT_TRUE(waitRadius(500ms)); + EXPECT_NEAR(circle_->getRadius(), CIRCLE_RADIUS, EPSILON); + EXPECT_NEAR(circle_->getRadiusSquared(), CIRCLE_RADIUS * CIRCLE_RADIUS, EPSILON); +} + TEST_F(Tester, testPolygonTopicUpdateDifferentFrame) { createPolygon("stop", false); @@ -789,7 +837,7 @@ TEST_F(Tester, testPolygonGetPointsInsideEdge) TEST_F(Tester, testCircleGetPointsInside) { - createCircle("stop"); + createCircle("stop", true); std::vector points; // Point out of radius From fbec0fa68a23e8257e1420075745aba0735b07c4 Mon Sep 17 00:00:00 2001 From: Chuanhong Guo Date: Thu, 25 Apr 2024 01:05:53 +0800 Subject: [PATCH 10/35] Fix some warnings on GCC13/Ubuntu 24.04 (#4265) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * nav_2d_utils: use get_parameter without default value the original prototype construct a Parameter from the ¶meter references. Our value is unassigned, which caused a warning. fix ‘value’ may be used uninitialized warning on gcc13. Signed-off-by: Chuanhong Guo * nav2_behaviors: use get_parameter without default value the two-parameter version of get_parameter constructs a Parameter from the ¶meter references. Our value is unassigned, which caused a warning. fix 'may be used uninitialized' warning on gcc13. Signed-off-by: Chuanhong Guo * waypoint_follower: use get_parameter without default value the two-parameter version of get_parameter constructs a Parameter from the ¶meter references. Our value is unassigned, which caused a warning. fix 'may be used uninitialized' warning on gcc13. Signed-off-by: Chuanhong Guo * nav2_behavior_tree: use get_parameter without default value the two-parameter version of get_parameter constructs a Parameter from the ¶meter references. Our value is unassigned, which caused a warning. fix 'may be used uninitialized' warning on gcc13. Signed-off-by: Chuanhong Guo * constrained_smoother: initialize prelast_dir in upsampleAndPopulate It's technically not needed but GCC can't figure this out and complains. Signed-off-by: Chuanhong Guo * waypoint_follower: fix warns about null pointer dereferencing gcc13 warns that get_current_goal() may be nullptr and doesn't allow the assignment of it to poses: error: potential null pointer dereference [-Werror=null-dereference] Check the pointer before using it. Signed-off-by: Chuanhong Guo * Update nav2_waypoint_follower/src/waypoint_follower.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Chuanhong Guo Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski --- .../nav2_behavior_tree/bt_action_server_impl.hpp | 4 ++-- nav2_behaviors/src/behavior_server.cpp | 2 +- .../include/nav2_constrained_smoother/smoother.hpp | 2 +- .../include/nav_2d_utils/parameters.hpp | 4 +--- .../plugins/input_at_waypoint.cpp | 2 +- nav2_waypoint_follower/src/waypoint_follower.cpp | 13 +++++++++---- 6 files changed, 15 insertions(+), 12 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index deaeea1bdc2..75dfff55979 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -144,8 +144,8 @@ bool BtActionServer::on_configure() rclcpp::copy_all_parameter_values(node, client_node_); // set the timeout in seconds for the action server to discard goal handles if not finished - double action_server_result_timeout; - node->get_parameter("action_server_result_timeout", action_server_result_timeout); + double action_server_result_timeout = + node->get_parameter("action_server_result_timeout").as_double(); rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 706055161b4..1017bf595a3 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -147,8 +147,8 @@ void BehaviorServer::setupResourcesForBehaviorPlugins() get_parameter("global_costmap_topic", global_costmap_topic); get_parameter("local_footprint_topic", local_footprint_topic); get_parameter("global_footprint_topic", global_footprint_topic); - get_parameter("transform_tolerance", transform_tolerance); get_parameter("robot_base_frame", robot_base_frame); + transform_tolerance = get_parameter("transform_tolerance").as_double(); bool need_local_costmap = false; bool need_global_costmap = false; diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp index 526754e8edd..a0d5dfa115e 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother.hpp @@ -293,7 +293,7 @@ class Smoother } int last_i = 0; int prelast_i = -1; - Eigen::Vector2d prelast_dir; + Eigen::Vector2d prelast_dir = {0, 0}; for (int i = 1; i <= static_cast(path_optim.size()); i++) { if (i == static_cast(path_optim.size()) || optimized[i]) { if (prelast_i != -1) { diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp index c3c30428feb..29a17532937 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp @@ -64,12 +64,10 @@ param_t searchAndGetParam( const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & param_name, const param_t & default_value) { - param_t value; nav2_util::declare_parameter_if_not_declared( nh, param_name, rclcpp::ParameterValue(default_value)); - nh->get_parameter(param_name, value); - return value; + return nh->get_parameter(param_name).get_value(); } } // namespace nav_2d_utils diff --git a/nav2_waypoint_follower/plugins/input_at_waypoint.cpp b/nav2_waypoint_follower/plugins/input_at_waypoint.cpp index 5c613219ff6..b6b4f84b9a5 100644 --- a/nav2_waypoint_follower/plugins/input_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/input_at_waypoint.cpp @@ -60,7 +60,7 @@ void InputAtWaypoint::initialize( nav2_util::declare_parameter_if_not_declared( node, plugin_name + ".input_topic", rclcpp::ParameterValue("input_at_waypoint/input")); - node->get_parameter(plugin_name + ".timeout", timeout); + timeout = node->get_parameter(plugin_name + ".timeout").as_double(); node->get_parameter(plugin_name + ".enabled", is_enabled_); node->get_parameter(plugin_name + ".input_topic", input_topic); diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 2bd895598ba..8dfd9b1461e 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -78,8 +78,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) get_node_waitables_interface(), "navigate_to_pose", callback_group_); - double action_server_result_timeout; - get_parameter("action_server_result_timeout", action_server_result_timeout); + double action_server_result_timeout = get_parameter("action_server_result_timeout").as_double(); rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); @@ -187,15 +186,21 @@ std::vector WaypointFollower::getLatestGoalPose const T & action_server) { std::vector poses; + const auto current_goal = action_server->get_current_goal(); + + if (!current_goal) { + RCLCPP_ERROR(get_logger(), "No current action goal found!"); + return poses; + } // compile time static check to decide which block of code to be built if constexpr (std::is_same>::value) { // If normal waypoint following callback was called, we build here - poses = action_server->get_current_goal()->poses; + poses = current_goal->poses; } else { // If GPS waypoint following callback was called, we build here poses = convertGPSPosesToMapPoses( - action_server->get_current_goal()->gps_poses); + current_goal->gps_poses); } return poses; } From 1f980a17d3d45f585f63159a11717493f64e102c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 24 Apr 2024 19:08:45 +0200 Subject: [PATCH 11/35] Make rviz use sim time with TB3 demo (#4274) Signed-off-by: Tony Najjar --- nav2_bringup/launch/rviz_launch.py | 9 +++++++++ nav2_bringup/launch/tb3_simulation_launch.py | 1 + 2 files changed, 10 insertions(+) diff --git a/nav2_bringup/launch/rviz_launch.py b/nav2_bringup/launch/rviz_launch.py index dee33f26d63..8728581df12 100644 --- a/nav2_bringup/launch/rviz_launch.py +++ b/nav2_bringup/launch/rviz_launch.py @@ -34,6 +34,7 @@ def generate_launch_description(): namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') rviz_config_file = LaunchConfiguration('rviz_config') + use_sim_time = LaunchConfiguration('use_sim_time') # Declare the launch arguments declare_namespace_cmd = DeclareLaunchArgument( @@ -57,6 +58,11 @@ def generate_launch_description(): description='Full path to the RVIZ config file to use', ) + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + # Launch rviz start_rviz_cmd = Node( condition=UnlessCondition(use_namespace), @@ -64,6 +70,7 @@ def generate_launch_description(): executable='rviz2', arguments=['-d', rviz_config_file], output='screen', + parameters=[{'use_sim_time': use_sim_time}], ) namespaced_rviz_config_file = ReplaceString( @@ -77,6 +84,7 @@ def generate_launch_description(): executable='rviz2', namespace=namespace, arguments=['-d', namespaced_rviz_config_file], + parameters=[{'use_sim_time': use_sim_time}], output='screen', remappings=[ ('/map', 'map'), @@ -111,6 +119,7 @@ def generate_launch_description(): ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) ld.add_action(declare_rviz_config_file_cmd) + ld.add_action(declare_use_sim_time_cmd) # Add any conditioned actions ld.add_action(start_rviz_cmd) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 6598ac94ba8..2655125a91e 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -242,6 +242,7 @@ def generate_launch_description(): launch_arguments={ 'namespace': namespace, 'use_namespace': use_namespace, + 'use_sim_time': use_sim_time, 'rviz_config': rviz_config_file, }.items(), ) From 9f6b0dd3fb61508b1e042eff6deaca3d21d99550 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Sat, 27 Apr 2024 21:02:44 -0700 Subject: [PATCH 12/35] removing clearable layer param (unused) (#4280) --- .../include/nav2_costmap_2d/clear_costmap_service.hpp | 1 - nav2_costmap_2d/src/clear_costmap_service.cpp | 2 -- nav2_costmap_2d/src/costmap_2d_ros.cpp | 3 --- 3 files changed, 6 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index 46d79bd9df5..2d74530b436 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp @@ -67,7 +67,6 @@ class ClearCostmapService // Clearing parameters unsigned char reset_value_; - std::vector clearable_layers_; // Server for clearing the costmap rclcpp::Service::SharedPtr clear_except_service_; diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 61c23881124..4a97b9ae1ca 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -40,8 +40,6 @@ ClearCostmapService::ClearCostmapService( logger_ = node->get_logger(); reset_value_ = costmap_.getCostmap()->getDefaultValue(); - node->get_parameter("clearable_layers", clearable_layers_); - clear_except_service_ = node->create_service( "clear_except_" + costmap_.getName(), std::bind( diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 2a36bab959c..104aae7fe23 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -109,8 +109,6 @@ void Costmap2DROS::init() { RCLCPP_INFO(get_logger(), "Creating Costmap"); - std::vector clearable_layers{"obstacle_layer", "voxel_layer", "range_layer"}; - declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false)); declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f)); declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]"))); @@ -135,7 +133,6 @@ void Costmap2DROS::init() declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); declare_parameter("use_maximum", rclcpp::ParameterValue(false)); - declare_parameter("clearable_layers", rclcpp::ParameterValue(clearable_layers)); } Costmap2DROS::~Costmap2DROS() From c93080c492021adc126caa247fa608b2d6d5149b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 29 Apr 2024 12:55:38 -0700 Subject: [PATCH 13/35] Collision monitor: Add polygon source (#4275) * Rename PushRosNamespace to PushROSNamespace * Fix min_points checking * Add polygon source * Revert "Merge branch 'add-collision-points-debug' into add-polygon-source" This reverts commit 15c261c0f01b97957711a3812a7b109b644a56d2, reversing changes made to 5a635848a0dde4d5541dee139dafa95d5afdd059. * . * fix * fix * fix lint * PR comments fixes * fixes * add test * fix space * use standard msg Polygonstamped * draft * . * fixes * fixes * fixes * fix * revert * fixes * add detector test * rebasing fixes * Adding polygon source * adjusting tests * linting --------- Co-authored-by: Tony Najjar Co-authored-by: Tony Najjar --- nav2_collision_monitor/CMakeLists.txt | 2 + .../collision_detector_node.hpp | 1 + .../collision_monitor_node.hpp | 1 + .../nav2_collision_monitor/pointcloud.hpp | 2 +- .../nav2_collision_monitor/polygon_source.hpp | 115 +++++++++++ .../include/nav2_collision_monitor/range.hpp | 2 +- .../include/nav2_collision_monitor/scan.hpp | 2 +- .../include/nav2_collision_monitor/source.hpp | 2 +- .../src/collision_detector_node.cpp | 8 + .../src/collision_monitor_node.cpp | 8 + nav2_collision_monitor/src/pointcloud.cpp | 3 +- nav2_collision_monitor/src/polygon.cpp | 5 +- nav2_collision_monitor/src/polygon_source.cpp | 186 ++++++++++++++++++ nav2_collision_monitor/src/range.cpp | 4 +- nav2_collision_monitor/src/scan.cpp | 4 +- .../test/collision_detector_node_test.cpp | 78 +++++++- .../test/collision_monitor_node_test.cpp | 134 ++++++++++++- nav2_collision_monitor/test/sources_test.cpp | 122 ++++++++++++ nav2_util/include/nav2_util/robot_utils.hpp | 1 + nav2_util/src/robot_utils.cpp | 1 + 20 files changed, 670 insertions(+), 11 deletions(-) create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp create mode 100644 nav2_collision_monitor/src/polygon_source.cpp diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index d812df7d654..5dfaf1576ac 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -56,6 +56,7 @@ add_library(${monitor_library_name} SHARED src/source.cpp src/scan.cpp src/pointcloud.cpp + src/polygon_source.cpp src/range.cpp src/kinematics.cpp ) @@ -67,6 +68,7 @@ add_library(${detector_library_name} SHARED src/source.cpp src/scan.cpp src/pointcloud.cpp + src/polygon_source.cpp src/range.cpp src/kinematics.cpp ) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index fae479d6504..7c9785588e2 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -38,6 +38,7 @@ #include "nav2_collision_monitor/scan.hpp" #include "nav2_collision_monitor/pointcloud.hpp" #include "nav2_collision_monitor/range.hpp" +#include "nav2_collision_monitor/polygon_source.hpp" namespace nav2_collision_monitor { diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 998dacb5191..ce4d96504bb 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -41,6 +41,7 @@ #include "nav2_collision_monitor/scan.hpp" #include "nav2_collision_monitor/pointcloud.hpp" #include "nav2_collision_monitor/range.hpp" +#include "nav2_collision_monitor/polygon_source.hpp" namespace nav2_collision_monitor { diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index bfe8d09ed33..ea370f9acca 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -73,7 +73,7 @@ class PointCloud : public Source */ bool getData( const rclcpp::Time & curr_time, - std::vector & data) const; + std::vector & data); protected: /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp new file mode 100644 index 00000000000..8855bb7241e --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp @@ -0,0 +1,115 @@ +// Copyright (c) 2023 Pixel Robotics GmbH +// +// 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_COLLISION_MONITOR__POLYGON_SOURCE_HPP_ +#define NAV2_COLLISION_MONITOR__POLYGON_SOURCE_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/polygon_instance_stamped.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" + +#include "nav2_collision_monitor/source.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Implementation for polygon source + */ +class PolygonSource : public Source +{ +public: + /** + * @brief PolygonSource constructor + * @param node Collision Monitor node pointer + * @param source_name Name of data source + * @param tf_buffer Shared pointer to a TF buffer + * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. + * @param global_frame_id Global frame ID for correct transform calculation + * @param transform_tolerance Transform tolerance + * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time + */ + PolygonSource( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); + /** + * @brief PolygonSource destructor + */ + ~PolygonSource(); + + /** + * @brief Data source configuration routine. Obtains ROS-parameters + * and creates subscriber. + */ + void configure(); + + /** + * @brief Adds latest data from polygon source to the data array. + * @param curr_time Current node time for data interpolation + * @param data Array where the data from source to be added. + * Added data is transformed to base_frame_id_ coordinate system at curr_time. + * @return false if an invalid source should block the robot + */ + bool getData( + const rclcpp::Time & curr_time, + std::vector & data); + + /** + * @brief Converts a PolygonInstanceStamped to a std::vector + * @param polygon Input Polygon to be converted + * @param data Output vector of Point + */ + void convertPolygonStampedToPoints( + const geometry_msgs::msg::PolygonStamped & polygon, + std::vector & data) const; + +protected: + /** + * @brief Getting sensor-specific ROS-parameters + * @param source_topic Output name of source subscription topic + */ + void getParameters(std::string & source_topic); + + /** + * @brief PolygonSource data callback + * @param msg Shared pointer to PolygonSource message + */ + void dataCallback(geometry_msgs::msg::PolygonInstanceStamped::ConstSharedPtr msg); + + // ----- Variables ----- + + /// @brief PolygonSource data subscriber + rclcpp::Subscription::SharedPtr data_sub_; + + /// @brief Latest data obtained + std::vector data_; + + /// @brief distance between sampled points on polygon edges + double sampling_distance_; +}; // class PolygonSource + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__POLYGON_SOURCE_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp index 1dc15195f6e..2bccaac108c 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -73,7 +73,7 @@ class Range : public Source */ bool getData( const rclcpp::Time & curr_time, - std::vector & data) const; + std::vector & data); protected: /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index efaf82e0e37..5c57cdb0148 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -73,7 +73,7 @@ class Scan : public Source */ bool getData( const rclcpp::Time & curr_time, - std::vector & data) const; + std::vector & data); protected: /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 62e0d60afab..4432ec243b1 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -73,7 +73,7 @@ class Source */ virtual bool getData( const rclcpp::Time & curr_time, - std::vector & data) const = 0; + std::vector & data) = 0; /** * @brief Obtains source enabled state diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 4f485520c76..4d8da1e241d 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -287,6 +287,13 @@ bool CollisionDetector::configureSources( r->configure(); sources_.push_back(r); + } else if (source_type == "polygon") { + std::shared_ptr ps = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + ps->configure(); + + sources_.push_back(ps); } else { // Error if something else RCLCPP_ERROR( get_logger(), @@ -345,6 +352,7 @@ void CollisionDetector::process() marker.color.r = 1.0; marker.color.a = 1.0; marker.lifetime = rclcpp::Duration(0, 0); + marker.frame_locked = true; for (const auto & point : collision_points) { geometry_msgs::msg::Point p; diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 94c116445aa..766ed713cd8 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -378,6 +378,13 @@ bool CollisionMonitor::configureSources( r->configure(); sources_.push_back(r); + } else if (source_type == "polygon") { + std::shared_ptr ps = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + ps->configure(); + + sources_.push_back(ps); } else { // Error if something else RCLCPP_ERROR( get_logger(), @@ -444,6 +451,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: marker.color.r = 1.0; marker.color.a = 1.0; marker.lifetime = rclcpp::Duration(0, 0); + marker.frame_locked = true; for (const auto & point : collision_points) { geometry_msgs::msg::Point p; diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 817fb48257c..77daab68055 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -17,6 +17,7 @@ #include #include "sensor_msgs/point_cloud2_iterator.hpp" +#include "tf2/transform_datatypes.h" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -67,7 +68,7 @@ void PointCloud::configure() bool PointCloud::getData( const rclcpp::Time & curr_time, - std::vector & data) const + std::vector & data) { // Ignore data from the source if it is not being published yet or // not published for a long time diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 7e0a69b84f4..08910c91458 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -19,6 +19,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/point32.hpp" +#include "tf2/transform_datatypes.h" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -195,7 +196,7 @@ void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/) std::size_t new_size = polygon_.polygon.points.size(); // Get the transform from PolygonStamped frame to base_frame_id_ - tf2::Transform tf_transform; + tf2::Stamped tf_transform; if ( !nav2_util::getTransform( polygon_.header.frame_id, base_frame_id_, @@ -478,7 +479,7 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m } // Get the transform from PolygonStamped frame to base_frame_id_ - tf2::Transform tf_transform; + tf2::Stamped tf_transform; if ( !nav2_util::getTransform( msg->header.frame_id, base_frame_id_, diff --git a/nav2_collision_monitor/src/polygon_source.cpp b/nav2_collision_monitor/src/polygon_source.cpp new file mode 100644 index 00000000000..149ac43153f --- /dev/null +++ b/nav2_collision_monitor/src/polygon_source.cpp @@ -0,0 +1,186 @@ +// Copyright (c) 2023 Pixel Robotics GmbH +// +// 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_collision_monitor/polygon_source.hpp" + +#include +#include + +#include "geometry_msgs/msg/polygon_stamped.hpp" +#include "tf2/transform_datatypes.h" + +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" + + +namespace nav2_collision_monitor +{ + +PolygonSource::PolygonSource( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) +: Source( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, source_timeout, base_shift_correction) +{ +} + +PolygonSource::~PolygonSource() +{ + data_sub_.reset(); +} + +void PolygonSource::configure() +{ + Source::configure(); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + std::string source_topic; + + getParameters(source_topic); + + rclcpp::QoS qos = rclcpp::SensorDataQoS(); // set to default + data_sub_ = node->create_subscription( + source_topic, qos, + std::bind(&PolygonSource::dataCallback, this, std::placeholders::_1)); +} + +bool PolygonSource::getData( + const rclcpp::Time & curr_time, + std::vector & data) +{ + // Ignore data from the source if it is not being published yet or + // not published for a long time + if (data_.empty()) { + return false; + } + + // Remove stale data + data_.erase( + std::remove_if( + data_.begin(), data_.end(), + [this, curr_time](const geometry_msgs::msg::PolygonInstanceStamped & polygon_stamped) { + return curr_time - rclcpp::Time(polygon_stamped.header.stamp) > source_timeout_; + }), data_.end()); + + tf2::Stamped tf_transform; + for (const auto & polygon_instance : data_) { + if (base_shift_correction_) { + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + if ( + !nav2_util::getTransform( + polygon_instance.header.frame_id, polygon_instance.header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } else { + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. + if ( + !nav2_util::getTransform( + polygon_instance.header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } + geometry_msgs::msg::PolygonStamped poly_out, polygon_stamped; + geometry_msgs::msg::TransformStamped tf = tf2::toMsg(tf_transform); + polygon_stamped.header = polygon_instance.header; + polygon_stamped.polygon = polygon_instance.polygon.polygon; + tf2::doTransform(polygon_stamped, poly_out, tf); + convertPolygonStampedToPoints(poly_out, data); + } + return true; +} + +void PolygonSource::convertPolygonStampedToPoints( + const geometry_msgs::msg::PolygonStamped & polygon, std::vector & data) const +{ + /* Full function generated by GPT */ + + // Iterate over the vertices of the polygon + for (size_t i = 0; i < polygon.polygon.points.size(); ++i) { + const auto & current_point = polygon.polygon.points[i]; + const auto & next_point = + polygon.polygon.points[(i + 1) % polygon.polygon.points.size()]; + + // Calculate the distance between the current and next points + double segment_length = + std::hypot(next_point.x - current_point.x, next_point.y - current_point.y); + + // Calculate the number of points to sample in the current segment + int num_points_in_segment = + std::max(static_cast(std::ceil(segment_length / sampling_distance_)), 1); + + // Calculate the step size for each pair of vertices + const double dx = (next_point.x - current_point.x) / num_points_in_segment; + const double dy = (next_point.y - current_point.y) / num_points_in_segment; + + // Sample the points with equal spacing + for (int j = 0; j <= num_points_in_segment; ++j) { + Point p; + p.x = current_point.x + j * dx; + p.y = current_point.y + j * dy; + data.push_back(p); + } + } +} + +void PolygonSource::getParameters(std::string & source_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + getCommonParameters(source_topic); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".sampling_distance", rclcpp::ParameterValue(0.1)); + sampling_distance_ = node->get_parameter(source_name_ + ".sampling_distance").as_double(); +} + +void PolygonSource::dataCallback(geometry_msgs::msg::PolygonInstanceStamped::ConstSharedPtr msg) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + auto curr_time = node->now(); + + // check if older similar polygon exists already and replace it with the new one + for (auto & polygon_stamped : data_) { + if (msg->polygon.id == polygon_stamped.polygon.id) { + polygon_stamped = *msg; + return; + } + } + data_.push_back(*msg); +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 8799827f047..fe555b171f3 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -18,6 +18,8 @@ #include #include +#include "tf2/transform_datatypes.h" + #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -67,7 +69,7 @@ void Range::configure() bool Range::getData( const rclcpp::Time & curr_time, - std::vector & data) const + std::vector & data) { // Ignore data from the source if it is not being published yet or // not being published for a long time diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 660d66e9167..f891df69a0a 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -17,6 +17,8 @@ #include #include +#include "tf2/transform_datatypes.h" + #include "nav2_util/robot_utils.hpp" namespace nav2_collision_monitor @@ -66,7 +68,7 @@ void Scan::configure() bool Scan::getData( const rclcpp::Time & curr_time, - std::vector & data) const + std::vector & data) { // Ignore data from the source if it is not being published yet or // not being published for a long time diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index e36ef6e583e..da2ecbc98e8 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -49,6 +49,7 @@ static const char ODOM_FRAME_ID[]{"odom"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char RANGE_NAME[]{"Range"}; +static const char POLYGON_NAME[]{"Polygon"}; static const char STATE_TOPIC[]{"collision_detector_state"}; static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_detector/collision_points_marker"}; static const int MIN_POINTS{1}; @@ -69,7 +70,8 @@ enum SourceType SOURCE_UNKNOWN = 0, SCAN = 1, POINTCLOUD = 2, - RANGE = 3 + RANGE = 3, + POLYGON_SOURCE = 4 }; class CollisionDetectorWrapper : public nav2_collision_monitor::CollisionDetector @@ -140,6 +142,7 @@ class Tester : public ::testing::Test void publishScan(const double dist, const rclcpp::Time & stamp); void publishPointCloud(const double dist, const rclcpp::Time & stamp); void publishRange(const double dist, const rclcpp::Time & stamp); + void publishPolygon(const double dist, const rclcpp::Time & stamp); bool waitData( const double expected_dist, const std::chrono::nanoseconds & timeout, @@ -157,6 +160,7 @@ class Tester : public ::testing::Test rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; rclcpp::Publisher::SharedPtr range_pub_; + rclcpp::Publisher::SharedPtr polygon_source_pub_; rclcpp::Subscription::SharedPtr state_sub_; nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_; @@ -177,6 +181,8 @@ Tester::Tester() POINTCLOUD_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); range_pub_ = cd_->create_publisher( RANGE_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + polygon_source_pub_ = cd_->create_publisher( + POLYGON_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); state_sub_ = cd_->create_subscription( STATE_TOPIC, rclcpp::SystemDefaultsQoS(), @@ -192,6 +198,7 @@ Tester::~Tester() scan_pub_.reset(); pointcloud_pub_.reset(); range_pub_.reset(); + polygon_source_pub_.reset(); collision_points_marker_sub_.reset(); cd_.reset(); @@ -353,6 +360,20 @@ void Tester::addSource( source_name + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 200)); cd_->set_parameter( rclcpp::Parameter(source_name + ".obstacles_angle", M_PI / 200)); + } else if (type == POLYGON_SOURCE) { + cd_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("polygon")); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".type", "polygon")); + + cd_->declare_parameter( + source_name + ".sampling_distance", rclcpp::ParameterValue(0.1)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".sampling_distance", 0.1)); + cd_->declare_parameter( + source_name + ".polygon_similarity_threshold", rclcpp::ParameterValue(2.0)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".polygon_similarity_threshold", 2.0)); } else { // type == SOURCE_UNKNOWN cd_->declare_parameter( source_name + ".type", rclcpp::ParameterValue("unknown")); @@ -476,6 +497,32 @@ void Tester::publishRange(const double dist, const rclcpp::Time & stamp) range_pub_->publish(std::move(msg)); } +void Tester::publishPolygon(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = dist; + msg->polygon.polygon.points.push_back(p); + p.x = -1.0; + p.y = dist; + msg->polygon.polygon.points.push_back(p); + p.x = -1.0; + p.y = dist + 1.0; + msg->polygon.polygon.points.push_back(p); + p.x = 1.0; + p.y = dist + 1.0; + msg->polygon.polygon.points.push_back(p); + msg->polygon.id = 1u; + + polygon_source_pub_->publish(std::move(msg)); +} + bool Tester::waitData( const double expected_dist, const std::chrono::nanoseconds & timeout, @@ -714,6 +761,35 @@ TEST_F(Tester, testPointcloudDetection) cd_->stop(); } +TEST_F(Tester, testPolygonSourceDetection) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Detector parameters. + setCommonParameters(); + // Create polygon + addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addSource(POLYGON_NAME, POLYGON_SOURCE); + setVectors({"DetectionRegion"}, {POLYGON_NAME}); + + // Start Collision Detector node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is in DetectionRegion + publishPolygon(2.5, curr_time); + + ASSERT_TRUE(waitData(std::hypot(2.5, 1.0), 500ms, curr_time)); + ASSERT_TRUE(waitState(300ms)); + ASSERT_NE(state_msg_->detections.size(), 0u); + ASSERT_EQ(state_msg_->detections[0], true); + + // Stop Collision Detector node + cd_->stop(); +} + TEST_F(Tester, testCollisionPointsMarkers) { rclcpp::Time curr_time = cd_->now(); diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 34690a6fd1c..69f8ac21e06 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -54,6 +54,7 @@ static const char FOOTPRINT_TOPIC[]{"footprint"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char RANGE_NAME[]{"Range"}; +static const char POLYGON_NAME[]{"Polygon"}; static const int MIN_POINTS{2}; static const double SLOWDOWN_RATIO{0.7}; static const double LINEAR_LIMIT{0.4}; @@ -77,7 +78,8 @@ enum SourceType SOURCE_UNKNOWN = 0, SCAN = 1, POINTCLOUD = 2, - RANGE = 3 + RANGE = 3, + POLYGON_SOURCE = 4 }; enum ActionType @@ -165,6 +167,7 @@ class Tester : public ::testing::Test void publishScan(const double dist, const rclcpp::Time & stamp); void publishPointCloud(const double dist, const rclcpp::Time & stamp); void publishRange(const double dist, const rclcpp::Time & stamp); + void publishPolygon(const double dist, const rclcpp::Time & stamp); void publishCmdVel(const double x, const double y, const double tw); bool waitData( const double expected_dist, @@ -192,6 +195,7 @@ class Tester : public ::testing::Test rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; rclcpp::Publisher::SharedPtr range_pub_; + rclcpp::Publisher::SharedPtr polygon_source_pub_; // Working with cmd_vel_in/cmd_vel_out rclcpp::Publisher::SharedPtr cmd_vel_in_pub_; @@ -225,6 +229,8 @@ Tester::Tester() POINTCLOUD_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); range_pub_ = cm_->create_publisher( RANGE_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + polygon_source_pub_ = cm_->create_publisher( + POLYGON_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); cmd_vel_in_pub_ = cm_->create_publisher( CMD_VEL_IN_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); @@ -251,6 +257,7 @@ Tester::~Tester() scan_pub_.reset(); pointcloud_pub_.reset(); range_pub_.reset(); + polygon_source_pub_.reset(); cmd_vel_in_pub_.reset(); cmd_vel_out_sub_.reset(); @@ -472,6 +479,20 @@ void Tester::addSource( source_name + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 200)); cm_->set_parameter( rclcpp::Parameter(source_name + ".obstacles_angle", M_PI / 200)); + } else if (type == POLYGON_SOURCE) { + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("polygon")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "polygon")); + + cm_->declare_parameter( + source_name + ".sampling_distance", rclcpp::ParameterValue(0.1)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".sampling_distance", 0.1)); + cm_->declare_parameter( + source_name + ".polygon_similarity_threshold", rclcpp::ParameterValue(2.0)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".polygon_similarity_threshold", 2.0)); } else { // type == SOURCE_UNKNOWN cm_->declare_parameter( source_name + ".type", rclcpp::ParameterValue("unknown")); @@ -628,6 +649,31 @@ void Tester::publishRange(const double dist, const rclcpp::Time & stamp) range_pub_->publish(std::move(msg)); } +void Tester::publishPolygon(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = dist; + msg->polygon.polygon.points.push_back(p); + p.x = -1.0; + p.y = dist; + msg->polygon.polygon.points.push_back(p); + p.x = -1.0; + p.y = dist + 1.0; + msg->polygon.polygon.points.push_back(p); + p.x = 1.0; + p.y = dist + 1.0; + msg->polygon.polygon.points.push_back(p); + + polygon_source_pub_->publish(std::move(msg)); +} + void Tester::publishCmdVel(const double x, const double y, const double tw) { // Reset cmd_vel_out_ before calling CollisionMonitor::process() @@ -813,6 +859,92 @@ TEST_F(Tester, testProcessStopSlowdownLimit) cm_->stop(); } +TEST_F(Tester, testPolygonSource) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner for robot stop. + setCommonParameters(); + // Set source_timeout to 0.0 to clear out quickly the polygons from test to test + cm_->set_parameter( + rclcpp::Parameter("source_timeout", 0.1)); + addPolygon("Limit", POLYGON, 3.0, "limit"); + addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(POLYGON_NAME, POLYGON_SOURCE); + setVectors({"Limit", "SlowDown", "Stop"}, {POLYGON_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is far away from robot + publishPolygon(4.5, curr_time); + ASSERT_TRUE(waitData(std::hypot(1.0, 4.5), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + + // 2. Obstacle is in limit robot zone + publishPolygon(3.0, curr_time); + EXPECT_TRUE(waitData(std::hypot(1.0, 3.0), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + EXPECT_TRUE(waitCmdVel(500ms)); + const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2); + const double ratio = LINEAR_LIMIT / speed; + EXPECT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON); + EXPECT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON); + EXPECT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON); + EXPECT_TRUE(waitActionState(500ms)); + EXPECT_EQ(action_state_->action_type, LIMIT); + EXPECT_EQ(action_state_->polygon_name, "Limit"); + + // 3. Obstacle is in slowdown robot zone + publishPolygon(1.5, curr_time); + EXPECT_TRUE(waitData(std::hypot(1.0, 1.5), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + EXPECT_TRUE(waitCmdVel(500ms)); + EXPECT_NEAR(cmd_vel_out_->linear.x, 0.5 * SLOWDOWN_RATIO, EPSILON); + EXPECT_NEAR(cmd_vel_out_->linear.y, 0.2 * SLOWDOWN_RATIO, EPSILON); + EXPECT_NEAR(cmd_vel_out_->angular.z, 0.1 * SLOWDOWN_RATIO, EPSILON); + EXPECT_TRUE(waitActionState(500ms)); + EXPECT_EQ(action_state_->action_type, SLOWDOWN); + EXPECT_EQ(action_state_->polygon_name, "SlowDown"); + + // 4. Obstacle is inside stop zone + curr_time = cm_->now(); + publishPolygon(0.5, curr_time); + EXPECT_TRUE(waitData(std::hypot(1.0, 0.5), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + EXPECT_TRUE(waitCmdVel(500ms)); + EXPECT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + EXPECT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + EXPECT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + EXPECT_TRUE(waitActionState(500ms)); + EXPECT_EQ(action_state_->action_type, STOP); + EXPECT_EQ(action_state_->polygon_name, "Stop"); + + // 5. Restoring back normal operation + publishPolygon(4.5, curr_time); + ASSERT_TRUE(waitData(std::hypot(1.0, 4.5), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, DO_NOTHING); + ASSERT_EQ(action_state_->polygon_name, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + TEST_F(Tester, testProcessApproach) { rclcpp::Time curr_time = cm_->now(); diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 0bfcd1a062c..24de5cbe056 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -38,6 +38,7 @@ #include "nav2_collision_monitor/scan.hpp" #include "nav2_collision_monitor/pointcloud.hpp" #include "nav2_collision_monitor/range.hpp" +#include "nav2_collision_monitor/polygon_source.hpp" using namespace std::chrono_literals; @@ -52,6 +53,8 @@ static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char POINTCLOUD_TOPIC[]{"pointcloud"}; static const char RANGE_NAME[]{"Range"}; static const char RANGE_TOPIC[]{"range"}; +static const char POLYGON_NAME[]{"Polygon"}; +static const char POLYGON_TOPIC[]{"polygon"}; static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; static const rclcpp::Duration DATA_TIMEOUT{rclcpp::Duration::from_seconds(5.0)}; @@ -68,6 +71,7 @@ class TestNode : public nav2_util::LifecycleNode scan_pub_.reset(); pointcloud_pub_.reset(); range_pub_.reset(); + polygon_pub_.reset(); } void publishScan(const rclcpp::Time & stamp, const double range) @@ -156,10 +160,43 @@ class TestNode : public nav2_util::LifecycleNode range_pub_->publish(std::move(msg)); } + void publishPolygon(const rclcpp::Time & stamp) + { + polygon_pub_ = this->create_publisher( + POLYGON_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + geometry_msgs::msg::Point32 point; + point.x = 1.0; + point.y = -1.0; + point.z = 0.0; + msg->polygon.polygon.points.push_back(point); + point.x = 1.0; + point.y = 1.0; + point.z = 0.0; + msg->polygon.polygon.points.push_back(point); + point.x = -1.0; + point.y = 1.0; + point.z = 0.0; + msg->polygon.polygon.points.push_back(point); + point.x = -1.0; + point.y = -1.0; + point.z = 0.0; + msg->polygon.polygon.points.push_back(point); + msg->polygon.id = 1u; + polygon_pub_->publish(std::move(msg)); + } + private: rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; rclcpp::Publisher::SharedPtr range_pub_; + rclcpp::Publisher::SharedPtr polygon_pub_; }; // TestNode class ScanWrapper : public nav2_collision_monitor::Scan @@ -231,6 +268,29 @@ class RangeWrapper : public nav2_collision_monitor::Range } }; // RangeWrapper +class PolygonWrapper : public nav2_collision_monitor::PolygonSource +{ +public: + PolygonWrapper( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & data_timeout, + const bool base_shift_correction) + : nav2_collision_monitor::PolygonSource( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, data_timeout, base_shift_correction) + {} + + bool dataReceived() const + { + return data_.size() > 0; + } +}; // PolygonWrapper + class Tester : public ::testing::Test { public: @@ -248,14 +308,17 @@ class Tester : public ::testing::Test bool waitScan(const std::chrono::nanoseconds & timeout); bool waitPointCloud(const std::chrono::nanoseconds & timeout); bool waitRange(const std::chrono::nanoseconds & timeout); + bool waitPolygon(const std::chrono::nanoseconds & timeout); void checkScan(const std::vector & data); void checkPointCloud(const std::vector & data); void checkRange(const std::vector & data); + void checkPolygon(const std::vector & data); std::shared_ptr test_node_; std::shared_ptr scan_; std::shared_ptr pointcloud_; std::shared_ptr range_; + std::shared_ptr polygon_; private: std::shared_ptr tf_buffer_; @@ -331,6 +394,21 @@ void Tester::createSources(const bool base_shift_correction) BASE_FRAME_ID, GLOBAL_FRAME_ID, TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); range_->configure(); + + // Create Polygon object + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".topic", rclcpp::ParameterValue(POLYGON_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".topic", POLYGON_TOPIC)); + + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".sampling_distance", rclcpp::ParameterValue(0.1)); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); + polygon_->configure(); } void Tester::sendTransforms(const rclcpp::Time & stamp) @@ -404,6 +482,19 @@ bool Tester::waitRange(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitPolygon(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (polygon_->dataReceived()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + void Tester::checkScan(const std::vector & data) { ASSERT_EQ(data.size(), 4u); @@ -458,6 +549,11 @@ void Tester::checkRange(const std::vector & data) ASSERT_NEAR(data[i].y, 1.0 * std::sin(angle) + 0.1, EPSILON); } +void Tester::checkPolygon(const std::vector & data) +{ + ASSERT_EQ(data.size(), 84u); +} + TEST_F(Tester, testGetData) { rclcpp::Time curr_time = test_node_->now(); @@ -470,11 +566,13 @@ TEST_F(Tester, testGetData) test_node_->publishScan(curr_time, 1.0); test_node_->publishPointCloud(curr_time); test_node_->publishRange(curr_time, 1.0); + test_node_->publishPolygon(curr_time); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); ASSERT_TRUE(waitRange(500ms)); + ASSERT_TRUE(waitPolygon(500ms)); // Check Scan data std::vector data; @@ -490,6 +588,11 @@ TEST_F(Tester, testGetData) data.clear(); range_->getData(curr_time, data); checkRange(data); + + // Check Polygon data + data.clear(); + polygon_->getData(curr_time, data); + checkPolygon(data); } TEST_F(Tester, testGetOutdatedData) @@ -504,11 +607,13 @@ TEST_F(Tester, testGetOutdatedData) test_node_->publishScan(curr_time - DATA_TIMEOUT - 1s, 1.0); test_node_->publishPointCloud(curr_time - DATA_TIMEOUT - 1s); test_node_->publishRange(curr_time - DATA_TIMEOUT - 1s, 1.0); + test_node_->publishPolygon(curr_time - DATA_TIMEOUT - 1s); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); ASSERT_TRUE(waitRange(500ms)); + ASSERT_TRUE(waitPolygon(500ms)); // Scan data should be empty std::vector data; @@ -522,6 +627,10 @@ TEST_F(Tester, testGetOutdatedData) // Range data should be empty range_->getData(curr_time, data); ASSERT_EQ(data.size(), 0u); + + // Polygon data should be empty + polygon_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); } TEST_F(Tester, testIncorrectFrameData) @@ -537,11 +646,13 @@ TEST_F(Tester, testIncorrectFrameData) test_node_->publishScan(curr_time, 1.0); test_node_->publishPointCloud(curr_time); test_node_->publishRange(curr_time, 1.0); + test_node_->publishPolygon(curr_time); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); ASSERT_TRUE(waitRange(500ms)); + ASSERT_TRUE(waitPolygon(500ms)); // Scan data should be empty std::vector data; @@ -555,6 +666,10 @@ TEST_F(Tester, testIncorrectFrameData) // Range data should be empty range_->getData(curr_time, data); ASSERT_EQ(data.size(), 0u); + + // Polygon data should be empty + polygon_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); } TEST_F(Tester, testIncorrectData) @@ -597,11 +712,13 @@ TEST_F(Tester, testIgnoreTimeShift) test_node_->publishScan(curr_time, 1.0); test_node_->publishPointCloud(curr_time); test_node_->publishRange(curr_time, 1.0); + test_node_->publishPolygon(curr_time); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); ASSERT_TRUE(waitRange(500ms)); + ASSERT_TRUE(waitPolygon(500ms)); // Scan data should be consistent std::vector data; @@ -617,6 +734,11 @@ TEST_F(Tester, testIgnoreTimeShift) data.clear(); range_->getData(curr_time, data); checkRange(data); + + // Polygon data should be consistent + data.clear(); + polygon_->getData(curr_time, data); + checkPolygon(data); } int main(int argc, char ** argv) diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 08c52af0402..cb7d2b9fdfb 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -25,6 +25,7 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2/time.h" +#include "tf2/transform_datatypes.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 75aa37ada3e..a9f4d137fa2 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -169,6 +169,7 @@ bool getTransform( tf2::fromMsg(transform.transform, tf2_transform); return true; } + return false; } From c6ccd8e6db1edc138c6cf3650e192cc595d44e7f Mon Sep 17 00:00:00 2001 From: James Ward Date: Wed, 1 May 2024 01:40:22 +1000 Subject: [PATCH 14/35] Ignore warnings in included xtensor library (#4285) Signed-off-by: James Ward --- .../include/nav2_mppi_controller/critic_data.hpp | 6 ++++++ .../include/nav2_mppi_controller/critic_manager.hpp | 6 ++++++ .../nav2_mppi_controller/models/control_sequence.hpp | 5 +++++ .../include/nav2_mppi_controller/models/path.hpp | 5 +++++ .../include/nav2_mppi_controller/models/state.hpp | 5 +++++ .../include/nav2_mppi_controller/models/trajectories.hpp | 5 +++++ .../include/nav2_mppi_controller/motion_models.hpp | 6 ++++++ .../include/nav2_mppi_controller/optimizer.hpp | 5 +++++ .../include/nav2_mppi_controller/tools/noise_generator.hpp | 5 +++++ .../nav2_mppi_controller/tools/trajectory_visualizer.hpp | 6 ++++++ .../include/nav2_mppi_controller/tools/utils.hpp | 5 +++++ 11 files changed, 59 insertions(+) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp index 31965eb1cfc..05adb417911 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp @@ -17,7 +17,13 @@ #include #include + +// xtensor creates warnings that needs to be ignored as we are building with -Werror +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include +#pragma GCC diagnostic pop #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_core/goal_checker.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp index 2de3e72a29b..7e09fcf91c3 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -19,7 +19,13 @@ #include #include #include + +// xtensor creates warnings that needs to be ignored as we are building with -Werror +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include +#pragma GCC diagnostic pop #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp index 7a96ad1c94e..ad797e9afaa 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp @@ -15,7 +15,12 @@ #ifndef NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_ #define NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_ +// xtensor creates warnings that needs to be ignored as we are building with -Werror +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include +#pragma GCC diagnostic pop namespace mppi::models { diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp index 241a6928bab..f1bab3f8032 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp @@ -15,7 +15,12 @@ #ifndef NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_ #define NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_ +// xtensor creates warnings that needs to be ignored as we are building with -Werror +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include +#pragma GCC diagnostic pop namespace mppi::models { diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp index 7dd19eaff39..75f8c7521a2 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp @@ -15,7 +15,12 @@ #ifndef NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_ #define NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_ +// xtensor creates warnings that needs to be ignored as we are building with -Werror +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include +#pragma GCC diagnostic pop #include #include diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp index fa2c0181204..0862c2caedc 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp @@ -15,8 +15,13 @@ #ifndef NAV2_MPPI_CONTROLLER__MODELS__TRAJECTORIES_HPP_ #define NAV2_MPPI_CONTROLLER__MODELS__TRAJECTORIES_HPP_ +// xtensor creates warnings that needs to be ignored as we are building with -Werror +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include #include +#pragma GCC diagnostic pop namespace mppi::models { diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 1132953bd9a..713c3d2a22a 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -19,10 +19,16 @@ #include "nav2_mppi_controller/models/control_sequence.hpp" #include "nav2_mppi_controller/models/state.hpp" + +// xtensor creates warnings that needs to be ignored as we are building with -Werror +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include #include #include #include +#pragma GCC diagnostic pop #include "nav2_mppi_controller/tools/parameters_handler.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 061654aa659..6e1c271d94d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -18,8 +18,13 @@ #include #include +// xtensor creates warnings that needs to be ignored as we are building with -Werror +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include #include +#pragma GCC diagnostic pop #include "rclcpp_lifecycle/lifecycle_node.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index a811d998e41..f16bb909531 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp @@ -21,8 +21,13 @@ #include #include +// xtensor creates warnings that needs to be ignored as we are building with -Werror +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include #include +#pragma GCC diagnostic pop #include "nav2_mppi_controller/models/optimizer_settings.hpp" #include "nav2_mppi_controller/tools/parameters_handler.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp index 6ecdbdcb8dc..d9cdc95ce98 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp @@ -17,7 +17,13 @@ #include #include + +// xtensor creates warnings that needs to be ignored as we are building with -Werror +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include +#pragma GCC diagnostic pop #include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 2232957c15f..fae3328f066 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -23,10 +23,15 @@ #include #include +// xtensor creates warnings that needs to be ignored as we are building with -Werror +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" +#pragma GCC diagnostic ignored "-Wstringop-overflow" #include #include #include #include +#pragma GCC diagnostic pop #include "angles/angles.h" From 8f003606af76a0cb4968d6ea2eb00ecf08f5e2f3 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Tue, 30 Apr 2024 23:43:42 +0800 Subject: [PATCH 15/35] provide message validation check API (#4276) * provide validation_message.hpp Signed-off-by: goes * fix typo Signed-off-by: goes * add test_validation_messages.cpp Signed-off-by: goes * change include-order Signed-off-by: goes * reformat Signed-off-by: goes * update test Signed-off-by: goes --------- Signed-off-by: goes Co-authored-by: goes --- nav2_amcl/src/amcl_node.cpp | 5 + nav2_costmap_2d/plugins/static_layer.cpp | 5 + .../include/nav2_util/validate_messages.hpp | 147 +++++++++++ nav2_util/test/CMakeLists.txt | 4 + nav2_util/test/test_validation_messages.cpp | 247 ++++++++++++++++++ 5 files changed, 408 insertions(+) create mode 100644 nav2_util/include/nav2_util/validate_messages.hpp create mode 100644 nav2_util/test/test_validation_messages.cpp diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index b311b7ac5e9..ba5c83447fd 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -49,6 +49,7 @@ #pragma GCC diagnostic pop #include "nav2_amcl/portable_utils.hpp" +#include "nav2_util/validate_messages.hpp" using namespace std::placeholders; using rcl_interfaces::msg::ParameterType; @@ -1390,6 +1391,10 @@ void AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { RCLCPP_DEBUG(get_logger(), "AmclNode: A new map was received."); + if (!nav2_util::validateMsg(*msg)) { + RCLCPP_ERROR(get_logger(), "Received map message is malformed. Rejecting."); + return; + } if (first_map_only_ && first_map_received_) { return; } diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 18cd51164f2..cc90b8409b1 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -45,6 +45,7 @@ #include "pluginlib/class_list_macros.hpp" #include "tf2/convert.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_util/validate_messages.hpp" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) @@ -277,6 +278,10 @@ StaticLayer::interpretValue(unsigned char value) void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) { + if (!nav2_util::validateMsg(*new_map)) { + RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting."); + return; + } if (!map_received_) { processMap(*new_map); map_received_ = true; diff --git a/nav2_util/include/nav2_util/validate_messages.hpp b/nav2_util/include/nav2_util/validate_messages.hpp new file mode 100644 index 00000000000..7ce942be42d --- /dev/null +++ b/nav2_util/include/nav2_util/validate_messages.hpp @@ -0,0 +1,147 @@ +// Copyright (c) 2024 GoesM +// +// 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_UTIL__VALIDATE_MESSAGES_HPP_ +#define NAV2_UTIL__VALIDATE_MESSAGES_HPP_ + +#include +#include + +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/odometry.hpp" + + +// @brief Validation Check +// Check recieved message is safe or not for the nav2-system +// For each msg-type known in nav2, we could check it as following: +// if(!validateMsg()) RCLCPP_ERROR(,"malformed msg. Rejecting.") +// +// Workflow of validateMsg(): +// if here's a sub-msg-type in the recieved msg, +// the content of sub-msg would be checked as sub-msg-type +// then, check the whole recieved msg. +// +// Following conditions are involved in check: +// 1> Value Check: to avoid damaged value like like `nan`, `INF`, empty string and so on +// 2> Logic Check: to avoid value with bad logic, +// like the size of `map` should be equal to `height*width` +// 3> Any other needed condition could be joint here in future + +namespace nav2_util +{ + + +bool validateMsg(const double & num) +{ + /* @brief double/float value check + * if here'a need to check message validation + * it should be avoid to use double value like `nan`, `inf` + * otherwise, we regard it as an invalid message + */ + if (std::isinf(num)) {return false;} + if (std::isnan(num)) {return false;} + return true; +} + +const int NSEC_PER_SEC = 1e9; // 1 second = 1e9 nanosecond +bool validateMsg(const builtin_interfaces::msg::Time & msg) +{ + if (msg.nanosec >= NSEC_PER_SEC) { + return false; // invalid nanosec-stamp + } + return true; +} + +bool validateMsg(const std_msgs::msg::Header & msg) +{ + // check sub-type + if (!validateMsg(msg.stamp)) {return false;} + + /* @brief frame_id check + * if here'a need to check message validation + * it should at least have a non-empty frame_id + * otherwise, we regard it as an invalid message + */ + if (msg.frame_id.empty()) {return false;} + return true; +} + +bool validateMsg(const geometry_msgs::msg::Point & msg) +{ + // check sub-type + if (!validateMsg(msg.x)) {return false;} + if (!validateMsg(msg.y)) {return false;} + if (!validateMsg(msg.z)) {return false;} + return true; +} + +const double epsilon = 1e-4; +bool validateMsg(const geometry_msgs::msg::Quaternion & msg) +{ + // check sub-type + if (!validateMsg(msg.x)) {return false;} + if (!validateMsg(msg.y)) {return false;} + if (!validateMsg(msg.z)) {return false;} + if (!validateMsg(msg.w)) {return false;} + + if (abs(msg.x * msg.x + msg.y * msg.y + msg.z * msg.z + msg.w * msg.w - 1.0) >= epsilon) { + return false; + } + + return true; +} + +bool validateMsg(const geometry_msgs::msg::Pose & msg) +{ + // check sub-type + if (!validateMsg(msg.position)) {return false;} + if (!validateMsg(msg.orientation)) {return false;} + return true; +} + + +// Function to verify map meta information +bool validateMsg(const nav_msgs::msg::MapMetaData & msg) +{ + // check sub-type + if (!validateMsg(msg.origin)) {return false;} + if (!validateMsg(msg.resolution)) {return false;} + + // logic check + // 1> we don't need an empty map + if (msg.height == 0 || msg.width == 0) {return false;} + return true; +} + +// for msg-type like map, costmap and others as `OccupancyGrid` +bool validateMsg(const nav_msgs::msg::OccupancyGrid & msg) +{ + // check sub-type + if (!validateMsg(msg.header)) {return false;} + // msg.data : @todo any check for it ? + if (!validateMsg(msg.info)) {return false;} + + // check logic + if (msg.data.size() != msg.info.width * msg.info.height) { + return false; // check map-size + } + return true; +} + + +} // namespace nav2_util + + +#endif // NAV2_UTIL__VALIDATE_MESSAGES_HPP_ diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 14e1a361d3b..4c0f2747210 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -56,3 +56,7 @@ target_link_libraries(test_twist_publisher ${library_name}) ament_add_gtest(test_twist_subscriber test_twist_subscriber.cpp) ament_target_dependencies(test_twist_subscriber rclcpp_lifecycle) target_link_libraries(test_twist_subscriber ${library_name}) + +ament_add_gtest(test_validation_messages test_validation_messages.cpp) +ament_target_dependencies(test_validation_messages rclcpp_lifecycle) +target_link_libraries(test_validation_messages ${library_name}) \ No newline at end of file diff --git a/nav2_util/test/test_validation_messages.cpp b/nav2_util/test/test_validation_messages.cpp new file mode 100644 index 00000000000..060ec11ede9 --- /dev/null +++ b/nav2_util/test/test_validation_messages.cpp @@ -0,0 +1,247 @@ +// Copyright (c) 2024 GoesM +// +// 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_util/validate_messages.hpp" + +TEST(ValidateMessagesTest, DoubleValueCheck) { + // Test valid double value + EXPECT_TRUE(nav2_util::validateMsg(3.14)); + // Test invalid double value (infinity) + EXPECT_FALSE(nav2_util::validateMsg(std::numeric_limits::infinity())); + // Test invalid double value (NaN) + EXPECT_FALSE(nav2_util::validateMsg(std::numeric_limits::quiet_NaN())); +} + +TEST(ValidateMessagesTest, TimeStampCheck) +{ + // Test valid time stamp + builtin_interfaces::msg::Time valid_time_stamp; + valid_time_stamp.sec = 123; + valid_time_stamp.nanosec = 456789; + EXPECT_TRUE(nav2_util::validateMsg(valid_time_stamp)); + // Test invalid time stamp (nanosec out of range) + builtin_interfaces::msg::Time invalid_time_stamp; + invalid_time_stamp.sec = 123; + invalid_time_stamp.nanosec = 1e9; // 1 second = 1e9 nanoseconds + EXPECT_FALSE(nav2_util::validateMsg(invalid_time_stamp)); +} + +TEST(ValidateMessagesTest, HeaderCheck) +{ + // Test valid header with non-empty frame_id + std_msgs::msg::Header valid_header; + valid_header.stamp.sec = 123; + valid_header.stamp.nanosec = 456789; + valid_header.frame_id = "map"; + EXPECT_TRUE(nav2_util::validateMsg(valid_header)); + // Test invalid header with empty frame_id + std_msgs::msg::Header invalid_header; + invalid_header.stamp.sec = 123; + invalid_header.stamp.nanosec = 456789; + invalid_header.frame_id = ""; + EXPECT_FALSE(nav2_util::validateMsg(invalid_header)); + invalid_header.stamp.sec = 123; + invalid_header.stamp.nanosec = 1e9; + invalid_header.frame_id = "map"; + EXPECT_FALSE(nav2_util::validateMsg(invalid_header)); +} + +TEST(ValidateMessagesTest, PointCheck) +{ + // Test valid Point message + geometry_msgs::msg::Point valid_point; + valid_point.x = 1.0; + valid_point.y = 2.0; + valid_point.z = 3.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_point)); + // Test invalid Point message with NaN value + geometry_msgs::msg::Point invalid_point; + invalid_point.x = 1.0; + invalid_point.y = std::numeric_limits::quiet_NaN(); + invalid_point.z = 3.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); + // Test invalid Point message with NaN value + invalid_point.x = std::numeric_limits::quiet_NaN(); + invalid_point.y = 2.0; + invalid_point.z = 3.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); + // Test invalid Point message with NaN value + invalid_point.x = 1.0; + invalid_point.y = 2.0; + invalid_point.z = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); +} + +TEST(ValidateMessagesTest, QuaternionCheck) +{ + // Test valid Quaternion message + geometry_msgs::msg::Quaternion valid_quaternion; + valid_quaternion.x = 0.0; + valid_quaternion.y = 0.0; + valid_quaternion.z = 0.0; + valid_quaternion.w = 1.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_quaternion)); + // Test invalid Quaternion message with invalid magnitude + geometry_msgs::msg::Quaternion invalid_quaternion; + invalid_quaternion.x = 0.1; + invalid_quaternion.y = 0.2; + invalid_quaternion.z = 0.3; + invalid_quaternion.w = 0.5; // Invalid magnitude (should be 1.0) + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + + // One NaN value + invalid_quaternion.x = 0.0; + invalid_quaternion.y = std::numeric_limits::quiet_NaN(); + invalid_quaternion.z = 0.0; + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = std::numeric_limits::quiet_NaN(); + invalid_quaternion.y = 0.0; + invalid_quaternion.z = 0.0; + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = 0.0; + invalid_quaternion.y = 0.0; + invalid_quaternion.z = std::numeric_limits::quiet_NaN(); + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = 0.0; + invalid_quaternion.y = 0.0; + invalid_quaternion.z = 1.0; + invalid_quaternion.w = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); +} + +TEST(ValidateMessagesTest, PoseCheck) +{ + // Test valid Pose message + geometry_msgs::msg::Pose valid_pose; + valid_pose.position.x = 1.0; + valid_pose.position.y = 2.0; + valid_pose.position.z = 3.0; + valid_pose.orientation.x = 1.0; + valid_pose.orientation.y = 0.0; + valid_pose.orientation.z = 0.0; + valid_pose.orientation.w = 0.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_pose)); + // Test invalid Pose message with invalid position + geometry_msgs::msg::Pose invalid_pose; + invalid_pose.position.x = 1.0; + invalid_pose.position.y = std::numeric_limits::quiet_NaN(); + invalid_pose.position.z = 3.0; + invalid_pose.orientation.x = 1.0; + invalid_pose.orientation.y = 0.0; + invalid_pose.orientation.z = 0.0; + invalid_pose.orientation.w = 0.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_pose)); + // Test invalid Pose message with invalid orientation + invalid_pose.position.x = 1.0; + invalid_pose.position.y = 2.0; + invalid_pose.position.z = 3.0; + invalid_pose.orientation.x = 0.1; + invalid_pose.orientation.y = 0.2; + invalid_pose.orientation.z = 0.3; + invalid_pose.orientation.w = 0.4; + EXPECT_FALSE(nav2_util::validateMsg(invalid_pose)); +} + + +TEST(ValidateMessagesTest, MapMetaDataCheck) { + // Test valid MapMetaData message + nav_msgs::msg::MapMetaData valid_map_meta_data; + valid_map_meta_data.resolution = 0.05; + valid_map_meta_data.width = 100; + valid_map_meta_data.height = 100; + geometry_msgs::msg::Pose valid_origin; + valid_origin.position.x = 0.0; + valid_origin.position.y = 0.0; + valid_origin.position.z = 0.0; + valid_origin.orientation.x = 0.0; + valid_origin.orientation.y = 0.0; + valid_origin.orientation.z = 0.0; + valid_origin.orientation.w = 1.0; + valid_map_meta_data.origin = valid_origin; + EXPECT_TRUE(nav2_util::validateMsg(valid_map_meta_data)); + + // Test invalid origin message + nav_msgs::msg::MapMetaData invalid_map_meta_data; + invalid_map_meta_data.resolution = 100.0; + invalid_map_meta_data.width = 100; + invalid_map_meta_data.height = 100; + geometry_msgs::msg::Pose invalid_origin; + invalid_origin.position.x = 0.0; + invalid_origin.position.y = 0.0; + invalid_origin.position.z = 0.0; + invalid_origin.orientation.x = 0.0; + invalid_origin.orientation.y = 0.0; + invalid_origin.orientation.z = 1.0; + invalid_origin.orientation.w = 1.0; + invalid_map_meta_data.origin = invalid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); + + // Test invalid resolution message + invalid_map_meta_data.resolution = std::numeric_limits::quiet_NaN(); + invalid_map_meta_data.width = 100; + invalid_map_meta_data.height = 100; + invalid_map_meta_data.origin = valid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); + + // Test invalid MapMetaData message with zero width + invalid_map_meta_data.resolution = 0.05; + invalid_map_meta_data.width = 0; + invalid_map_meta_data.height = 100; + invalid_map_meta_data.origin = valid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); +} + +TEST(ValidateMessagesTest, OccupancyGridCheck) { + // Test valid OccupancyGrid message + nav_msgs::msg::OccupancyGrid valid_occupancy_grid; + valid_occupancy_grid.header.frame_id = "map"; + valid_occupancy_grid.info.resolution = 0.05; + valid_occupancy_grid.info.width = 100; + valid_occupancy_grid.info.height = 100; + std::vector data(100 * 100, 0); // Initialize with zeros + valid_occupancy_grid.data = data; + EXPECT_TRUE(nav2_util::validateMsg(valid_occupancy_grid)); + + // Test invalid header message with wrong data size + nav_msgs::msg::OccupancyGrid invalid_occupancy_grid; + invalid_occupancy_grid.header.frame_id = ""; // Incorrect id + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 100; + invalid_occupancy_grid.info.height = 100; + invalid_occupancy_grid.data = data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); + + // Test invalid info message with wrong data size + invalid_occupancy_grid.header.frame_id = "map"; + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 0; // Incorrect width + invalid_occupancy_grid.info.height = 100; + invalid_occupancy_grid.data = data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); + + // Test invalid OccupancyGrid message with wrong data size + invalid_occupancy_grid.header.frame_id = "map"; + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 100; + invalid_occupancy_grid.info.height = 100; + std::vector invalid_data(100 * 99, 0); // Incorrect data size + invalid_occupancy_grid.data = invalid_data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); +} + +// Add more test cases for other validateMsg functions if needed From be4760a36c02f168f3eb7e5c0151fbce1b76cd51 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 1 May 2024 01:02:24 +0200 Subject: [PATCH 16/35] nav2_waypoint_follower: Fix opencv includes (#4287) Signed-off-by: Silvio Traversaro --- .../nav2_waypoint_follower/plugins/photo_at_waypoint.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp index 9c46fdf1f86..1e6048aefca 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp @@ -32,8 +32,8 @@ #include "sensor_msgs/msg/image.hpp" #include "nav2_core/waypoint_task_executor.hpp" -#include "opencv4/opencv2/core.hpp" -#include "opencv4/opencv2/opencv.hpp" +#include "opencv2/core.hpp" +#include "opencv2/opencv.hpp" #include "cv_bridge/cv_bridge.hpp" #include "image_transport/image_transport.hpp" From de19a7b30f77f765b2244dbf849d389a8dc984cb Mon Sep 17 00:00:00 2001 From: Saitama Date: Thu, 2 May 2024 20:39:51 +0200 Subject: [PATCH 17/35] [RotationShimController] rotate also on short paths (#4290) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit When the path is shorter than ´forward_sampling_distance´ the rotatitonShimController would directly give control to the primary controller to navigate to the goal. This would lead to the exact behavior that was tried to be fixed by the rotationShimController: "The result is an awkward, stuttering, or whipping around behavior" [1]. It often resulted in navigation failure. In this case, the controller should try to rotate towards the last point of the path, so that the primary controller can have a better starting orientation. [1] https://navigation.ros.org/tuning/index.html#rotate-in-place-behavior --- .../src/nav2_rotation_shim_controller.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index afd32381459..5097a1fc9cd 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -195,10 +195,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() } } - throw nav2_core::ControllerException( - std::string( - "Unable to find a sampling point at least %0.2f from the robot," - "passing off to primary controller plugin.", forward_sampling_distance_)); + return current_path_.poses.back(); } geometry_msgs::msg::Pose From 329875ac042d45eb122a6d50cca1468d968fee02 Mon Sep 17 00:00:00 2001 From: Christian Clauss Date: Fri, 3 May 2024 18:41:44 +0200 Subject: [PATCH 18/35] ruff rule F601 - Dictionary key literal `'use_composition'` repeated (#4293) % `ruff check` ``` Error: nav2_system_tests/src/system/test_wrong_init_pose_launch.py:117:21: F601 Dictionary key literal `'use_composition'` repeated ``` % ` ruff rule F601` # multi-value-repeated-key-literal (F601) Derived from the **Pyflakes** linter. Fix is sometimes available. ## What it does Checks for dictionary literals that associate multiple values with the same key. ## Why is this bad? Dictionary keys should be unique. If a key is associated with multiple values, the earlier values will be overwritten. Including multiple values for the same key in a dictionary literal is likely a mistake. ## Example ```python foo = { "bar": 1, "baz": 2, "baz": 3, } foo["baz"] # 3 ``` Use instead: ```python foo = { "bar": 1, "baz": 2, } foo["baz"] # 2 ``` ## References - [Python documentation: Dictionaries](https://docs.python.org/3/tutorial/datastructures.html#dictionaries) Signed-off-by: Christian Clauss --- nav2_system_tests/src/system/test_wrong_init_pose_launch.py | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index d09cc549195..f42d276fcce 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -114,7 +114,6 @@ def generate_launch_description(): 'bt_xml_file': bt_navigator_xml, 'use_composition': 'False', 'autostart': 'True', - 'use_composition': 'False', }.items(), ), ] From 1e3a68b9f6d840a368f5ab365eba4660c9e696dd Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Sat, 4 May 2024 00:02:56 +0200 Subject: [PATCH 19/35] Add footprint clearing for static layer (#4282) * Add footprint clearing for static layer Signed-off-by: Tony Najjar * fix flckering --------- Signed-off-by: Tony Najjar --- .../include/nav2_costmap_2d/static_layer.hpp | 12 ++++++++ nav2_costmap_2d/plugins/static_layer.cpp | 30 ++++++++++++++++++- 2 files changed, 41 insertions(+), 1 deletion(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index dd1a65cef0c..967a6c3789a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -48,6 +48,7 @@ #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/footprint.hpp" namespace nav2_costmap_2d { @@ -160,6 +161,17 @@ class StaticLayer : public CostmapLayer rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); + std::vector transformed_footprint_; + bool footprint_clearing_enabled_; + /** + * @brief Clear costmap layer info below the robot's footprint + */ + void updateFootprint( + double robot_x, double robot_y, double robot_yaw, double * min_x, + double * min_y, + double * max_x, + double * max_y); + std::string global_frame_; ///< @brief The global frame for the costmap std::string map_frame_; /// @brief frame that map is located in diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index cc90b8409b1..729530d82b7 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -133,6 +133,7 @@ StaticLayer::getParameters() declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); declareParameter("map_topic", rclcpp::ParameterValue("")); + declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false)); auto node = node_.lock(); if (!node) { @@ -141,6 +142,7 @@ StaticLayer::getParameters() node->get_parameter(name_ + "." + "enabled", enabled_); node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); + node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); std::string private_map_topic, global_map_topic; node->get_parameter(name_ + "." + "map_topic", private_map_topic); node->get_parameter("map_topic", global_map_topic); @@ -333,7 +335,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u void StaticLayer::updateBounds( - double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x, + double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y) @@ -371,6 +373,24 @@ StaticLayer::updateBounds( *max_y = std::max(wy, *max_y); has_updated_data_ = false; + + updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void +StaticLayer::updateFootprint( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, + double * max_x, + double * max_y) +{ + if (!footprint_clearing_enabled_) {return;} + + transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); + + for (unsigned int i = 0; i < transformed_footprint_.size(); i++) { + touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); + } } void @@ -392,6 +412,10 @@ StaticLayer::updateCosts( return; } + if (footprint_clearing_enabled_) { + setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE); + } + if (!layered_costmap_->isRolling()) { // if not rolling, the layered costmap (master_grid) has same coordinates as this layer if (!use_maximum_) { @@ -474,6 +498,10 @@ StaticLayer::dynamicParametersCallback( has_updated_data_ = true; current_ = false; } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "footprint_clearing_enabled") { + footprint_clearing_enabled_ = parameter.as_bool(); + } } } result.successful = true; From 07fbc50edb75ee97b3650716227caa6925874e84 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 3 May 2024 15:26:57 -0700 Subject: [PATCH 20/35] Fix #4268 (#4296) Signed-off-by: Steve Macenski --- nav2_smac_planner/src/collision_checker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 7e0396bddcf..8b9e69aa778 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -101,7 +101,7 @@ bool GridCollisionChecker::inCollision( if (outsideRange(costmap_->getSizeInCellsX(), x) || outsideRange(costmap_->getSizeInCellsY(), y)) { - return false; + return true; } // Assumes setFootprint already set From d0a65d5afbab3b758408b79ebf5cd344b3377b00 Mon Sep 17 00:00:00 2001 From: Pradheep Krishna Date: Mon, 6 May 2024 19:21:15 +0200 Subject: [PATCH 21/35] fixes the issue when the decorator goes to idle state (#4300) Signed-off-by: PRP --- .../plugins/decorator/path_longer_on_approach.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index cb39a245579..eb401f6762a 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -62,12 +62,11 @@ inline BT::NodeStatus PathLongerOnApproach::tick() getInput("prox_len", prox_len_); getInput("length_factor", length_factor_); - if (!BT::isStatusActive(status())) { - // Reset the starting point since we're starting a new iteration of - // PathLongerOnApproach (moving from IDLE to RUNNING) - first_time_ = true; + if (first_time_ == false) { + if (old_path_.poses.back() != new_path_.poses.back()) { + first_time_ = true; + } } - setStatus(BT::NodeStatus::RUNNING); // Check if the path is updated and valid, compare the old and the new path length, From b6693c665146c3100019f0c92bfa34749a76b3ca Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 6 May 2024 15:52:19 -0700 Subject: [PATCH 22/35] Continuation of #4284 (#4295) * 64 bit for index Signed-off-by: Guillaume Doisy * Graph storing in uint64 * Remove incorrect usage --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy --- .../include/nav2_smac_planner/a_star.hpp | 6 ++--- .../nav2_smac_planner/analytic_expansion.hpp | 2 +- .../include/nav2_smac_planner/node_2d.hpp | 18 ++++++++------- .../include/nav2_smac_planner/node_basic.hpp | 5 +++-- .../include/nav2_smac_planner/node_hybrid.hpp | 22 +++++++++++-------- .../nav2_smac_planner/node_lattice.hpp | 12 +++++----- .../include/nav2_smac_planner/types.hpp | 2 +- nav2_smac_planner/src/a_star.cpp | 8 ++++--- nav2_smac_planner/src/analytic_expansion.cpp | 2 +- nav2_smac_planner/src/node_2d.cpp | 9 ++++---- nav2_smac_planner/src/node_hybrid.cpp | 13 ++++++----- nav2_smac_planner/src/node_lattice.cpp | 7 +++--- nav2_smac_planner/test/test_node2d.cpp | 6 +++-- nav2_smac_planner/test/test_nodehybrid.cpp | 6 +++-- nav2_smac_planner/test/test_nodelattice.cpp | 6 +++-- 15 files changed, 71 insertions(+), 53 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index b59fa9d66b6..56e5bbccafb 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -49,13 +49,13 @@ class AStarAlgorithm { public: typedef NodeT * NodePtr; - typedef robin_hood::unordered_node_map Graph; + typedef robin_hood::unordered_node_map Graph; typedef std::vector NodeVector; typedef std::pair> NodeElement; typedef typename NodeT::Coordinates Coordinates; typedef typename NodeT::CoordinateVector CoordinateVector; typedef typename NodeVector::iterator NeighborIterator; - typedef std::function NodeGetter; + typedef std::function NodeGetter; /** * @struct nav2_smac_planner::NodeComparator @@ -210,7 +210,7 @@ class AStarAlgorithm * @brief Adds node to graph * @param index Node index to add */ - inline NodePtr addToGraph(const unsigned int & index); + inline NodePtr addToGraph(const uint64_t & index); /** * @brief Check if this node is the goal node diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 9dc317dd8a4..76719d72313 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -35,7 +35,7 @@ class AnalyticExpansion public: typedef NodeT * NodePtr; typedef typename NodeT::Coordinates Coordinates; - typedef std::function NodeGetter; + typedef std::function NodeGetter; /** * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 10a975577a4..3ccadefdedb 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -62,7 +62,7 @@ class Node2D * @brief A constructor for nav2_smac_planner::Node2D * @param index The index of this node for self-reference */ - explicit Node2D(const unsigned int index); + explicit Node2D(const uint64_t index); /** * @brief A destructor for nav2_smac_planner::Node2D @@ -158,7 +158,7 @@ class Node2D * @brief Gets cell index * @return Reference to cell index */ - inline unsigned int getIndex() + inline uint64_t getIndex() { return _index; } @@ -185,10 +185,11 @@ class Node2D * @param width width of costmap * @return index */ - static inline unsigned int getIndex( + static inline uint64_t getIndex( const unsigned int & x, const unsigned int & y, const unsigned int & width) { - return x + y * width; + return static_cast(x) + static_cast(y) * + static_cast(width); } /** @@ -199,7 +200,7 @@ class Node2D * @return coordinates of point */ static inline Coordinates getCoords( - const unsigned int & index, const unsigned int & width, const unsigned int & angles) + const uint64_t & index, const unsigned int & width, const unsigned int & angles) { if (angles != 1) { throw std::runtime_error("Node type Node2D does not have a valid angle quantization."); @@ -213,7 +214,7 @@ class Node2D * @param Index Index of point * @return coordinates of point */ - static inline Coordinates getCoords(const unsigned int & index) + static inline Coordinates getCoords(const uint64_t & index) { const unsigned int & size_x = _neighbors_grid_offsets[3]; return Coordinates(index % size_x, index / size_x); @@ -253,7 +254,8 @@ class Node2D * @param neighbors Vector of neighbors to be filled */ void getNeighbors( - std::function & validity_checker, + std::function & validity_checker, GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors); @@ -272,7 +274,7 @@ class Node2D private: float _cell_cost; float _accumulated_cost; - unsigned int _index; + uint64_t _index; bool _was_visited; bool _is_queued; }; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp index 0fbace43604..dfb26c5ddaf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -49,7 +49,7 @@ class NodeBasic * @brief A constructor for nav2_smac_planner::NodeBasic * @param index The index of this node for self-reference */ - explicit NodeBasic(const unsigned int index) + explicit NodeBasic(const uint64_t index) : index(index), graph_node_ptr(nullptr) { @@ -73,7 +73,8 @@ class NodeBasic typename NodeT::Coordinates pose; // Used by NodeHybrid and NodeLattice NodeT * graph_node_ptr; MotionPrimitive * prim_ptr; // Used by NodeLattice - unsigned int index, motion_index; + uint64_t index; + unsigned int motion_index; bool backward; TurnDirection turn_dir; }; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index b6a33a444ae..b1e265e139b 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -40,7 +40,7 @@ namespace nav2_smac_planner typedef std::vector LookupTable; typedef std::pair TrigValues; -typedef std::pair ObstacleHeuristicElement; +typedef std::pair ObstacleHeuristicElement; struct ObstacleHeuristicComparator { bool operator()(const ObstacleHeuristicElement & a, const ObstacleHeuristicElement & b) const @@ -184,7 +184,7 @@ class NodeHybrid * @brief A constructor for nav2_smac_planner::NodeHybrid * @param index The index of this node for self-reference */ - explicit NodeHybrid(const unsigned int index); + explicit NodeHybrid(const uint64_t index); /** * @brief A destructor for nav2_smac_planner::NodeHybrid @@ -291,7 +291,7 @@ class NodeHybrid * @brief Gets cell index * @return Reference to cell index */ - inline unsigned int getIndex() + inline uint64_t getIndex() { return _index; } @@ -319,11 +319,14 @@ class NodeHybrid * @param angle_quantization Number of theta bins * @return Index */ - static inline unsigned int getIndex( + static inline uint64_t getIndex( const unsigned int & x, const unsigned int & y, const unsigned int & angle, const unsigned int & width, const unsigned int & angle_quantization) { - return angle + x * angle_quantization + y * width * angle_quantization; + return static_cast(angle) + static_cast(x) * + static_cast(angle_quantization) + + static_cast(y) * static_cast(width) * + static_cast(angle_quantization); } /** @@ -333,7 +336,7 @@ class NodeHybrid * @param angle Theta coordinate of point * @return Index */ - static inline unsigned int getIndex( + static inline uint64_t getIndex( const unsigned int & x, const unsigned int & y, const unsigned int & angle) { return getIndex( @@ -349,7 +352,7 @@ class NodeHybrid * @return Coordinates */ static inline Coordinates getCoords( - const unsigned int & index, + const uint64_t & index, const unsigned int & width, const unsigned int & angle_quantization) { return Coordinates( @@ -447,7 +450,8 @@ class NodeHybrid * @param neighbors Vector of neighbors to be filled */ void getNeighbors( - std::function & validity_checker, + std::function & validity_checker, GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors); @@ -478,7 +482,7 @@ class NodeHybrid private: float _cell_cost; float _accumulated_cost; - unsigned int _index; + uint64_t _index; bool _was_visited; unsigned int _motion_primitive_index; TurnDirection _turn_dir; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 99f50d6a92b..47363ab9eba 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -132,7 +132,7 @@ class NodeLattice * @brief A constructor for nav2_smac_planner::NodeLattice * @param index The index of this node for self-reference */ - explicit NodeLattice(const unsigned int index); + explicit NodeLattice(const uint64_t index); /** * @brief A destructor for nav2_smac_planner::NodeLattice @@ -229,7 +229,7 @@ class NodeLattice * @brief Gets cell index * @return Reference to cell index */ - inline unsigned int getIndex() + inline uint64_t getIndex() { return _index; } @@ -281,7 +281,7 @@ class NodeLattice * @param angle Theta coordinate of point * @return Index */ - static inline unsigned int getIndex( + static inline uint64_t getIndex( const unsigned int & x, const unsigned int & y, const unsigned int & angle) { // Hybrid-A* and State Lattice share a coordinate system @@ -298,7 +298,7 @@ class NodeLattice * @return Coordinates */ static inline Coordinates getCoords( - const unsigned int & index, + const uint64_t & index, const unsigned int & width, const unsigned int & angle_quantization) { // Hybrid-A* and State Lattice share a coordinate system @@ -396,7 +396,7 @@ class NodeLattice * @param neighbors Vector of neighbors to be filled */ void getNeighbors( - std::function & validity_checker, GridCollisionChecker * collision_checker, const bool & traverse_unknown, @@ -425,7 +425,7 @@ class NodeLattice private: float _cell_cost; float _accumulated_cost; - unsigned int _index; + uint64_t _index; bool _was_visited; MotionPrimitive * _motion_primitive; bool _backwards; diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index 6baf51f4a4d..18c2e1315f1 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -26,7 +26,7 @@ namespace nav2_smac_planner { -typedef std::pair NodeHeuristicPair; +typedef std::pair NodeHeuristicPair; /** * @struct nav2_smac_planner::SearchInfo diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 92a89230e92..8c8ec7197ff 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -120,7 +120,7 @@ void AStarAlgorithm::setCollisionChecker(GridCollisionChecker * collision template typename AStarAlgorithm::NodePtr AStarAlgorithm::addToGraph( - const unsigned int & index) + const uint64_t & index) { auto iter = _graph.find(index); if (iter != _graph.end()) { @@ -287,9 +287,11 @@ bool AStarAlgorithm::createPath( int closest_distance = std::numeric_limits::max(); // Given an index, return a node ptr reference if its collision-free and valid - const unsigned int max_index = getSizeX() * getSizeY() * getSizeDim3(); + const uint64_t max_index = static_cast(getSizeX()) * + static_cast(getSizeY()) * + static_cast(getSizeDim3()); NodeGetter neighborGetter = - [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool + [&, this](const uint64_t & index, NodePtr & neighbor_rtn) -> bool { if (index >= max_index) { return false; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 5f5f97d9ec7..f2667d3ba8e 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -201,7 +201,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion Node2D::_neighbors_grid_offsets; float Node2D::cost_travel_multiplier = 2.0; -Node2D::Node2D(const unsigned int index) +Node2D::Node2D(const uint64_t index) : parent(nullptr), _cell_cost(std::numeric_limits::quiet_NaN()), _accumulated_cost(std::numeric_limits::max()), @@ -108,7 +108,8 @@ void Node2D::initMotionModel( } void Node2D::getNeighbors( - std::function & NeighborGetter, + std::function & NeighborGetter, GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors) @@ -124,9 +125,9 @@ void Node2D::getNeighbors( // 100 100 100 where lower-middle '100' is visited with same cost by both bottom '50' nodes // Therefore, it is valuable to have some low-potential across the entire map // rather than a small inflation around the obstacles - int index; + uint64_t index; NodePtr neighbor; - int node_i = this->getIndex(); + uint64_t node_i = this->getIndex(); const Coordinates parent = getCoords(this->getIndex()); Coordinates child; diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 63f121de3dd..b18e8e559e5 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -338,7 +338,7 @@ float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) return bin_idx * bin_size; } -NodeHybrid::NodeHybrid(const unsigned int index) +NodeHybrid::NodeHybrid(const uint64_t index) : parent(nullptr), pose(0.0f, 0.0f, 0.0f), _cell_cost(std::numeric_limits::quiet_NaN()), @@ -468,7 +468,7 @@ void NodeHybrid::initMotionModel( } inline float distanceHeuristic2D( - const unsigned int idx, const unsigned int size_x, + const uint64_t idx, const unsigned int size_x, const unsigned int target_x, const unsigned int target_y) { int dx = static_cast(idx % size_x) - static_cast(target_x); @@ -611,8 +611,8 @@ float NodeHybrid::getObstacleHeuristic( const int size_x_int = static_cast(size_x); const float sqrt2 = sqrtf(2.0f); float c_cost, cost, travel_cost, new_cost, existing_cost; - unsigned int idx, mx, my; - unsigned int new_idx = 0; + unsigned int mx, my; + unsigned int idx, new_idx = 0; const std::vector neighborhood = {1, -1, // left right size_x_int, -size_x_int, // up down @@ -855,12 +855,13 @@ void NodeHybrid::precomputeDistanceHeuristic( } void NodeHybrid::getNeighbors( - std::function & NeighborGetter, + std::function & NeighborGetter, GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors) { - unsigned int index = 0; + uint64_t index = 0; NodePtr neighbor = nullptr; Coordinates initial_node_coords; const MotionPoses motion_projections = motion_table.getProjections(this); diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index c3303e0a113..c68a4e60e18 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -180,7 +180,7 @@ float & LatticeMotionTable::getAngleFromBin(const unsigned int & bin_idx) return lattice_metadata.heading_angles[bin_idx]; } -NodeLattice::NodeLattice(const unsigned int index) +NodeLattice::NodeLattice(const uint64_t index) : parent(nullptr), pose(0.0f, 0.0f, 0.0f), _cell_cost(std::numeric_limits::quiet_NaN()), @@ -469,12 +469,13 @@ void NodeLattice::precomputeDistanceHeuristic( } void NodeLattice::getNeighbors( - std::function & NeighborGetter, + std::function & NeighborGetter, GridCollisionChecker * collision_checker, const bool & traverse_unknown, NodeVector & neighbors) { - unsigned int index = 0; + uint64_t index = 0; float angle; bool backwards = false; NodePtr neighbor = nullptr; diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 36715948550..1326fb02136 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -142,8 +142,10 @@ TEST(Node2DTest, test_node_2d_neighbors) unsigned char cost = static_cast(1); nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1); node->setCost(cost); - std::function neighborGetter = - [&, this](const unsigned int & index, nav2_smac_planner::Node2D * & neighbor_rtn) -> bool + std::function neighborGetter = + [&, this](const uint64_t & index, + nav2_smac_planner::Node2D * & neighbor_rtn) -> bool { return false; }; diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index f6d917c08f1..f499ec8fb41 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -360,8 +360,10 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) std::make_unique(costmap_ros, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid(49); - std::function neighborGetter = - [&, this](const unsigned int & index, nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool + std::function neighborGetter = + [&, this](const uint64_t & index, + nav2_smac_planner::NodeHybrid * & neighbor_rtn) -> bool { // because we don't return a real object return false; diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp index b3eadab5f91..2355ac9a842 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -303,8 +303,10 @@ TEST(NodeLatticeTest, test_get_neighbors) std::make_unique(costmap_ros, 72, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); - std::function neighborGetter = - [&, this](const unsigned int & index, nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool + std::function neighborGetter = + [&, this](const uint64_t & index, + nav2_smac_planner::NodeLattice * & neighbor_rtn) -> bool { // because we don't return a real object return false; From 397f37f06f67ea9ab797bec23b729e16b4916e7c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Britto?= Date: Tue, 7 May 2024 17:23:36 +0200 Subject: [PATCH 23/35] Update README.md of nav2_bt_navigator (#4309) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Update link to docs Signed-off-by: João Britto --- nav2_bt_navigator/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md index 8dd51a6c6ec..48787a9473a 100644 --- a/nav2_bt_navigator/README.md +++ b/nav2_bt_navigator/README.md @@ -2,7 +2,7 @@ The BT Navigator (Behavior Tree Navigator) module implements the NavigateToPose and NavigateThroughPoses task interfaces. It is a [Behavior Tree](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/docs/BT_basics.md)-based implementation of navigation that is intended to allow for flexibility in the navigation task and provide a way to easily specify complex robot behaviors. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-navigator.html) for additional parameter descriptions, as well as the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-bt-navigator.html) for additional parameter descriptions, as well as the [Nav2 Behavior Tree Explanation](https://docs.nav2.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. ## Overview From 12c31c69666fc8e5642ca9db041be8f3faf47684 Mon Sep 17 00:00:00 2001 From: Pradheep Krishna Date: Tue, 7 May 2024 17:24:03 +0200 Subject: [PATCH 24/35] updating readme links to the new domain (#4311) Signed-off-by: Pradheep --- README.md | 20 +++++++++---------- nav2_amcl/README.md | 2 +- nav2_behavior_tree/README.md | 4 ++-- nav2_behaviors/README.md | 4 ++-- nav2_bringup/README.md | 2 +- nav2_collision_monitor/README.md | 4 ++-- nav2_constrained_smoother/README.md | 6 +++--- nav2_controller/README.md | 4 ++-- nav2_costmap_2d/README.md | 6 +++--- nav2_dwb_controller/README.md | 2 +- nav2_graceful_controller/README.md | 2 +- nav2_lifecycle_manager/README.md | 2 +- nav2_map_server/README.md | 2 +- nav2_navfn_planner/README.md | 2 +- nav2_planner/README.md | 4 ++-- .../README.md | 4 ++-- nav2_rotation_shim_controller/README.md | 2 +- nav2_simple_commander/README.md | 2 +- nav2_smac_planner/README.md | 2 +- nav2_smoother/README.md | 4 ++-- nav2_theta_star_planner/README.md | 2 +- nav2_velocity_smoother/README.md | 2 +- nav2_waypoint_follower/README.md | 2 +- 23 files changed, 43 insertions(+), 43 deletions(-) diff --git a/README.md b/README.md index 6df210791bd..abb5468b6f7 100644 --- a/README.md +++ b/README.md @@ -7,18 +7,18 @@

For detailed instructions on how to: -- [Getting Started](https://navigation.ros.org/getting_started/index.html) -- [Concepts](https://navigation.ros.org/concepts/index.html) -- [Build](https://navigation.ros.org/development_guides/build_docs/index.html#build) -- [Install](https://navigation.ros.org/development_guides/build_docs/index.html#install) -- [General Tutorials](https://navigation.ros.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://navigation.ros.org/plugin_tutorials/index.html) -- [Configure](https://navigation.ros.org/configuration/index.html) -- [Navigation Plugins](https://navigation.ros.org/plugins/index.html) -- [Migration Guides](https://navigation.ros.org/migration/index.html) +- [Getting Started](https://docs.nav2.org/getting_started/index.html) +- [Concepts](https://docs.nav2.org/concepts/index.html) +- [Build](https://docs.nav2.org/development_guides/build_docs/index.html#build) +- [Install](https://docs.nav2.org/development_guides/build_docs/index.html#install) +- [General Tutorials](https://docs.nav2.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://docs.nav2.org/plugin_tutorials/index.html) +- [Configure](https://docs.nav2.org/configuration/index.html) +- [Navigation Plugins](https://docs.nav2.org/plugins/index.html) +- [Migration Guides](https://docs.nav2.org/migration/index.html) - [Container Images for Building Nav2](https://github.com/orgs/ros-planning/packages/container/package/navigation2) -- [Contribute](https://navigation.ros.org/development_guides/involvement_docs/index.html) +- [Contribute](https://docs.nav2.org/development_guides/involvement_docs/index.html) -Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). +Please visit our [documentation site](https://docs.nav2.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). If you need professional services related to Nav2, please contact Open Navigation at info@opennav.org. diff --git a/nav2_amcl/README.md b/nav2_amcl/README.md index 7215500919a..5e8d737a760 100644 --- a/nav2_amcl/README.md +++ b/nav2_amcl/README.md @@ -1,4 +1,4 @@ # AMCL Adaptive Monte Carlo Localization (AMCL) is a probabilistic localization module which estimates the position and orientation (i.e. Pose) of a robot in a given known map using a 2D laser scanner. This is largely a refactored port from ROS 1 without any algorithmic changes. -See the [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-amcl.html) for more details about configurable settings and their meanings. +See the [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-amcl.html) for more details about configurable settings and their meanings. diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 0ade4e0a016..dab5b9a8a06 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -7,9 +7,9 @@ The nav2_behavior_tree module provides: * Navigation-specific behavior tree nodes, and * a generic BehaviorTreeEngine class that simplifies the integration of BT processing into ROS2 nodes for navigation or higher-level autonomy applications. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-xml.html) for additional parameter descriptions and a list of XML nodes made available in this package. Also review the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. A [tutorial](https://navigation.ros.org/plugin_tutorials/docs/writing_new_bt_plugin.html) is also provided to explain how to create a simple BT plugin. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-bt-xml.html) for additional parameter descriptions and a list of XML nodes made available in this package. Also review the [Nav2 Behavior Tree Explanation](https://docs.nav2.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. A [tutorial](https://docs.nav2.org/plugin_tutorials/docs/writing_new_bt_plugin.html) is also provided to explain how to create a simple BT plugin. -See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. ## The bt_action_node Template and the Behavior Tree Engine diff --git a/nav2_behaviors/README.md b/nav2_behaviors/README.md index fdc24a26b31..7c87ae91a37 100644 --- a/nav2_behaviors/README.md +++ b/nav2_behaviors/README.md @@ -10,9 +10,9 @@ The only required class a behavior must derive from is the `nav2_core/behavior.h The value of the centralized behavior server is to **share resources** amongst several behaviors that would otherwise be independent nodes. Subscriptions to TF, costmaps, and more can be quite heavy and add non-trivial compute costs to a robot system. By combining these independent behaviors into a single server, they may share these resources while retaining complete independence in execution and interface. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-behavior-server.html) for additional parameter descriptions and a [tutorial about writing behaviors](https://navigation.ros.org/plugin_tutorials/docs/writing_new_behavior_plugin.html). +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-behavior-server.html) for additional parameter descriptions and a [tutorial about writing behaviors](https://docs.nav2.org/plugin_tutorials/docs/writing_new_behavior_plugin.html). -See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. The `TimedBehavior` template makes use of a [nav2_util::TwistPublisher](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). The `AssistedTeleop` behavior makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). \ No newline at end of file diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index 16088452cb8..da45591352b 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -10,7 +10,7 @@ Dynamically composed bringup (based on [ROS2 Composition](https://docs.ros.org/ * Some discussions about performance improvement of composed bringup could be found here: https://discourse.ros.org/t/nav2-composition/22175. -To use, please see the Nav2 [Getting Started Page](https://navigation.ros.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://navigation.ros.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more. +To use, please see the Nav2 [Getting Started Page](https://docs.nav2.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://docs.nav2.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more. Note: * gazebo should be started with both libgazebo_ros_init.so and libgazebo_ros_factory.so to work correctly. diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 4f81b241b67..eca25192b6e 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -59,7 +59,7 @@ The following diagram is showing the high-level design of Collision Monitor modu ### Configuration -Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://navigation.ros.org/tutorials/docs/using_collision_monitor.html) pages. +Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://docs.nav2.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://docs.nav2.org/tutorials/docs/using_collision_monitor.html) pages. ### Metrics @@ -94,6 +94,6 @@ The zones around the robot and the data sources are the same as for the Collisio ### Configuration -Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html). +Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://docs.nav2.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html). The `CollisionMonitor` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). diff --git a/nav2_constrained_smoother/README.md b/nav2_constrained_smoother/README.md index 55482d5dcf9..72768ede25e 100644 --- a/nav2_constrained_smoother/README.md +++ b/nav2_constrained_smoother/README.md @@ -2,7 +2,7 @@ A smoother plugin for `nav2_smoother` based on the original deprecated smoother in `nav2_smac_planner` by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) and put into operational state by [**RoboTech Vision**](https://robotechvision.com/). Suitable for applications which need planned global path to be pushed away from obstacles and/or for Reeds-Shepp motion models. -See documentation on navigation.ros.org: https://navigation.ros.org/configuration/packages/configuring-constrained-smoother.html +See documentation on docs.nav2.org: https://docs.nav2.org/configuration/packages/configuring-constrained-smoother.html Example of configuration (see indoor_navigation package of this repo for a full launch configuration): @@ -26,13 +26,13 @@ smoother_server: w_cost: 0.015 # weight to steer robot away from collision and cost # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes) - # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification + # See the [docs page](https://docs.nav2.org/configuration/packages/configuring-constrained-smoother) for further clarification w_cost_cusp_multiplier: 3.0 # option to have higher weight during forward/reverse direction change which is often accompanied with dangerous rotations cusp_zone_length: 2.5 # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight equals w_cost*w_cost_cusp_multiplier) # Points in robot frame to grab costmap values from. Format: [x1, y1, weight1, x2, y2, weight2, ...] # IMPORTANT: Requires much higher number of iterations to actually improve the path. Uncomment only if you really need it (highly elongated/asymmetric robots) - # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification + # See the [docs page](https://docs.nav2.org/configuration/packages/configuring-constrained-smoother) for further clarification # cost_check_points: [-0.185, 0.0, 1.0] optimizer: diff --git a/nav2_controller/README.md b/nav2_controller/README.md index 98b76c33555..c718a4e56f7 100644 --- a/nav2_controller/README.md +++ b/nav2_controller/README.md @@ -4,8 +4,8 @@ The Nav2 Controller is a Task Server in Nav2 that implements the `nav2_msgs::act An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The nav2_controller package is designed to be loaded with multiple plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `controller` header file in `nav2_core` package. It also contains progress checkers and goal checker plugins to abstract out that logic from specific controller implementations. -See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available controller plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available controller plugins. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-controller-server.html) for additional parameter descriptions and a [tutorial about writing controller plugins](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html). +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-controller-server.html) for additional parameter descriptions and a [tutorial about writing controller plugins](https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html). The `ControllerServer` makes use of a [nav2_util::TwistPublisher](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). diff --git a/nav2_costmap_2d/README.md b/nav2_costmap_2d/README.md index b2eb9a2b94a..d6c60ea915f 100644 --- a/nav2_costmap_2d/README.md +++ b/nav2_costmap_2d/README.md @@ -2,9 +2,9 @@ The costmap_2d package is responsible for building a 2D costmap of the environment, consisting of several "layers" of data about the environment. It can be initialized via the map server or a local rolling window and updates the layers by taking observations from sensors. A plugin interface allows for the layers to be combined into the costmap and finally inflated via an inflation radius based on the robot footprint. The nav2 version of the costmap_2d package is mostly a direct ROS2 port of the ROS1 navigation stack version, with minimal noteable changes necessary due to support in ROS2. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-costmaps.html) for additional parameter descriptions for the costmap and its included plugins. The [tutorials](https://navigation.ros.org/tutorials/index.html) and [first-time setup guides](https://navigation.ros.org/setup_guides/index.html) also provide helpful context for working with the costmap 2D package and its layers. A [tutorial](https://navigation.ros.org/plugin_tutorials/docs/writing_new_costmap2d_plugin.html) is also provided to explain how to create costmap plugins. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-costmaps.html) for additional parameter descriptions for the costmap and its included plugins. The [tutorials](https://docs.nav2.org/tutorials/index.html) and [first-time setup guides](https://docs.nav2.org/setup_guides/index.html) also provide helpful context for working with the costmap 2D package and its layers. A [tutorial](https://docs.nav2.org/plugin_tutorials/docs/writing_new_costmap2d_plugin.html) is also provided to explain how to create costmap plugins. -See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. ## To visualize the voxels in RVIZ: - Make sure `publish_voxel_map` in `voxel_layer` param's scope is set to `True`. @@ -24,4 +24,4 @@ See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) ### Overview -Costmap Filters - is a costmap layer-based instrument which provides an ability to apply to map spatial-dependent raster features named as filter-masks. These features are used in plugin algorithms when filling costmaps in order to allow robots to change their trajectory, behavior or speed when a robot enters/leaves an area marked in a filter masks. Examples of costmap filters include keep-out/safety zones where robots will never enter, speed restriction areas, preferred lanes for robots moving in industries and warehouses. More information about design, architecture of the feature and how it works could be found on Nav2 website: https://navigation.ros.org. +Costmap Filters - is a costmap layer-based instrument which provides an ability to apply to map spatial-dependent raster features named as filter-masks. These features are used in plugin algorithms when filling costmaps in order to allow robots to change their trajectory, behavior or speed when a robot enters/leaves an area marked in a filter masks. Examples of costmap filters include keep-out/safety zones where robots will never enter, speed restriction areas, preferred lanes for robots moving in industries and warehouses. More information about design, architecture of the feature and how it works could be found on Nav2 website: https://docs.nav2.org. diff --git a/nav2_dwb_controller/README.md b/nav2_dwb_controller/README.md index b440f29a825..da572d6332d 100644 --- a/nav2_dwb_controller/README.md +++ b/nav2_dwb_controller/README.md @@ -10,7 +10,7 @@ DWB improves on DWA in a few major ways: It is possible to tune DWB to gain both DWA and base local planner behaviors, as well as expansions using new plugins for totally use-case specific behaviors. The current trajectory generator plugins work for omnidirectional and differential drive robots, though an ackermann generator would be trivial to add. The current critic plugins work for both circular and non-circular robots and include many of the cost functions needed to build a path tracking system with various attributes. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-dwb-controller.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-dwb-controller.html) for additional parameter descriptions. ## DWB Plugins diff --git a/nav2_graceful_controller/README.md b/nav2_graceful_controller/README.md index 9cb5a72ec70..5c3f8a39fb5 100644 --- a/nav2_graceful_controller/README.md +++ b/nav2_graceful_controller/README.md @@ -1,7 +1,7 @@ # Graceful Motion Controller The graceful motion controller implements a controller based on the works of Jong Jin Park in "Graceful Navigation for Mobile Robots in Dynamic and Uncertain Environments". (2016). In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-graceful-motion-controller.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-graceful-motion-controller.html) for additional parameter descriptions. ## Smooth control law The smooth control law is a pose-following kinematic control law that generates a smooth and confortable trajectory for the robot to follow. It is Lyapunov-based feedback control law made of three components: diff --git a/nav2_lifecycle_manager/README.md b/nav2_lifecycle_manager/README.md index 9a0905006ce..e4622119885 100644 --- a/nav2_lifecycle_manager/README.md +++ b/nav2_lifecycle_manager/README.md @@ -1,7 +1,7 @@ ### Background on lifecycle enabled nodes Using ROS2’s managed/lifecycle nodes feature allows the system startup to ensure that all required nodes have been instantiated correctly before they begin their execution. Using lifecycle nodes also allows nodes to be restarted or replaced on-line. More details about managed nodes can be found on [ROS2 Design website](https://design.ros2.org/articles/node_lifecycle.html). Several nodes in Nav2, such as map_server, planner_server, and controller_server, are lifecycle enabled. These nodes provide the required overrides of the lifecycle functions: ```on_configure()```, ```on_activate()```, ```on_deactivate()```, ```on_cleanup()```, ```on_shutdown()```, and ```on_error()```. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-lifecycle.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-lifecycle.html) for additional parameter descriptions. ### nav2_lifecycle_manager Nav2's lifecycle manager is used to change the states of the lifecycle nodes in order to achieve a controlled _startup_, _shutdown_, _reset_, _pause_, or _resume_ of the navigation stack. The lifecycle manager presents a ```lifecycle_manager/manage_nodes``` service, from which clients can invoke the startup, shutdown, reset, pause, or resume functions. Based on this service request, the lifecycle manager calls the necessary lifecycle services in the lifecycle managed nodes. Currently, the RVIZ panel uses this ```lifecycle_manager/manage_nodes``` service when user presses the buttons on the RVIZ panel (e.g.,startup, reset, shutdown, etc.), but it is meant to be called on bringup through a production system application. diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md index 94a6bd66af0..4e811a24277 100644 --- a/nav2_map_server/README.md +++ b/nav2_map_server/README.md @@ -3,7 +3,7 @@ The `Map Server` provides maps to the rest of the Nav2 system using both topic and service interfaces. Map server will expose maps on the node bringup, but can also change maps using a `load_map` service during run-time, as well as save maps using a `save_map` server. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-map-server.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-map-server.html) for additional parameter descriptions. ### Architecture diff --git a/nav2_navfn_planner/README.md b/nav2_navfn_planner/README.md index ad980d17657..8b4a07e051d 100644 --- a/nav2_navfn_planner/README.md +++ b/nav2_navfn_planner/README.md @@ -4,4 +4,4 @@ The NavfnPlanner is a global planner plugin for the Nav2 Planner server. It impl The `global_planner` package from ROS (1) is a refactor on NavFn to make it more easily understandable, but it lacks in run-time performance and introduces suboptimal behaviors. As NavFn has been extremely stable for about 10 years at the time of porting, the maintainers felt no compelling reason to port over another, largely equivalent (but poorer functioning) planner. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-navfn.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-navfn.html) for additional parameter descriptions. diff --git a/nav2_planner/README.md b/nav2_planner/README.md index 3a17c825d42..302fed50db1 100644 --- a/nav2_planner/README.md +++ b/nav2_planner/README.md @@ -4,6 +4,6 @@ The Nav2 planner is a Task Server in Nav2 that implements the `nav2_behavior_tre A planning module implementing the `nav2_behavior_tree::ComputePathToPose` interface is responsible for generating a feasible path given start and end robot poses. It loads a map of potential planner plugins to do the path generation in different user-defined situations. -See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available planner plugins. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-planner-server.html) for additional parameter descriptions and a [tutorial about writing planner plugins](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2planner_plugin.html). +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-planner-server.html) for additional parameter descriptions and a [tutorial about writing planner plugins](https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2planner_plugin.html). diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 57b820b16d5..acdd0eb1ed9 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -2,7 +2,7 @@ This is a controller (local trajectory planner) that implements a variant on the pure pursuit algorithm to track a path. This variant we call the Regulated Pure Pursuit Algorithm, due to its additional regulation terms on collision and linear speed. It also implements the basics behind the Adaptive Pure Pursuit algorithm to vary lookahead distances by current speed. It was developed by [Shrijit Singh](https://www.linkedin.com/in/shrijitsingh99/) and [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/) as part of the Nav2 working group. -Code based on a simplified version of this controller is referenced in the [Writing a New Nav2 Controller](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html) tutorial. +Code based on a simplified version of this controller is referenced in the [Writing a New Nav2 Controller](https://docs.nav2.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html) tutorial. This plugin implements the `nav2_core::Controller` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (`controller_server`). @@ -14,7 +14,7 @@ This controller has been measured to run at well over 1 kHz on a modern intel pr

-See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-regulated-pp.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-regulated-pp.html) for additional parameter descriptions. If you use the Regulated Pure Pursuit Controller algorithm or software from this repository, please cite this work in your papers! diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md index 9129e9af619..8858e20c4dd 100644 --- a/nav2_rotation_shim_controller/README.md +++ b/nav2_rotation_shim_controller/README.md @@ -23,7 +23,7 @@ This plugin implements the `nav2_core::Controller` interface allowing it to be u

-See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-rotation-shim-controller.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-rotation-shim-controller.html) for additional parameter descriptions. ## Configuration diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 339d986ed41..668fd9fe7c8 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -10,7 +10,7 @@ This was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41 ## API -See its [API Guide Page](https://navigation.ros.org/commander_api/index.html) for additional parameter descriptions. +See its [API Guide Page](https://docs.nav2.org/commander_api/index.html) for additional parameter descriptions. The methods provided by the basic navigator are shown below, with inputs and expected returns. If a server fails, it may throw an exception or return a `None` object, so please be sure to properly wrap your navigation calls in try/catch and check results for `None` type. diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 78adee940be..ebb25bb52be 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -17,7 +17,7 @@ We have users reporting using this on: - Vertical farming - Solar farms -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smac-planner.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-smac-planner.html) for additional parameter descriptions. ## Introduction diff --git a/nav2_smoother/README.md b/nav2_smoother/README.md index c612dba43cd..04498a0034d 100644 --- a/nav2_smoother/README.md +++ b/nav2_smoother/README.md @@ -4,8 +4,8 @@ The Nav2 smoother is a Task Server in Nav2 that implements the `nav2_behavior_tr A smoothing module implementing the `nav2_behavior_tree::SmoothPath` interface is responsible for improving path smoothness and/or quality, typically given an unsmoothed path from the planner module in `nav2_planner`. It loads a map of potential smoother plugins to do the path smoothing in different user-defined situations. -See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available smoother plugins. +See the [Navigation Plugin list](https://docs.nav2.org/plugins/index.html) for a list of the currently known and available smoother plugins. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions. This package contains the Simple Smoother and Savitzky-Golay Smoother plugins. diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index 831beebbb3f..0ac1fa9b7aa 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -1,7 +1,7 @@ # Nav2 Theta Star Planner The Theta Star Planner is a global planning plugin meant to be used with the Nav2 Planner Server. The `nav2_theta_star_planner` implements a highly optimized version of the Theta\* Planner (specifically the [Lazy Theta\* P variant](http://idm-lab.org/bib/abstracts/papers/aaai10b.pdf)) meant to plan any-angle paths using A\*. The planner supports differential-drive and omni-directional robots. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-thetastar.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-thetastar.html) for additional parameter descriptions. ## Features - The planner uses A\* search along with line of sight (LOS) checks to form any-angle paths thus avoiding zig-zag paths that may be present in the usual implementation of A\* diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md index a9554997e28..579b3a6737a 100644 --- a/nav2_velocity_smoother/README.md +++ b/nav2_velocity_smoother/README.md @@ -5,7 +5,7 @@ The aim of this package is to implement velocity, acceleration, and deadband smo It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some interpretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-velocity-smoother.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-velocity-smoother.html) for additional parameter descriptions. ## Features diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index 5d6a0498574..822314fbf39 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -2,7 +2,7 @@ The Nav2 waypoint follower is an example application of how to use the navigation action to complete some sort of orchestrated task. In this example, that task is to take a given set of waypoints and navigate to a set of positions in the order provided in the action request. The last waypoint in the waypoint array is the final position. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-waypoint-follower.html) for additional parameter descriptions. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-waypoint-follower.html) for additional parameter descriptions. The package exposes the `follow_waypoints` action server of type `nav2_msgs/FollowWaypoints`. It is given an array of waypoints to visit, gives feedback about the current index of waypoint it is processing, and returns a list of waypoints it was unable to complete. From f12544dd4aebb546de8c5117d8b003c4844ab136 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 7 May 2024 10:36:25 -0700 Subject: [PATCH 25/35] complete navigation.ros.org -> docs.nav2.org migration (#4313) --- .devcontainer/caddy/srv/nav2/index.md | 6 +++--- .github/PULL_REQUEST_TEMPLATE.md | 2 +- .github/mergify.yml | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.devcontainer/caddy/srv/nav2/index.md b/.devcontainer/caddy/srv/nav2/index.md index 60e2db1bfb6..1f8dc3d0fda 100644 --- a/.devcontainer/caddy/srv/nav2/index.md +++ b/.devcontainer/caddy/srv/nav2/index.md @@ -14,9 +14,9 @@ For more related documentation: -- [Nav2 Documentation](https://navigation.ros.org) - - [Development Guides](https://navigation.ros.org/development_guides) - - [Dev Containers](https://navigation.ros.org/development_guides/devcontainer_docs) +- [Nav2 Documentation](https://docs.nav2.org) + - [Development Guides](https://docs.nav2.org/development_guides) + - [Dev Containers](https://docs.nav2.org/development_guides/devcontainer_docs) ## Session Info diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index c1cfa2787f7..88c59fe4eda 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -38,7 +38,7 @@ --> #### For Maintainers: -- [ ] Check that any new parameters added are updated in navigation.ros.org +- [ ] Check that any new parameters added are updated in docs.nav2.org - [ ] Check that any significant change is added to the migration guide - [ ] Check that any new features **OR** changes to existing behaviors are reflected in the tuning guide - [ ] Check that any new functions have Doxygen added diff --git a/.github/mergify.yml b/.github/mergify.yml index 1c9acea3c36..a0427e60540 100644 --- a/.github/mergify.yml +++ b/.github/mergify.yml @@ -83,7 +83,7 @@ pull_request_rules: comment: message: | @{{author}}, please properly fill in PR template in the future. @stevemacenski, use this instead. - - [ ] Check that any new parameters added are updated in navigation.ros.org + - [ ] Check that any new parameters added are updated in docs.nav2.org - [ ] Check that any significant change is added to the migration guide - [ ] Check that any new features **OR** changes to existing behaviors are reflected in the tuning guide - [ ] Check that any new functions have Doxygen added From 59131785a1a32ae0c10a090c2b69d2e5b6b91c45 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Wed, 8 May 2024 16:22:56 +0200 Subject: [PATCH 26/35] Fix undefined symbols in `libpf_lib.so` (#4312) When I build `nav2_amcl` with `-Wl,--no-undefined` I noticed `libpf_lib.so` has undefined symbols. This PR correctly links `libpf_lib.so` to `libm` so all symbols can be found. You can verify this by executing the following command: ``` ldd -r ./build/nav2_amcl/src/pf/libpf_lib.so linux-vdso.so.1 (0x00007ffd1f8c0000) libc.so.6 => /lib/x86_64-linux-gnu/libc.so.6 (0x000074e909a00000) /lib64/ld-linux-x86-64.so.2 (0x000074e909e60000) undefined symbol: ceil (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: atan2 (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: sin (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: hypot (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: cos (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: log (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: sqrt (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: floor (./build/nav2_amcl/src/pf/libpf_lib.so) ``` Signed-off-by: Ramon Wijnands --- nav2_amcl/src/pf/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_amcl/src/pf/CMakeLists.txt b/nav2_amcl/src/pf/CMakeLists.txt index c6f16e6a7dc..3b4b2fa5ca1 100644 --- a/nav2_amcl/src/pf/CMakeLists.txt +++ b/nav2_amcl/src/pf/CMakeLists.txt @@ -15,6 +15,7 @@ target_include_directories(pf_lib PRIVATE ../include) if(HAVE_DRAND48) target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48") endif() +target_link_libraries(pf_lib m) install(TARGETS pf_lib From 7ef4473b266802f06736150fdac987ba7cb0e47a Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Wed, 8 May 2024 23:23:30 +0800 Subject: [PATCH 27/35] msg validation check for `/initialpose` in `nav2_amcl` (#4301) * add validation check for PoseWithCovarianceStamped Signed-off-by: goes * remove rebundant check before Signed-off-by: goes * reformat Signed-off-by: goes * typo fixed Signed-off-by: goes * change the type-name Signed-off-by: goes * update test Signed-off-by: goes * reformat Signed-off-by: goes * . Signed-off-by: goes * add comment Signed-off-by: goes * update comment Signed-off-by: goes * change header Signed-off-by: goes * update test Signed-off-by: goes * typo fixed Signed-off-by: goes --------- Signed-off-by: goes Co-authored-by: goes --- nav2_amcl/src/amcl_node.cpp | 7 +- .../include/nav2_util/validate_messages.hpp | 32 +++++ nav2_util/test/test_validation_messages.cpp | 121 ++++++++++++++++++ 3 files changed, 155 insertions(+), 5 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index ba5c83447fd..764a8f059fc 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -524,11 +524,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha RCLCPP_INFO(get_logger(), "initialPoseReceived"); - if (msg->header.frame_id == "") { - // This should be removed at some point - RCLCPP_WARN( - get_logger(), - "Received initial pose with empty frame_id. You should always supply a frame_id."); + if (!nav2_util::validateMsg(*msg)) { + RCLCPP_ERROR(get_logger(), "Received initialpose message is malformed. Rejecting."); return; } if (nav2_util::strip_leading_slash(msg->header.frame_id) != global_frame_id_) { diff --git a/nav2_util/include/nav2_util/validate_messages.hpp b/nav2_util/include/nav2_util/validate_messages.hpp index 7ce942be42d..58594f1ecd5 100644 --- a/nav2_util/include/nav2_util/validate_messages.hpp +++ b/nav2_util/include/nav2_util/validate_messages.hpp @@ -21,6 +21,7 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" // @brief Validation Check @@ -55,6 +56,20 @@ bool validateMsg(const double & num) return true; } +template +bool validateMsg(const std::array & msg) +{ + /* @brief value check for double-array + * like the field `covariance` used in the msg-type: + * geometry_msgs::msg::PoseWithCovarianceStamped + */ + for (const auto & element : msg) { + if (!validateMsg(element)) {return false;} + } + + return true; +} + const int NSEC_PER_SEC = 1e9; // 1 second = 1e9 nanosecond bool validateMsg(const builtin_interfaces::msg::Time & msg) { @@ -111,6 +126,23 @@ bool validateMsg(const geometry_msgs::msg::Pose & msg) return true; } +bool validateMsg(const geometry_msgs::msg::PoseWithCovariance & msg) +{ + // check sub-type + if (!validateMsg(msg.pose)) {return false;} + if (!validateMsg(msg.covariance)) {return false;} + + return true; +} + +bool validateMsg(const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +{ + // check sub-type + if (!validateMsg(msg.header)) {return false;} + if (!validateMsg(msg.pose)) {return false;} + return true; +} + // Function to verify map meta information bool validateMsg(const nav_msgs::msg::MapMetaData & msg) diff --git a/nav2_util/test/test_validation_messages.cpp b/nav2_util/test/test_validation_messages.cpp index 060ec11ede9..a7d8ed89041 100644 --- a/nav2_util/test/test_validation_messages.cpp +++ b/nav2_util/test/test_validation_messages.cpp @@ -244,4 +244,125 @@ TEST(ValidateMessagesTest, OccupancyGridCheck) { EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); } +TEST(ValidateMessagesTest, PoseWithCovarianceCheck) { + // Valid message + geometry_msgs::msg::PoseWithCovariance validate_msg; + validate_msg.covariance[0] = 0.25; + // assign other covariance values... + validate_msg.covariance[35] = 0.06853891909122467; + + validate_msg.pose.position.x = 0.50010401010515571; + validate_msg.pose.position.y = 1.7468730211257935; + validate_msg.pose.position.z = 0.0; + + validate_msg.pose.orientation.x = 0.9440542194053062; + validate_msg.pose.orientation.y = 0.0; + validate_msg.pose.orientation.z = 0.0; + validate_msg.pose.orientation.w = -0.32979028309372299; + + EXPECT_TRUE(nav2_util::validateMsg(validate_msg)); + + // Invalid messages + geometry_msgs::msg::PoseWithCovariance invalidate_msg1; + invalidate_msg1.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg1.covariance[7] = NAN; + invalidate_msg1.covariance[9] = NAN; + invalidate_msg1.covariance[35] = 0.06853891909122467; + + invalidate_msg1.pose.position.x = 0.50010401010515571; + invalidate_msg1.pose.position.y = 1.7468730211257935; + invalidate_msg1.pose.position.z = 0.0; + + invalidate_msg1.pose.orientation.x = 0.9440542194053062; + invalidate_msg1.pose.orientation.y = 0.0; + invalidate_msg1.pose.orientation.z = 0.0; + invalidate_msg1.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1)); + + geometry_msgs::msg::PoseWithCovariance invalidate_msg2; + invalidate_msg2.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg2.covariance[35] = 0.06853891909122467; + + invalidate_msg2.pose.position.x = NAN; + invalidate_msg2.pose.position.y = 1.7468730211257935; + invalidate_msg2.pose.position.z = 0.0; + + invalidate_msg2.pose.orientation.x = 0.9440542194053062; + invalidate_msg2.pose.orientation.y = 0.0; + invalidate_msg2.pose.orientation.z = 0.0; + invalidate_msg2.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2)); +} + +TEST(ValidateMessagesTest, PoseWithCovarianceStampedCheck) { + // Valid message + geometry_msgs::msg::PoseWithCovarianceStamped validate_msg; + validate_msg.header.frame_id = "map"; + validate_msg.header.stamp.sec = 1711029956; + validate_msg.header.stamp.nanosec = 146734875; + + validate_msg.pose.covariance[0] = 0.25; + // assign other covariance values... + validate_msg.pose.covariance[35] = 0.06853891909122467; + + validate_msg.pose.pose.position.x = 0.50010401010515571; + validate_msg.pose.pose.position.y = 1.7468730211257935; + validate_msg.pose.pose.position.z = 0.0; + + validate_msg.pose.pose.orientation.x = 0.9440542194053062; + validate_msg.pose.pose.orientation.y = 0.0; + validate_msg.pose.pose.orientation.z = 0.0; + validate_msg.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_TRUE(nav2_util::validateMsg(validate_msg)); + + // Invalid messages + geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg1; + invalidate_msg1.header.frame_id = "map"; + invalidate_msg1.header.stamp.sec = 1711029956; + invalidate_msg1.header.stamp.nanosec = 146734875; + + invalidate_msg1.pose.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg1.pose.covariance[7] = NAN; + invalidate_msg1.pose.covariance[9] = NAN; + invalidate_msg1.pose.covariance[35] = 0.06853891909122467; + + invalidate_msg1.pose.pose.position.x = 0.50010401010515571; + invalidate_msg1.pose.pose.position.y = 1.7468730211257935; + invalidate_msg1.pose.pose.position.z = 0.0; + + invalidate_msg1.pose.pose.orientation.x = 0.9440542194053062; + invalidate_msg1.pose.pose.orientation.y = 0.0; + invalidate_msg1.pose.pose.orientation.z = 0.0; + invalidate_msg1.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1)); + + geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg2; + invalidate_msg2.header.frame_id = ""; + invalidate_msg2.header.stamp.sec = 1711029956; + invalidate_msg2.header.stamp.nanosec = 146734875; + + invalidate_msg2.pose.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg2.pose.covariance[35] = 0.06853891909122467; + + invalidate_msg2.pose.pose.position.x = 0.50010401010515571; + invalidate_msg2.pose.pose.position.y = 1.7468730211257935; + invalidate_msg2.pose.pose.position.z = 0.0; + + invalidate_msg2.pose.pose.orientation.x = 0.9440542194053062; + invalidate_msg2.pose.pose.orientation.y = 0.0; + invalidate_msg2.pose.pose.orientation.z = 0.0; + invalidate_msg2.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2)); +} + + // Add more test cases for other validateMsg functions if needed From 30d3aff32986bd2b677967e8b86ea9b283d2b62f Mon Sep 17 00:00:00 2001 From: AzaelCicero Date: Fri, 10 May 2024 18:25:52 +0200 Subject: [PATCH 28/35] 4320: Changed precision of calculations of the HybridNode MotionTable::getClosestAngularBin. (#4324) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Krzysztof Pawełczyk Co-authored-by: Krzysztof Pawełczyk --- nav2_smac_planner/src/node_hybrid.cpp | 3 ++- nav2_smac_planner/test/test_nodehybrid.cpp | 15 ++++++++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index b18e8e559e5..f5eec4ec5d2 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -330,7 +330,8 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) { - return static_cast(floor(static_cast(theta) / bin_size)); + return static_cast(floor(theta / static_cast(bin_size))) % + num_angle_quantization; } float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index f499ec8fb41..856f37c0a11 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -13,6 +13,7 @@ // limitations under the License. Reserved. #include +#include #include #include #include @@ -384,6 +385,7 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = 3.1415926; + motion_table.num_angle_quantization = 2; double test_theta = 3.1415926; unsigned int expected_angular_bin = 1; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); @@ -392,17 +394,28 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = M_PI; + motion_table.num_angle_quantization = 2; double test_theta = M_PI; - unsigned int expected_angular_bin = 1; + unsigned int expected_angular_bin = 0; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); } { motion_table.bin_size = M_PI; + motion_table.num_angle_quantization = 2; float test_theta = M_PI; unsigned int expected_angular_bin = 1; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); } + + { + motion_table.bin_size = 0.0872664675; + motion_table.num_angle_quantization = 72; + double test_theta = 6.28318526567925; + unsigned int expected_angular_bin = 71; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } } From 69cfc1cf87563259e65bcd7d49afa6e3dc5048fd Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 10 May 2024 12:40:23 -0700 Subject: [PATCH 29/35] Update docker workflow readme badge URL Signed-off-by: Steve Macenski --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index abb5468b6f7..577f354cef1 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # Nav2 -[![GitHub Workflow Status](https://github.com/ros-planning/navigation2/actions/workflows/update_ci_image.yaml/badge.svg)](https://github.com/ros-planning/navigation2/actions/workflows/update_ci_image.yaml) +[![GitHub Workflow Status](https://github.com/ros-navigation/navigation2/actions/workflows/update_ci_image.yaml/badge.svg)](https://github.com/ros-navigation/navigation2/actions/workflows/update_ci_image.yaml) [![codecov](https://codecov.io/gh/ros-planning/navigation2/branch/main/graph/badge.svg?token=S3iRmypwlg)](https://codecov.io/gh/ros-planning/navigation2)

From ade0061d192a809dc905ce588844cba19b573a98 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 10 May 2024 12:41:52 -0700 Subject: [PATCH 30/35] Update CircleCI badge on readme table Signed-off-by: Steve Macenski --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 577f354cef1..15a18c39375 100644 --- a/README.md +++ b/README.md @@ -117,7 +117,7 @@ If you use the Regulated Pure Pursuit Controller algorithm or software from this | Service | Humble | Iron | Main | | :---: | :---: | :---: | :---: | | ROS Build Farm | [![Build Status](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Hdev__navigation2__ubuntu_jammy_amd64/) | [![Build Status](https://build.ros2.org/job/Idev__navigation2__ubuntu_jammy_amd64/badge/icon)](https://build.ros2.org/job/Idev__navigation2__ubuntu_jammy_amd64/) | N/A | -| Circle CI | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-planning/navigation2/tree/main.svg?style=svg)](https://circleci.com/gh/ros-planning/navigation2/tree/main) | +| Circle CI | N/A | N/A | [![Build Status](https://circleci.com/gh/ros-navigation/navigation2/tree/main.svg?style=svg)](https://circleci.com/gh/ros-navigation/navigation2/tree/main) | | Package | Humble Source | Humble Debian | Iron Source | Iron Debian | From 4fa12acec79a08f472d9d04ac08997db47f2a398 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 10 May 2024 12:47:31 -0700 Subject: [PATCH 31/35] Update README.md Signed-off-by: Steve Macenski --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 15a18c39375..ab34c01ce3d 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Nav2 [![GitHub Workflow Status](https://github.com/ros-navigation/navigation2/actions/workflows/update_ci_image.yaml/badge.svg)](https://github.com/ros-navigation/navigation2/actions/workflows/update_ci_image.yaml) -[![codecov](https://codecov.io/gh/ros-planning/navigation2/branch/main/graph/badge.svg?token=S3iRmypwlg)](https://codecov.io/gh/ros-planning/navigation2) +[![codecov](https://codecov.io/gh/ros-navigation/navigation2/branch/main/graph/badge.svg?token=S3iRmypwlg)](https://codecov.io/gh/ros-navigation/navigation2)

From 2ae649b6b6ca92251472246d89a725d05e2cdfaa Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 14 May 2024 22:43:15 +0100 Subject: [PATCH 32/35] [LifecycleNode] add bond_heartbeat_period (#4342) * add bond_heartbeat_period Signed-off-by: Guillaume Doisy * lint Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy --- .../include/nav2_util/lifecycle_node.hpp | 1 + nav2_util/src/lifecycle_node.cpp | 31 ++++++++++++------- 2 files changed, 21 insertions(+), 11 deletions(-) diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp index 6da348c5f1a..7f1ef071570 100644 --- a/nav2_util/include/nav2_util/lifecycle_node.hpp +++ b/nav2_util/include/nav2_util/lifecycle_node.hpp @@ -205,6 +205,7 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode // Connection to tell that server is still up std::unique_ptr bond_{nullptr}; + double bond_heartbeat_period; }; } // namespace nav2_util diff --git a/nav2_util/src/lifecycle_node.cpp b/nav2_util/src/lifecycle_node.cpp index e09ae54b5d6..5976d098a86 100644 --- a/nav2_util/src/lifecycle_node.cpp +++ b/nav2_util/src/lifecycle_node.cpp @@ -19,6 +19,7 @@ #include #include "lifecycle_msgs/msg/state.hpp" +#include "nav2_util/node_utils.hpp" namespace nav2_util { @@ -35,6 +36,10 @@ LifecycleNode::LifecycleNode( rclcpp::Parameter( bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true)); + nav2_util::declare_parameter_if_not_declared( + this, "bond_heartbeat_period", rclcpp::ParameterValue(0.1)); + this->get_parameter("bond_heartbeat_period", bond_heartbeat_period); + printLifecycleNodeNotification(); register_rcl_preshutdown_callback(); @@ -55,16 +60,18 @@ LifecycleNode::~LifecycleNode() void LifecycleNode::createBond() { - RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name()); + if (bond_heartbeat_period > 0.0) { + RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", this->get_name()); - bond_ = std::make_unique( - std::string("bond"), - this->get_name(), - shared_from_this()); + bond_ = std::make_unique( + std::string("bond"), + this->get_name(), + shared_from_this()); - bond_->setHeartbeatPeriod(0.10); - bond_->setHeartbeatTimeout(4.0); - bond_->start(); + bond_->setHeartbeatPeriod(bond_heartbeat_period); + bond_->setHeartbeatTimeout(4.0); + bond_->start(); + } } void LifecycleNode::runCleanups() @@ -110,10 +117,12 @@ void LifecycleNode::register_rcl_preshutdown_callback() void LifecycleNode::destroyBond() { - RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name()); + if (bond_heartbeat_period > 0.0) { + RCLCPP_INFO(get_logger(), "Destroying bond (%s) to lifecycle manager.", this->get_name()); - if (bond_) { - bond_.reset(); + if (bond_) { + bond_.reset(); + } } } From 7b3dac8be775a8941cae15592c9de67c94913240 Mon Sep 17 00:00:00 2001 From: StetroF <120172218+StetroF@users.noreply.github.com> Date: Wed, 15 May 2024 23:48:48 +0800 Subject: [PATCH 33/35] Update path_longer_on_approach.cpp (#4344) Fix the bug that isPathUpdated function will return false for the reason that it compare the timestaped between new path and old path's last pose Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com> --- .../plugins/decorator/path_longer_on_approach.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index eb401f6762a..1b3f8abd564 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -36,7 +36,7 @@ bool PathLongerOnApproach::isPathUpdated( { return new_path != old_path && old_path.poses.size() != 0 && new_path.poses.size() != 0 && - old_path.poses.back() == new_path.poses.back(); + old_path.poses.back().pose == new_path.poses.back().pose; } bool PathLongerOnApproach::isRobotInGoalProximity( From 5acdb4f6bfbf33ff00308fe76f56aad62e1e8b5c Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 15 May 2024 16:51:04 +0100 Subject: [PATCH 34/35] [LifecycleManagerClient] clean set_initial_pose and navigate_to_pose (#4346) Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy --- .../lifecycle_manager_client.hpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index c5f2be8bb62..076e848902c 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -84,23 +84,6 @@ class LifecycleManagerClient */ SystemStatus is_active(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - // A couple convenience methods to facilitate scripting tests - /** - * @brief Set initial pose with covariance - * @param x X position - * @param y Y position - * @param theta orientation - */ - void set_initial_pose(double x, double y, double theta); - /** - * @brief Send goal pose to NavigationToPose action server - * @param x X position - * @param y Y position - * @param theta orientation - * @return true or false - */ - bool navigate_to_pose(double x, double y, double theta); - protected: using ManageLifecycleNodes = nav2_msgs::srv::ManageLifecycleNodes; From bf291d704dbb1c2572575dd0620b12f816407233 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 16 May 2024 14:14:28 -0700 Subject: [PATCH 35/35] Remove flaky spin test that needs to be rewritten (#4348) * remove spin test that is failing reliably * cont. --- .../spin/test_spin_behavior_node.cpp | 66 +++++++++---------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp index 6838e012219..cf0f97021db 100644 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp @@ -58,27 +58,27 @@ class SpinBehaviorTestFixture SpinBehaviorTester * SpinBehaviorTestFixture::spin_recovery_tester = nullptr; -TEST_P(SpinBehaviorTestFixture, testSpinRecovery) -{ - float target_yaw = std::get<0>(GetParam()); - float tolerance = std::get<1>(GetParam()); - - bool success = false; - int num_tries = 3; - for (int i = 0; i != num_tries; i++) { - success = success || spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance); - if (success) { - break; - } - } - if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(target_yaw) > M_PI_2f32) { - // if this variable is set, make a fake costmap - // in the fake spin test, we expect a collision for angles > M_PI_2 - EXPECT_EQ(false, success); - } else { - EXPECT_EQ(true, success); - } -} +// TEST_P(SpinBehaviorTestFixture, testSpinRecovery) +// { +// float target_yaw = std::get<0>(GetParam()); +// float tolerance = std::get<1>(GetParam()); + +// bool success = false; +// int num_tries = 3; +// for (int i = 0; i != num_tries; i++) { +// success = success || spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance); +// if (success) { +// break; +// } +// } +// if (std::getenv("MAKE_FAKE_COSTMAP") != NULL && abs(target_yaw) > M_PI_2f32) { +// // if this variable is set, make a fake costmap +// // in the fake spin test, we expect a collision for angles > M_PI_2 +// EXPECT_EQ(false, success); +// } else { +// EXPECT_EQ(true, success); +// } +// } TEST_F(SpinBehaviorTestFixture, testSpinPreemption) { @@ -114,18 +114,18 @@ TEST_F(SpinBehaviorTestFixture, testSpinCancel) EXPECT_EQ(false, success); } -INSTANTIATE_TEST_SUITE_P( - SpinRecoveryTests, - SpinBehaviorTestFixture, - ::testing::Values( - std::make_tuple(-M_PIf32 / 6.0, 0.1), - // std::make_tuple(M_PI_4f32, 0.1), - // std::make_tuple(-M_PI_2f32, 0.1), - std::make_tuple(M_PIf32, 0.1), - std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15), - std::make_tuple(-2.0 * M_PIf32, 0.1), - std::make_tuple(4.0 * M_PIf32, 0.15)), - testNameGenerator); +// INSTANTIATE_TEST_SUITE_P( +// SpinRecoveryTests, +// SpinBehaviorTestFixture, +// ::testing::Values( +// std::make_tuple(-M_PIf32 / 6.0, 0.1), +// // std::make_tuple(M_PI_4f32, 0.1), +// // std::make_tuple(-M_PI_2f32, 0.1), +// std::make_tuple(M_PIf32, 0.1), +// std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15), +// std::make_tuple(-2.0 * M_PIf32, 0.1), +// std::make_tuple(4.0 * M_PIf32, 0.15)), +// testNameGenerator); int main(int argc, char ** argv) {