diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt index c68732d4d7..c859303edf 100644 --- a/nav2_constrained_smoother/CMakeLists.txt +++ b/nav2_constrained_smoother/CMakeLists.txt @@ -4,47 +4,48 @@ project(nav2_constrained_smoother) set(CMAKE_BUILD_TYPE Release) # significant Ceres optimization speedup find_package(ament_cmake REQUIRED) +find_package(Ceres REQUIRED COMPONENTS SuiteSparse) +find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(nav2_core REQUIRED) find_package(nav2_common REQUIRED) -find_package(angles REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(nav2_util REQUIRED) -find_package(nav2_msgs REQUIRED) find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) find_package(pluginlib REQUIRED) -find_package(Ceres REQUIRED COMPONENTS SuiteSparse) - -set(CMAKE_CXX_STANDARD 17) - -if(${CERES_VERSION} VERSION_LESS_EQUAL 2.0.0) - add_definitions(-DUSE_OLD_CERES_API) -endif() +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(tf2_ros REQUIRED) nav2_package() set(library_name nav2_constrained_smoother) -include_directories( - include +add_library(${library_name} SHARED src/constrained_smoother.cpp) +if(${CERES_VERSION} VERSION_LESS_EQUAL 2.0.0) + target_compile_definitions(${library_name} PUBLIC -DUSE_OLD_CERES_API) +endif() +target_include_directories(${library_name} + PUBLIC + "$" + "$" ) - -set(dependencies - angles - rclcpp - rclcpp_action - nav2_msgs - nav2_costmap_2d - nav2_util - nav2_core - pluginlib +target_link_libraries(${library_name} PUBLIC + Ceres::ceres + Eigen3::Eigen + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_client + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros +) +target_link_libraries(${library_name} PRIVATE + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + pluginlib::pluginlib ) - -add_library(${library_name} SHARED src/constrained_smoother.cpp) -target_link_libraries(${library_name} Ceres::ceres) -# prevent pluginlib from using boost -target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -ament_target_dependencies(${library_name} ${dependencies}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -53,29 +54,29 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + find_package(angles REQUIRED) + + ament_find_gtest() + add_subdirectory(test) endif() install( - TARGETS - ${library_name} + 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) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -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(ceres eigen3 nav2_core nav2_costmap_2d nav2_util rclcpp rclcpp_lifecycle tf2_ros) +ament_export_targets(${library_name}) pluginlib_export_plugin_description_file(nav2_core nav2_constrained_smoother.xml) diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp index bfd564772e..4c276cdc69 100644 --- a/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp +++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/constrained_smoother.hpp @@ -27,7 +27,6 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_util/odometry_utils.hpp" #include "nav2_util/geometry_utils.hpp" -#include "geometry_msgs/msg/pose2_d.hpp" namespace nav2_constrained_smoother { diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index b363b1bf3b..0628a74526 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -9,20 +9,25 @@ Apache-2.0 ament_cmake - nav2_common - angles - rclcpp - nav2_util - nav2_msgs - nav2_costmap_2d + eigen + nav2_common + + geometry_msgs + libceres-dev nav2_core + nav2_costmap_2d + nav2_msgs + nav2_util pluginlib - libceres-dev + rclcpp + rclcpp_lifecycle + tf2_ros ament_lint_common ament_lint_auto ament_cmake_gtest ament_cmake_pytest + angles ament_cmake diff --git a/nav2_constrained_smoother/src/constrained_smoother.cpp b/nav2_constrained_smoother/src/constrained_smoother.cpp index 406ca377c3..6ff2f504a2 100644 --- a/nav2_constrained_smoother/src/constrained_smoother.cpp +++ b/nav2_constrained_smoother/src/constrained_smoother.cpp @@ -20,6 +20,8 @@ #include #include +#include "geometry_msgs/msg/pose_stamped.hpp" + #include "nav2_constrained_smoother/constrained_smoother.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" diff --git a/nav2_constrained_smoother/test/CMakeLists.txt b/nav2_constrained_smoother/test/CMakeLists.txt index cf4ecd1967..85981eeeb8 100644 --- a/nav2_constrained_smoother/test/CMakeLists.txt +++ b/nav2_constrained_smoother/test/CMakeLists.txt @@ -1,12 +1,15 @@ ament_add_gtest(test_constrained_smoother test_constrained_smoother.cpp ) - target_link_libraries(test_constrained_smoother ${library_name} -) -ament_target_dependencies(test_constrained_smoother - ${dependencies} + angles::angles + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_client + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core + rclcpp::rclcpp + tf2_ros::tf2_ros ) ament_add_gtest(test_smoother_cost_function @@ -14,7 +17,5 @@ ament_add_gtest(test_smoother_cost_function ) target_link_libraries(test_smoother_cost_function ${library_name} + rclcpp::rclcpp ) -ament_target_dependencies(test_smoother_cost_function - ${dependencies} -) \ No newline at end of file