From 9e90417dd57b27c99c29b849321ad90205ca5523 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Mon, 5 Aug 2024 17:27:50 -0400 Subject: [PATCH 1/8] Switch nav2_planner to modern CMake idioms. (#4592) Signed-off-by: Chris Lalancette --- nav2_planner/CMakeLists.txt | 97 ++++++++++--------- .../include/nav2_planner/planner_server.hpp | 1 - nav2_planner/package.xml | 23 ++--- nav2_planner/src/planner_server.cpp | 1 - nav2_planner/test/CMakeLists.txt | 6 +- nav2_planner/test/test_dynamic_parameters.cpp | 4 +- 6 files changed, 68 insertions(+), 64 deletions(-) diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt index 197150837c..a5bce30e77 100644 --- a/nav2_planner/CMakeLists.txt +++ b/nav2_planner/CMakeLists.txt @@ -2,71 +2,62 @@ cmake_minimum_required(VERSION 3.5) project(nav2_planner) find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(lifecycle_msgs REQUIRED) find_package(nav2_common REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(std_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(nav2_util REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(nav2_costmap_2d REQUIRED) find_package(pluginlib REQUIRED) -find_package(nav2_core REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) nav2_package() -include_directories( - include -) - set(executable_name planner_server) set(library_name ${executable_name}_core) -set(dependencies - rclcpp - rclcpp_action - rclcpp_lifecycle - rclcpp_components - std_msgs - visualization_msgs - nav2_util - nav2_msgs - nav_msgs - geometry_msgs - builtin_interfaces - tf2_ros - nav2_costmap_2d - pluginlib - nav2_core -) - add_library(${library_name} SHARED src/planner_server.cpp ) - -ament_target_dependencies(${library_name} - ${dependencies} +target_include_directories(${library_name} + PUBLIC + "$" + "$" +) +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + tf2_ros::tf2_ros +) +target_link_libraries(${library_name} PRIVATE + ${lifecycle_msgs_TARGETS} + rclcpp_components::component + tf2::tf2 ) add_executable(${executable_name} src/main.cpp ) - -target_link_libraries(${executable_name} ${library_name}) - -ament_target_dependencies(${executable_name} - ${dependencies} -) +target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp) rclcpp_components_register_nodes(${library_name} "nav2_planner::PlannerServer") install(TARGETS ${library_name} + EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -77,17 +68,31 @@ install(TARGETS ${executable_name} ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) ament_lint_auto_find_test_dependencies() + + ament_find_gtest() add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${library_name}) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + geometry_msgs + nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + tf2_ros +) +ament_export_targets(${library_name}) ament_package() diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index bdffda281a..12e364c710 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -31,7 +31,6 @@ #include "nav2_msgs/msg/costmap.hpp" #include "nav2_util/robot_utils.hpp" #include "nav2_util/simple_action_server.hpp" -#include "visualization_msgs/msg/marker.hpp" #include "tf2_ros/transform_listener.h" #include "tf2_ros/create_timer_ros.h" #include "nav2_costmap_2d/costmap_2d_ros.hpp" diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 397dd8ae1d..26de6cfa13 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -8,22 +8,23 @@ Apache-2.0 ament_cmake + nav2_common - rclcpp - rclcpp_action - rclcpp_lifecycle - visualization_msgs - nav2_util - nav2_msgs - nav_msgs geometry_msgs - builtin_interfaces - nav2_common - tf2_ros + lifecycle_msgs + nav2_core nav2_costmap_2d + nav2_msgs + nav2_util + nav_msgs pluginlib - nav2_core + rclcpp + rclcpp_components + rclcpp_lifecycle + tf2 + tf2_ros + ament_cmake_gtest ament_lint_common ament_lint_auto diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 6abeddd29b..d572360fc9 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -24,7 +24,6 @@ #include #include -#include "builtin_interfaces/msg/duration.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "nav2_util/costmap.hpp" #include "nav2_util/node_utils.hpp" diff --git a/nav2_planner/test/CMakeLists.txt b/nav2_planner/test/CMakeLists.txt index d415d906ef..a551d1208f 100644 --- a/nav2_planner/test/CMakeLists.txt +++ b/nav2_planner/test/CMakeLists.txt @@ -2,9 +2,9 @@ ament_add_gtest(test_dynamic_parameters test_dynamic_parameters.cpp ) -ament_target_dependencies(test_dynamic_parameters - ${dependencies} -) target_link_libraries(test_dynamic_parameters ${library_name} + nav2_util::nav2_util_core + rclcpp::rclcpp + ${rcl_interfaces_TARGETS} ) diff --git a/nav2_planner/test/test_dynamic_parameters.cpp b/nav2_planner/test/test_dynamic_parameters.cpp index 8215e80e43..3dc3ab2b55 100644 --- a/nav2_planner/test/test_dynamic_parameters.cpp +++ b/nav2_planner/test/test_dynamic_parameters.cpp @@ -12,8 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include - +#include #include #include #include @@ -22,6 +21,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_planner/planner_server.hpp" #include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" class PlannerShim : public nav2_planner::PlannerServer { From 44eec9147a82328ddd5d0ea0aec746b256a82e54 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 6 Aug 2024 12:28:15 -0400 Subject: [PATCH 2/8] Switch nav2_regulated_pure_pursuit_controller to modern CMake idioms. (#4597) Signed-off-by: Chris Lalancette --- .../CMakeLists.txt | 85 ++++++++++++------- .../package.xml | 14 +-- .../test/CMakeLists.txt | 17 +++- 3 files changed, 75 insertions(+), 41 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index e0340a131c..91364c36bc 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -3,57 +3,61 @@ project(nav2_regulated_pure_pursuit_controller) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_util REQUIRED) -find_package(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) nav2_package() -set(CMAKE_CXX_STANDARD 17) - -include_directories( - include -) - -set(dependencies - rclcpp - geometry_msgs - nav2_costmap_2d - pluginlib - nav_msgs - angles - nav2_util - nav2_core - tf2 - tf2_geometry_msgs -) set(library_name nav2_regulated_pure_pursuit_controller) add_library(${library_name} SHARED - src/regulated_pure_pursuit_controller.cpp - src/collision_checker.cpp - src/parameter_handler.cpp - src/path_handler.cpp) - -ament_target_dependencies(${library_name} - ${dependencies} + src/regulated_pure_pursuit_controller.cpp + src/collision_checker.cpp + src/parameter_handler.cpp + src/path_handler.cpp) +target_include_directories(${library_name} + PUBLIC + "$" + "$" +) +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + ${std_msgs_TARGETS} + tf2::tf2 + tf2_ros::tf2_ros +) +target_link_libraries(${library_name} PRIVATE + angles::angles ) install(TARGETS ${library_name} + EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -61,14 +65,31 @@ if(BUILD_TESTING) # the following line skips the linter which checks for copyrights set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + find_package(tf2_geometry_msgs REQUIRED) + + ament_find_gtest() add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${library_name}) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + geometry_msgs + nav2_core + nav2_costmap_2d + nav2_util + nav_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcl_interfaces + std_msgs + tf2 + tf2_ros +) +ament_export_targets(${library_name}) pluginlib_export_plugin_description_file(nav2_core nav2_regulated_pure_pursuit_controller.xml) ament_package() - diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index e690351e19..ff4105b62e 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -9,22 +9,26 @@ Apache-2.0 ament_cmake + nav2_common angles - nav2_common + geometry_msgs nav2_core - nav2_util nav2_costmap_2d - rclcpp - geometry_msgs nav2_msgs + nav2_util pluginlib + rclcpp + rclcpp_lifecycle + rcl_interfaces + std_msgs tf2 - tf2_geometry_msgs + tf2_ros ament_cmake_gtest ament_lint_common ament_lint_auto + tf2_geometry_msgs ament_cmake diff --git a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt index 3e8d4c5b0a..81ccfc4d28 100644 --- a/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt @@ -3,13 +3,22 @@ ament_add_gtest(test_regulated_pp test_regulated_pp.cpp path_utils/path_utils.cpp ) -ament_target_dependencies(test_regulated_pp - ${dependencies} -) target_link_libraries(test_regulated_pp ${library_name} + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Path utils test ament_add_gtest(test_path_utils path_utils/test_path_utils.cpp path_utils/path_utils.cpp) -ament_target_dependencies(test_path_utils nav_msgs geometry_msgs tf2_geometry_msgs) +target_link_libraries(test_path_utils + ${nav_msgs_TARGETS} + ${geometry_msgs_TARGETS} + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs +) From 4d5b902068bebe156d0468016b6c9a18ea1c96c8 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 6 Aug 2024 16:53:42 -0400 Subject: [PATCH 3/8] Switch nav2_rotation_shim_controller to modern CMake idioms. (#4599) Signed-off-by: Chris Lalancette --- nav2_rotation_shim_controller/CMakeLists.txt | 69 +++++++++++-------- .../nav2_rotation_shim_controller.hpp | 4 -- nav2_rotation_shim_controller/package.xml | 13 ++-- .../src/nav2_rotation_shim_controller.cpp | 5 ++ .../test/CMakeLists.txt | 9 +-- 5 files changed, 60 insertions(+), 40 deletions(-) diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt index 2e141d5f44..fafa98863c 100644 --- a/nav2_rotation_shim_controller/CMakeLists.txt +++ b/nav2_rotation_shim_controller/CMakeLists.txt @@ -2,53 +2,56 @@ cmake_minimum_required(VERSION 3.5) project(nav2_rotation_shim_controller) find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_util REQUIRED) -find_package(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(tf2 REQUIRED) -find_package(angles REQUIRED) +find_package(tf2_ros REQUIRED) nav2_package() -set(CMAKE_CXX_STANDARD 17) - -include_directories( - include -) - -set(dependencies - rclcpp - geometry_msgs - nav2_costmap_2d - pluginlib - nav_msgs - nav2_util - nav2_core - tf2 - angles -) set(library_name nav2_rotation_shim_controller) add_library(${library_name} SHARED - src/nav2_rotation_shim_controller.cpp) - -ament_target_dependencies(${library_name} - ${dependencies} + src/nav2_rotation_shim_controller.cpp) +target_include_directories(${library_name} + PUBLIC + "$" + "$" +) +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + tf2_ros::tf2_ros +) +target_link_libraries(${library_name} PRIVATE + angles::angles + nav2_util::nav2_util_core + tf2::tf2 ) install(TARGETS ${library_name} + EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -56,12 +59,24 @@ if(BUILD_TESTING) # the following line skips the linter which checks for copyrights set(ament_cmake_copyright_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + ament_find_gtest() add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${library_name}) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + geometry_msgs + nav2_core + nav2_costmap_2d + pluginlib + rclcpp + rclcpp_lifecycle + rcl_interfaces + tf2_ros +) +ament_export_targets(${library_name}) pluginlib_export_plugin_description_file(nav2_core nav2_rotation_shim_controller.xml) diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index 6d4e8b86b3..f0da5e0fba 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -24,13 +24,9 @@ #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav2_util/geometry_utils.hpp" -#include "nav2_util/robot_utils.hpp" #include "nav2_core/controller.hpp" #include "nav2_core/controller_exceptions.hpp" -#include "nav2_util/node_utils.hpp" #include "nav2_costmap_2d/footprint_collision_checker.hpp" -#include "angles/angles.h" namespace nav2_rotation_shim_controller { diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 1fadbbe2c5..9e450f7573 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -8,17 +8,20 @@ Apache-2.0 ament_cmake + nav2_common - nav2_common + angles + geometry_msgs nav2_core - nav2_util nav2_costmap_2d - rclcpp - geometry_msgs nav2_msgs - angles + nav2_util pluginlib + rclcpp + rclcpp_lifecycle + rcl_interfaces tf2 + tf2_ros ament_cmake_gtest ament_lint_common 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 afb74c357e..4e111edfda 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -18,6 +18,11 @@ #include #include +#include "angles/angles.h" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" + #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp" #include "nav2_rotation_shim_controller/tools/utils.hpp" diff --git a/nav2_rotation_shim_controller/test/CMakeLists.txt b/nav2_rotation_shim_controller/test/CMakeLists.txt index ae3161d1a7..e2d79806a3 100644 --- a/nav2_rotation_shim_controller/test/CMakeLists.txt +++ b/nav2_rotation_shim_controller/test/CMakeLists.txt @@ -4,10 +4,11 @@ find_package(nav2_controller REQUIRED) ament_add_gtest(test_shim_controller test_shim_controller.cpp ) -ament_target_dependencies(test_shim_controller - ${dependencies} - nav2_controller -) target_link_libraries(test_shim_controller ${library_name} + rclcpp::rclcpp + nav2_costmap_2d::nav2_costmap_2d_core + nav2_controller::simple_goal_checker + nav2_util::nav2_util_core + tf2_ros::tf2_ros ) From 9a5f7b2d104765031e34bb2e4bf7c8458d9298ab Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 7 Aug 2024 11:58:05 -0400 Subject: [PATCH 4/8] Switch nav2_rviz_plugins to modern CMake idioms. (#4600) * Switch nav2_rviz_plugins to modern CMake idioms. In addition, make some fixes to the code to more appropriately hold on to the Node object (there were warnings from the compiler without these fixes). Signed-off-by: Chris Lalancette * Feedback from review. Signed-off-by: Chris Lalancette --------- Signed-off-by: Chris Lalancette --- nav2_rviz_plugins/CMakeLists.txt | 108 ++++++++---------- .../nav2_rviz_plugins/costmap_cost_tool.hpp | 6 +- .../nav2_rviz_plugins/docking_panel.hpp | 5 + .../include/nav2_rviz_plugins/nav2_panel.hpp | 4 + nav2_rviz_plugins/package.xml | 9 +- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 25 ++-- nav2_rviz_plugins/src/docking_panel.cpp | 21 +++- nav2_rviz_plugins/src/nav2_panel.cpp | 11 +- 8 files changed, 109 insertions(+), 80 deletions(-) diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index c5f5c788aa..bd9059d698 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -1,29 +1,17 @@ cmake_minimum_required(VERSION 3.5) project(nav2_rviz_plugins) -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -# Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html -set(CMAKE_INCLUDE_CURRENT_DIR ON) -set(CMAKE_AUTOMOC ON) - find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(nav2_util REQUIRED) +find_package(nav2_common REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) find_package(nav2_msgs REQUIRED) -find_package(nav_msgs REQUIRED) +find_package(nav2_util REQUIRED) find_package(pluginlib REQUIRED) find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_default_plugins REQUIRED) find_package(rviz_ogre_vendor REQUIRED) @@ -32,23 +20,25 @@ find_package(std_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(yaml_cpp_vendor REQUIRED) +find_package(yaml-cpp REQUIRED) + +nav2_package() +# We specifically don't turn on CMAKE_AUTOMOC, since it generates one huge +# mocs_compilation.cpp file that takes a lot of memory to compile. Instead +# we create individual moc files that can be compiled separately. set(nav2_rviz_plugins_headers_to_moc include/nav2_rviz_plugins/costmap_cost_tool.hpp include/nav2_rviz_plugins/docking_panel.hpp include/nav2_rviz_plugins/goal_pose_updater.hpp - include/nav2_rviz_plugins/goal_common.hpp include/nav2_rviz_plugins/goal_tool.hpp include/nav2_rviz_plugins/nav2_panel.hpp include/nav2_rviz_plugins/selector.hpp - include/nav2_rviz_plugins/utils.hpp - include/nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp include/nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp ) - -include_directories( - include -) +foreach(header "${nav2_rviz_plugins_headers_to_moc}") + qt5_wrap_cpp(nav2_rviz_plugins_moc_files "${header}") +endforeach() set(library_name ${PROJECT_NAME}) @@ -61,45 +51,33 @@ add_library(${library_name} SHARED src/utils.cpp src/particle_cloud_display/flat_weighted_arrows_array.cpp src/particle_cloud_display/particle_cloud_display.cpp - ${nav2_rviz_plugins_headers_to_moc} -) - -set(dependencies - geometry_msgs - nav2_util - nav2_lifecycle_manager - nav2_msgs - nav_msgs - pluginlib - Qt5 - rclcpp - rclcpp_lifecycle - rviz_common - rviz_default_plugins - rviz_ogre_vendor - rviz_rendering - std_msgs - tf2_geometry_msgs - yaml_cpp_vendor -) - -ament_target_dependencies(${library_name} - ${dependencies} + ${nav2_rviz_plugins_moc_files} ) - target_include_directories(${library_name} PUBLIC ${Qt5Widgets_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} + "$" + "$" ) - -target_link_libraries(${library_name} +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + nav2_lifecycle_manager::nav2_lifecycle_manager_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_action::rclcpp_action rviz_common::rviz_common + rviz_default_plugins::rviz_default_plugins + rviz_rendering::rviz_rendering + ${std_msgs_TARGETS} + tf2_geometry_msgs::tf2_geometry_msgs + ${visualization_msgs_TARGETS} +) +target_link_libraries(${library_name} PRIVATE + ament_index_cpp::ament_index_cpp + pluginlib::pluginlib + yaml-cpp::yaml-cpp ) - -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -# TODO: Make this specific to this project (not rviz default plugins) -target_compile_definitions(${library_name} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) @@ -109,12 +87,11 @@ install( ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include ) install( DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) install( @@ -127,15 +104,22 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_targets(${library_name} HAS_LIBRARY_TARGET) ament_export_dependencies( - Qt5 - rviz_common geometry_msgs - map_msgs - nav_msgs + nav2_lifecycle_manager + nav2_msgs + nav2_util + Qt5 rclcpp + rclcpp_action + rviz_common + rviz_default_plugins + rviz_rendering + std_msgs + tf2_geometry_msgs + visualization_msgs ) ament_package() diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp index 197fe2b655..b70529f11a 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/costmap_cost_tool.hpp @@ -15,7 +15,10 @@ #ifndef NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_ #define NAV2_RVIZ_PLUGINS__COSTMAP_COST_TOOL_HPP_ +#include + #include +#include #include #include #include @@ -47,7 +50,8 @@ private Q_SLOTS: private: rclcpp::Client::SharedPtr local_cost_client_; rclcpp::Client::SharedPtr global_cost_client_; - rclcpp::Node::SharedPtr node_; + // The Node pointer that we need to keep alive for the duration of this plugin. + std::shared_ptr node_ptr_; QCursor std_cursor_; QCursor hit_cursor_; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp index 68380cb43e..868c644e4b 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/docking_panel.hpp @@ -19,12 +19,14 @@ #include #include +#include #include // ROS #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rviz_common/panel.hpp" +#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" #include "sensor_msgs/msg/battery_state.hpp" #include "nav2_msgs/action/dock_robot.hpp" #include "nav2_msgs/action/undock_robot.hpp" @@ -125,6 +127,9 @@ private Q_SLOTS: DockGoalHandle::SharedPtr dock_goal_handle_; UndockGoalHandle::SharedPtr undock_goal_handle_; + // The Node pointer that we need to keep alive for the duration of this plugin. + std::shared_ptr node_ptr_; + // Docking / Undocking action feedback subscribers rclcpp::Subscription::SharedPtr docking_feedback_sub_; rclcpp::Subscription::SharedPtr undocking_feedback_sub_; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index ce4212a4b8..471dce5ce5 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -31,6 +31,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "rviz_common/panel.hpp" +#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include "nav2_util/geometry_utils.hpp" @@ -92,6 +93,9 @@ private Q_SLOTS: std::string loop_no_ = "0"; std::string base_frame_; + // The Node pointer that we need to keep alive for the duration of this plugin. + std::shared_ptr node_ptr_; + // Call to send NavigateToPose action request for goal poses geometry_msgs::msg::PoseStamped convert_to_msg( std::vector pose, diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 90c4bdfa4d..c90944df77 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -8,24 +8,23 @@ Apache-2.0 ament_cmake + nav2_common qtbase5-dev + ament_index_cpp geometry_msgs - nav2_util nav2_lifecycle_manager nav2_msgs - nav_msgs + nav2_util pluginlib rclcpp - rclcpp_lifecycle - resource_retriever + rclcpp_action rviz_common rviz_default_plugins rviz_ogre_vendor rviz_rendering std_msgs tf2_geometry_msgs - urdf visualization_msgs yaml_cpp_vendor diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index f25a71cd80..5873b54f45 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -43,11 +43,20 @@ void CostmapCostTool::onInitialize() setName("Costmap Cost"); setIcon(rviz_common::loadPixmap("package://rviz_default_plugins/icons/classes/PointStamped.png")); - node_ = context_->getRosNodeAbstraction().lock()->get_raw_node(); + node_ptr_ = context_->getRosNodeAbstraction().lock(); + if (node_ptr_ == nullptr) { + // The node no longer exists, so just don't initialize + RCLCPP_ERROR( + rclcpp::get_logger("costmap_cost_tool"), + "Underlying ROS node no longer exists, initialization failed"); + return; + } + rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); + local_cost_client_ = - node_->create_client("/local_costmap/get_cost_local_costmap"); + node->create_client("/local_costmap/get_cost_local_costmap"); global_cost_client_ = - node_->create_client("/global_costmap/get_cost_global_costmap"); + node->create_client("/global_costmap/get_cost_global_costmap"); } void CostmapCostTool::activate() {} @@ -106,22 +115,24 @@ void CostmapCostTool::callCostService(float x, float y) void CostmapCostTool::handleLocalCostResponse( rclcpp::Client::SharedFuture future) { + rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); auto response = future.get(); if (response->cost != -1) { - RCLCPP_INFO(node_->get_logger(), "Local costmap cost: %.1f", response->cost); + RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->cost); } else { - RCLCPP_ERROR(node_->get_logger(), "Failed to get local costmap cost"); + RCLCPP_ERROR(node->get_logger(), "Failed to get local costmap cost"); } } void CostmapCostTool::handleGlobalCostResponse( rclcpp::Client::SharedFuture future) { + rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); auto response = future.get(); if (response->cost != -1) { - RCLCPP_INFO(node_->get_logger(), "Global costmap cost: %.1f", response->cost); + RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->cost); } else { - RCLCPP_ERROR(node_->get_logger(), "Failed to get global costmap cost"); + RCLCPP_ERROR(node->get_logger(), "Failed to get global costmap cost"); } } } // namespace nav2_rviz_plugins diff --git a/nav2_rviz_plugins/src/docking_panel.cpp b/nav2_rviz_plugins/src/docking_panel.cpp index 909e9a74fd..7daa327079 100644 --- a/nav2_rviz_plugins/src/docking_panel.cpp +++ b/nav2_rviz_plugins/src/docking_panel.cpp @@ -12,14 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -// C++ -#include - // QT #include #include #include #include + +// C++ +#include +#include +#include +#include + +#include #include #include "nav2_util/geometry_utils.hpp" @@ -114,7 +119,15 @@ DockingPanel::DockingPanel(QWidget * parent) void DockingPanel::onInitialize() { - auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + node_ptr_ = getDisplayContext()->getRosNodeAbstraction().lock(); + if (node_ptr_ == nullptr) { + // The node no longer exists, so just don't initialize + RCLCPP_ERROR( + rclcpp::get_logger("docking_panel"), + "Underlying ROS node no longer exists, initialization failed"); + return; + } + rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); // Create action feedback subscriber docking_feedback_sub_ = node->create_subscription( diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index a3742685f3..e10f1eccdb 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -29,6 +29,7 @@ #include "nav2_rviz_plugins/goal_common.hpp" #include "nav2_rviz_plugins/utils.hpp" +#include "rclcpp/rclcpp.hpp" #include "rviz_common/display_context.hpp" #include "rviz_common/load_resource.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" @@ -777,7 +778,15 @@ void Nav2Panel::handleGoalSaver() void Nav2Panel::onInitialize() { - auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + node_ptr_ = getDisplayContext()->getRosNodeAbstraction().lock(); + if (node_ptr_ == nullptr) { + // The node no longer exists, so just don't initialize + RCLCPP_ERROR( + rclcpp::get_logger("nav2_panel"), + "Underlying ROS node no longer exists, initialization failed"); + return; + } + rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); // declaring parameter to get the base frame node->declare_parameter("base_frame", rclcpp::ParameterValue(std::string("base_footprint"))); From ec7d39db4ab21300d136e56d9581f753c2f11cb8 Mon Sep 17 00:00:00 2001 From: Vince Reda <60265874+redvinaa@users.noreply.github.com> Date: Wed, 7 Aug 2024 18:00:17 +0200 Subject: [PATCH 5/8] Copy fix-terminate diff from opennav_docking repo (#4598) * Copy fix-terminate diff from opennav_docking repo Signed-off-by: redvinaa * Lint Signed-off-by: redvinaa --------- Signed-off-by: redvinaa --- .../opennav_docking/docking_server.hpp | 1 + .../include/opennav_docking/navigator.hpp | 8 +++- .../opennav_docking/src/docking_server.cpp | 9 +++- .../opennav_docking/src/navigator.cpp | 43 +++++++++++++++---- .../opennav_docking/test/test_navigator.cpp | 24 +++++++++-- 5 files changed, 69 insertions(+), 16 deletions(-) diff --git a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp index bab26bef5a..08c3cc1f7c 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/docking_server.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" diff --git a/nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp b/nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp index 341ad936dd..f12b92883e 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/navigator.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -67,11 +68,14 @@ class Navigator * May throw exception if fails to navigate or communicate * Blocks until completion. * @param pose Pose to go to - * @param max_staging_duration Maximum time to get to the staging pose + * @param remaining_staging_duration Remaining time to get to the staging pose + * @param isPreempted Function to check if preempted + * @param recursed True if recursed (used to retry once) */ void goToPose( const geometry_msgs::msg::PoseStamped & pose, - const rclcpp::Duration & max_staging_duration, + rclcpp::Duration remaining_staging_duration, + std::function isPreempted, bool recursed = false); protected: diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index e1b7c19279..a8bf62a6b2 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -253,8 +253,15 @@ void DockingServer::dockRobot() { RCLCPP_INFO(get_logger(), "Robot already within pre-staging pose tolerance for dock"); } else { + std::function isPreempted = [this]() { + return checkAndWarnIfCancelled(docking_action_server_, "dock_robot") || + checkAndWarnIfPreempted(docking_action_server_, "dock_robot"); + }; + navigator_->goToPose( - initial_staging_pose, rclcpp::Duration::from_seconds(goal->max_staging_time)); + initial_staging_pose, + rclcpp::Duration::from_seconds(goal->max_staging_time), + isPreempted); RCLCPP_INFO(get_logger(), "Successful navigation to staging pose"); } diff --git a/nav2_docking/opennav_docking/src/navigator.cpp b/nav2_docking/opennav_docking/src/navigator.cpp index aae3ff9709..e792411a75 100644 --- a/nav2_docking/opennav_docking/src/navigator.cpp +++ b/nav2_docking/opennav_docking/src/navigator.cpp @@ -51,13 +51,16 @@ void Navigator::deactivate() void Navigator::goToPose( const geometry_msgs::msg::PoseStamped & pose, - const rclcpp::Duration & max_staging_duration, + rclcpp::Duration remaining_staging_duration, + std::function isPreempted, bool recursed) { + auto node = node_.lock(); + Nav2Pose::Goal goal; goal.pose = pose; goal.behavior_tree = navigator_bt_xml_; - const auto timeout = max_staging_duration.to_chrono(); + const auto start_time = node->now(); // Wait for server to be active nav_to_pose_client_->wait_for_action_server(1s); @@ -66,19 +69,41 @@ void Navigator::goToPose( future_goal_handle, 2s) == rclcpp::FutureReturnCode::SUCCESS) { auto future_result = nav_to_pose_client_->async_get_result(future_goal_handle.get()); - if (executor_.spin_until_future_complete( - future_result, timeout) == rclcpp::FutureReturnCode::SUCCESS) - { - auto result = future_result.get(); - if (result.code == rclcpp_action::ResultCode::SUCCEEDED && result.result->error_code == 0) { - return; // Success! + + while (rclcpp::ok()) { + if (isPreempted()) { + auto cancel_future = nav_to_pose_client_->async_cancel_goal(future_goal_handle.get()); + executor_.spin_until_future_complete(cancel_future, 1s); + throw opennav_docking_core::FailedToStage("Navigation request to staging pose preempted."); + } + + if (node->now() - start_time > remaining_staging_duration) { + auto cancel_future = nav_to_pose_client_->async_cancel_goal(future_goal_handle.get()); + executor_.spin_until_future_complete(cancel_future, 1s); + throw opennav_docking_core::FailedToStage("Navigation request to staging pose timed out."); + } + + if (executor_.spin_until_future_complete( + future_result, 10ms) == rclcpp::FutureReturnCode::SUCCESS) + { + auto result = future_result.get(); + if (result.code == rclcpp_action::ResultCode::SUCCEEDED && + result.result->error_code == 0) + { + return; // Success! + } else { + RCLCPP_WARN(node->get_logger(), "Navigation request to staging pose failed."); + break; + } } } } // Attempt to retry once using single iteration recursion if (!recursed) { - goToPose(pose, max_staging_duration, true); + auto elapsed_time = node->now() - start_time; + remaining_staging_duration = remaining_staging_duration - elapsed_time; + goToPose(pose, remaining_staging_duration, isPreempted, true); return; } diff --git a/nav2_docking/opennav_docking/test/test_navigator.cpp b/nav2_docking/opennav_docking/test/test_navigator.cpp index 80766f0782..8d81dec81b 100644 --- a/nav2_docking/opennav_docking/test/test_navigator.cpp +++ b/nav2_docking/opennav_docking/test/test_navigator.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_util/simple_action_server.hpp" @@ -94,26 +96,40 @@ TEST(NavigatorTests, TestNavigator) auto navigator = std::make_unique(node); navigator->activate(); + std::function is_preempted_true = []() {return true;}; + std::function is_preempted_false = []() {return false;}; + // Should succeed, action server set to succeed dummy_navigator_node->setReturn(true); EXPECT_NO_THROW( - navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0))); + navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), + is_preempted_false)); // Should fail, timeout exceeded EXPECT_THROW( - navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0)), + navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(0.0, 0.0), + is_preempted_false), opennav_docking_core::FailedToStage); // Should fail, action server set to succeed dummy_navigator_node->setReturn(false); EXPECT_THROW( - navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0)), + navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), + is_preempted_false), opennav_docking_core::FailedToStage); // First should fail, recursion should succeed dummy_navigator_node->setToggle(); EXPECT_NO_THROW( - navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0))); + navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), + is_preempted_false)); + + // Should fail, preempted + dummy_navigator_node->setReturn(true); + EXPECT_THROW( + navigator->goToPose(geometry_msgs::msg::PoseStamped(), rclcpp::Duration(10.0, 10.0), + is_preempted_true), + opennav_docking_core::FailedToStage); navigator->deactivate(); navigator.reset(); From 4173c545ae4baa28e51a077187e38aaf1d720e96 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 7 Aug 2024 14:20:53 -0400 Subject: [PATCH 6/8] Switch nav2_smoother to modern CMake idioms. (#4603) Signed-off-by: Chris Lalancette --- nav2_smoother/CMakeLists.txt | 144 +++++++++++++++++++----------- nav2_smoother/package.xml | 21 +++-- nav2_smoother/test/CMakeLists.txt | 44 +++++---- 3 files changed, 132 insertions(+), 77 deletions(-) diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt index 0ab7c83628..7e93a035c0 100644 --- a/nav2_smoother/CMakeLists.txt +++ b/nav2_smoother/CMakeLists.txt @@ -2,79 +2,108 @@ cmake_minimum_required(VERSION 3.5) project(nav2_smoother) find_package(ament_cmake REQUIRED) -find_package(nav2_core REQUIRED) -find_package(nav2_common REQUIRED) find_package(angles REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(std_msgs REQUIRED) -find_package(nav2_util REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_costmap_2d REQUIRED) find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) find_package(nav_2d_utils REQUIRED) -find_package(nav_2d_msgs REQUIRED) find_package(pluginlib REQUIRED) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) nav2_package() -include_directories( - include -) - set(executable_name smoother_server) set(library_name ${executable_name}_core) -set(dependencies - angles - rclcpp - rclcpp_components - rclcpp_action - rclcpp_components - std_msgs - nav2_msgs - nav_2d_utils - nav_2d_msgs - nav2_util - nav2_core - pluginlib -) - # Main library add_library(${library_name} SHARED src/nav2_smoother.cpp ) -ament_target_dependencies(${library_name} - ${dependencies} +target_include_directories(${library_name} + PUBLIC + "$" + "$") +target_link_libraries(${library_name} PUBLIC + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_client + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros +) +target_link_libraries(${library_name} PRIVATE + nav_2d_utils::conversions + nav_2d_utils::tf_help + rclcpp_components::component + tf2::tf2 ) # Main executable add_executable(${executable_name} src/main.cpp ) -ament_target_dependencies(${executable_name} - ${dependencies} -) -target_link_libraries(${executable_name} ${library_name}) +target_include_directories(${executable_name} + PUBLIC + "$" + "$") +target_link_libraries(${executable_name} PRIVATE rclcpp::rclcpp ${library_name}) # Simple Smoother plugin add_library(simple_smoother SHARED src/simple_smoother.cpp ) -ament_target_dependencies(simple_smoother - ${dependencies} +target_include_directories(simple_smoother + PUBLIC + "$" + "$") +target_link_libraries(simple_smoother PUBLIC + angles::angles + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + nav2_costmap_2d::nav2_costmap_2d_client + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2::tf2 + tf2_ros::tf2_ros +) +target_link_libraries(simple_smoother PRIVATE + pluginlib::pluginlib ) # Savitzky Golay Smoother plugin add_library(savitzky_golay_smoother SHARED src/savitzky_golay_smoother.cpp ) -ament_target_dependencies(savitzky_golay_smoother - ${dependencies} +target_include_directories(savitzky_golay_smoother + PUBLIC + "$" + "$") +target_link_libraries(savitzky_golay_smoother PUBLIC + angles::angles + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + nav2_costmap_2d::nav2_costmap_2d_client + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2::tf2 + tf2_ros::tf2_ros +) +target_link_libraries(savitzky_golay_smoother PRIVATE + pluginlib::pluginlib ) pluginlib_export_plugin_description_file(nav2_core plugins.xml) @@ -87,12 +116,18 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + find_package(ament_index_cpp REQUIRED) + find_package(rclcpp_action REQUIRED) + + ament_find_gtest() + add_subdirectory(test) endif() rclcpp_components_register_nodes(${library_name} "nav2_smoother::SmootherServer") install( TARGETS ${library_name} simple_smoother savitzky_golay_smoother + EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -103,17 +138,22 @@ install(TARGETS ${executable_name} ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - find_package(ament_cmake_gtest REQUIRED) - add_subdirectory(test) -endif() - -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${library_name} simple_smoother savitzky_golay_smoother) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + angles + nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util + pluginlib + rclcpp + rclcpp_lifecycle + tf2 + tf2_ros +) +ament_export_targets(${library_name}) ament_package() diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index e4f7e5eca3..1e85b02a42 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -10,22 +10,25 @@ ament_cmake nav2_common + angles - rclcpp - rclcpp_components - rclcpp_action - std_msgs - nav2_util + nav2_core + nav2_costmap_2d nav2_msgs + nav2_util nav_2d_utils - nav_2d_msgs - nav2_core pluginlib + rclcpp + rclcpp_components + rclcpp_lifecycle + tf2 + tf2_ros + ament_cmake_gtest + ament_index_cpp ament_lint_common ament_lint_auto - ament_cmake_gtest - ament_cmake_pytest + rclcpp_action ament_cmake diff --git a/nav2_smoother/test/CMakeLists.txt b/nav2_smoother/test/CMakeLists.txt index 623447b5f9..ce361ad9a4 100644 --- a/nav2_smoother/test/CMakeLists.txt +++ b/nav2_smoother/test/CMakeLists.txt @@ -1,36 +1,48 @@ ament_add_gtest(test_smoother_server test_smoother_server.cpp ) - target_link_libraries(test_smoother_server ${library_name} -) - -ament_target_dependencies(test_smoother_server - ${dependencies} + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_client + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_action::rclcpp_action + rclcpp_lifecycle::rclcpp_lifecycle ) ament_add_gtest(test_simple_smoother test_simple_smoother.cpp ) - target_link_libraries(test_simple_smoother simple_smoother + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_client + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2::tf2 ) -ament_target_dependencies(test_simple_smoother - ${dependencies} -) - - ament_add_gtest(test_savitzky_golay_smoother test_savitzky_golay_smoother.cpp ) - target_link_libraries(test_savitzky_golay_smoother savitzky_golay_smoother -) - -ament_target_dependencies(test_savitzky_golay_smoother - ${dependencies} + ament_index_cpp::ament_index_cpp + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_client + nav2_costmap_2d::nav2_costmap_2d_core + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros ) From 31b9557dec2707f73787fd63f14f8d6a7c64f94d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 7 Aug 2024 17:40:04 -0400 Subject: [PATCH 7/8] Switch nav2_theta_star_planner to modern CMake idioms. (#4604) Signed-off-by: Chris Lalancette --- nav2_theta_star_planner/CMakeLists.txt | 82 +++++++++++-------- nav2_theta_star_planner/package.xml | 15 ++-- nav2_theta_star_planner/src/theta_star.cpp | 4 + .../src/theta_star_planner.cpp | 4 + 4 files changed, 63 insertions(+), 42 deletions(-) diff --git a/nav2_theta_star_planner/CMakeLists.txt b/nav2_theta_star_planner/CMakeLists.txt index 8cf8a28c35..de6778fe2b 100644 --- a/nav2_theta_star_planner/CMakeLists.txt +++ b/nav2_theta_star_planner/CMakeLists.txt @@ -2,80 +2,94 @@ cmake_minimum_required(VERSION 3.5) project(nav2_theta_star_planner) find_package(ament_cmake REQUIRED) -find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_costmap_2d REQUIRED) -find_package(nav2_msgs REQUIRED) find_package(nav2_util REQUIRED) +find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(rcl_interfaces REQUIRED) find_package(tf2_ros REQUIRED) -nav2_package() #Calls the nav2_package.cmake file -add_compile_options(-O3) - -include_directories( - include -) +nav2_package() set(library_name ${PROJECT_NAME}) -set(dependencies ament_cmake - builtin_interfaces - nav2_common - nav2_core - nav2_costmap_2d - nav2_msgs - nav2_util - pluginlib - rclcpp - rclcpp_action - rclcpp_lifecycle - tf2_ros -) - - add_library(${library_name} SHARED src/theta_star.cpp src/theta_star_planner.cpp ) - -ament_target_dependencies(${library_name} ${dependencies}) - -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB_DISABLE_BOOST_FUNCTIONS") +target_include_directories(${library_name} + PUBLIC + "$" + "$") +target_link_libraries(${library_name} PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core + ${nav_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + tf2_ros::tf2_ros +) +target_link_libraries(${library_name} PRIVATE + pluginlib::pluginlib +) +target_compile_options(${library_name} PRIVATE -O3) pluginlib_export_plugin_description_file(nav2_core theta_star_planner.xml) install(TARGETS ${library_name} + EXPORT ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) install(FILES theta_star_planner.xml DESTINATION share/${PROJECT_NAME} ) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(gtest_disable_pthreads OFF) ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + + ament_find_gtest() + ament_add_gtest(test_theta_star test/test_theta_star.cpp) - ament_target_dependencies(test_theta_star ${dependencies}) - target_link_libraries(test_theta_star ${library_name}) + target_link_libraries(test_theta_star + ${library_name} + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ) endif() -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${library_name}) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + geometry_msgs + nav2_core + nav2_costmap_2dnav2_util + nav_msgs + rclcpp + rclcpp_lifecycle + rcl_interfaces + tf2_ros +) +ament_export_targets(${library_name}) ament_package() diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 0fcbd0e288..0da357b581 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -9,26 +9,25 @@ Apache-2.0 ament_cmake + nav2_common - builtin_interfaces - nav2_common + geometry_msgs nav2_core nav2_costmap_2d - nav2_msgs nav2_util + nav_msgs pluginlib rclcpp - rclcpp_action rclcpp_lifecycle + rcl_interfaces tf2_ros - + + ament_cmake_gtest ament_lint_auto ament_lint_common - ament_cmake_gtest - + ament_cmake - diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index ba3629bcbf..48a3f6c1bd 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -12,7 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include + +#include "geometry_msgs/msg/pose_stamped.hpp" + #include "nav2_core/planner_exceptions.hpp" #include "nav2_theta_star_planner/theta_star.hpp" diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 24f2b81000..5d14368cc0 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -15,6 +15,10 @@ #include #include #include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + #include "nav2_theta_star_planner/theta_star_planner.hpp" #include "nav2_theta_star_planner/theta_star.hpp" From 46c40cb603bb37afcf82cb5f353d51a104bb6284 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 7 Aug 2024 16:32:32 -0700 Subject: [PATCH 8/8] Fix race condition in AMCL for #4537 (#4605) --- nav2_amcl/src/amcl_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 5c6b165d59..595ffb7626 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -537,8 +537,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha global_frame_id_.c_str()); return; } - if (abs(msg->pose.pose.position.x) > map_->size_x || - abs(msg->pose.pose.position.y) > map_->size_y) + if (first_map_received_ && (abs(msg->pose.pose.position.x) > map_->size_x || + abs(msg->pose.pose.position.y) > map_->size_y)) { RCLCPP_ERROR( get_logger(), "Received initialpose from message is out of the size of map. Rejecting.");