diff --git a/nav2_bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/rviz/nav2_namespaced_view.rviz new file mode 100644 index 00000000000..4dcd23e24b4 --- /dev/null +++ b/nav2_bringup/rviz/nav2_namespaced_view.rviz @@ -0,0 +1,379 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 195 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /RobotModel1/Status1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Global Planner1/Global Costmap1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 464 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: true + Max Arrow Length: 0.3 + Min Arrow Length: 0.02 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_cloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + Value: true + Enabled: true + Name: Controller + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: -1.5708 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 160 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 914 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000338fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003070000006e0000006200ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1545 + X: 186 + Y: 117 + diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt index d778f4b46d9..add25f47544 100644 --- a/nav2_docking/opennav_docking/CMakeLists.txt +++ b/nav2_docking/opennav_docking/CMakeLists.txt @@ -3,65 +3,45 @@ project(opennav_docking) find_package(ament_cmake REQUIRED) find_package(angles 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(sensor_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_common REQUIRED) find_package(nav2_graceful_controller 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(opennav_docking_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(pluginlib REQUIRED) find_package(yaml_cpp_vendor REQUIRED) find_package(yaml-cpp REQUIRED) -find_package(opennav_docking_core REQUIRED) - -# potentially replace with nav2_common, nav2_package() -set(CMAKE_CXX_STANDARD 17) -add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) -include_directories( - include -) +nav2_package() set(executable_name opennav_docking) set(library_name ${executable_name}_core) -set(dependencies - angles - rclcpp - rclcpp_action - rclcpp_lifecycle - rclcpp_components - std_msgs - sensor_msgs - visualization_msgs - nav2_graceful_controller - nav2_util - nav2_msgs - nav_msgs - geometry_msgs - builtin_interfaces - tf2_ros - tf2_geometry_msgs - pluginlib - yaml_cpp_vendor - opennav_docking_core -) - add_library(controller SHARED src/controller.cpp ) - -ament_target_dependencies(controller - ${dependencies} +target_include_directories(controller + PUBLIC + "$" + "$" +) +target_link_libraries(controller PUBLIC + ${geometry_msgs_TARGETS} + nav2_graceful_controller::nav2_graceful_controller + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} ) add_library(${library_name} SHARED @@ -69,55 +49,111 @@ add_library(${library_name} SHARED src/dock_database.cpp src/navigator.cpp ) - -ament_target_dependencies(${library_name} - ${dependencies} +target_include_directories(${library_name} + PUBLIC + "$" + "$" ) - -target_link_libraries(${library_name} - yaml-cpp::yaml-cpp +target_link_libraries(${library_name} PUBLIC + angles::angles controller + ${geometry_msgs_TARGETS} + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + opennav_docking_core::opennav_docking_core + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_action::rclcpp_action + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + tf2_ros::tf2_ros + yaml-cpp::yaml-cpp +) +target_link_libraries(${library_name} PRIVATE + rclcpp_components::component + tf2_geometry_msgs::tf2_geometry_msgs ) add_library(pose_filter SHARED src/pose_filter.cpp ) - -ament_target_dependencies(pose_filter - ${dependencies} +target_include_directories(pose_filter + PUBLIC + "$" + "$" +) +target_link_libraries(pose_filter PUBLIC + ${geometry_msgs_TARGETS} +) +target_link_libraries(pose_filter PRIVATE + rclcpp::rclcpp + tf2_geometry_msgs::tf2_geometry_msgs ) add_executable(${executable_name} src/main.cpp ) - -target_link_libraries(${executable_name} ${library_name}) - -ament_target_dependencies(${executable_name} - ${dependencies} +target_include_directories(${executable_name} + PUBLIC + "$" + "$" ) +target_link_libraries(${executable_name} PRIVATE ${library_name} rclcpp::rclcpp) rclcpp_components_register_nodes(${library_name} "opennav_docking::DockingServer") add_library(simple_charging_dock SHARED src/simple_charging_dock.cpp ) -ament_target_dependencies(simple_charging_dock - ${dependencies} +target_include_directories(simple_charging_dock + PUBLIC + "$" + "$" +) +target_link_libraries(simple_charging_dock PUBLIC + ${geometry_msgs_TARGETS} + opennav_docking_core::opennav_docking_core + pose_filter + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${sensor_msgs_TARGETS} + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros +) +target_link_libraries(simple_charging_dock PRIVATE + nav2_util::nav2_util_core + pluginlib::pluginlib + tf2::tf2 ) -target_link_libraries(simple_charging_dock pose_filter) add_library(simple_non_charging_dock SHARED src/simple_non_charging_dock.cpp ) -ament_target_dependencies(simple_non_charging_dock - ${dependencies} +target_include_directories(simple_non_charging_dock + PUBLIC + "$" + "$" +) +target_link_libraries(simple_non_charging_dock PUBLIC + ${geometry_msgs_TARGETS} + opennav_docking_core::opennav_docking_core + pose_filter + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${sensor_msgs_TARGETS} + tf2_geometry_msgs::tf2_geometry_msgs + tf2_ros::tf2_ros +) +target_link_libraries(simple_non_charging_dock PRIVATE + nav2_util::nav2_util_core + pluginlib::pluginlib + tf2::tf2 ) -target_link_libraries(simple_non_charging_dock pose_filter) pluginlib_export_plugin_description_file(opennav_docking_core plugins.xml) install(TARGETS ${library_name} controller pose_filter simple_charging_dock simple_non_charging_dock + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -128,7 +164,7 @@ install(TARGETS ${executable_name} ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) install(FILES test/test_dock_file.yaml @@ -139,11 +175,28 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) + find_package(ament_index_cpp 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} controller pose_filter) -ament_export_dependencies(${dependencies} yaml-cpp) +ament_export_dependencies( + geometry_msgs + nav2_graceful_controller + nav2_msgs + nav2_util + opennav_docking_core + pluginlib + rclcpp + rclcpp_action + rclcpp_lifecycle + rcl_interfaces + tf2_ros + yaml-cpp +) +ament_export_targets(${PROJECT_NAME}) ament_package() diff --git a/nav2_docking/opennav_docking/include/opennav_docking/pose_filter.hpp b/nav2_docking/opennav_docking/include/opennav_docking/pose_filter.hpp index 50b538bfd7b..99f8359b5e1 100644 --- a/nav2_docking/opennav_docking/include/opennav_docking/pose_filter.hpp +++ b/nav2_docking/opennav_docking/include/opennav_docking/pose_filter.hpp @@ -16,7 +16,6 @@ #define OPENNAV_DOCKING__POSE_FILTER_HPP_ #include "geometry_msgs/msg/pose_stamped.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace opennav_docking { diff --git a/nav2_docking/opennav_docking/package.xml b/nav2_docking/opennav_docking/package.xml index 4c5ac1789b2..bb802980419 100644 --- a/nav2_docking/opennav_docking/package.xml +++ b/nav2_docking/opennav_docking/package.xml @@ -8,25 +8,30 @@ Apache-2.0 ament_cmake + nav2_common angles - rclcpp - rclcpp_action - rclcpp_lifecycle + geometry_msgs nav2_graceful_controller nav2_msgs nav2_util nav_msgs - geometry_msgs - builtin_interfaces - sensor_msgs - pluginlib - yaml_cpp_vendor opennav_docking_core + pluginlib + rclcpp + rclcpp_action + rclcpp_components + rclcpp_lifecycle + rcl_interfaces + sensor_msgs + tf2 + tf2_geometry_msgs tf2_ros + yaml_cpp_vendor ament_cmake_gtest ament_cmake_pytest + ament_index_cpp ament_lint_common ament_lint_auto diff --git a/nav2_docking/opennav_docking/src/pose_filter.cpp b/nav2_docking/opennav_docking/src/pose_filter.cpp index 53f91a734c4..e010087f63c 100644 --- a/nav2_docking/opennav_docking/src/pose_filter.cpp +++ b/nav2_docking/opennav_docking/src/pose_filter.cpp @@ -14,6 +14,7 @@ #include "opennav_docking/pose_filter.hpp" #include "rclcpp/rclcpp.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace opennav_docking { diff --git a/nav2_docking/opennav_docking/test/CMakeLists.txt b/nav2_docking/opennav_docking/test/CMakeLists.txt index 3400c1d8db5..65d6e1d68ab 100644 --- a/nav2_docking/opennav_docking/test/CMakeLists.txt +++ b/nav2_docking/opennav_docking/test/CMakeLists.txt @@ -2,55 +2,63 @@ ament_add_gtest(test_utils test_utils.cpp ) -ament_target_dependencies(test_utils - ${dependencies} -) target_link_libraries(test_utils + ament_index_cpp::ament_index_cpp + ${geometry_msgs_TARGETS} ${library_name} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test dock database ament_add_gtest(test_dock_database test_dock_database.cpp ) -ament_target_dependencies(test_dock_database - ${dependencies} -) target_link_libraries(test_dock_database + ament_index_cpp::ament_index_cpp ${library_name} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test navigator ament_add_gtest(test_navigator test_navigator.cpp ) -ament_target_dependencies(test_navigator - ${dependencies} -) target_link_libraries(test_navigator + ament_index_cpp::ament_index_cpp + ${geometry_msgs_TARGETS} ${library_name} + ${nav2_msgs_TARGETS} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test Controller ament_add_gtest(test_controller test_controller.cpp ) -ament_target_dependencies(test_controller - ${dependencies} -) target_link_libraries(test_controller + ${geometry_msgs_TARGETS} ${library_name} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test Simple Dock ament_add_gtest(test_simple_charging_dock test_simple_charging_dock.cpp ) -ament_target_dependencies(test_simple_charging_dock - ${dependencies} -) target_link_libraries(test_simple_charging_dock - ${library_name} simple_charging_dock + ament_index_cpp::ament_index_cpp + ${geometry_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${sensor_msgs_TARGETS} + simple_charging_dock + tf2_geometry_msgs::tf2_geometry_msgs ) ament_add_gtest(test_simple_non_charging_dock test_simple_non_charging_dock.cpp @@ -66,22 +74,27 @@ target_link_libraries(test_simple_non_charging_dock ament_add_gtest(test_pose_filter test_pose_filter.cpp ) -ament_target_dependencies(test_pose_filter - ${dependencies} -) target_link_libraries(test_pose_filter - ${library_name} + ${geometry_msgs_TARGETS} pose_filter + rclcpp::rclcpp + tf2::tf2 + tf2_geometry_msgs::tf2_geometry_msgs ) # Test dock pluing for server tests add_library(test_dock SHARED testing_dock.cpp) -ament_target_dependencies(test_dock ${dependencies}) -target_compile_definitions(test_dock PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_link_libraries(test_dock PUBLIC + ${geometry_msgs_TARGETS} + opennav_docking_core::opennav_docking_core + pluginlib::pluginlib + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros +) install(TARGETS test_dock - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) ament_export_libraries(test_dock) @@ -89,11 +102,12 @@ ament_export_libraries(test_dock) ament_add_gtest(test_docking_server_unit test_docking_server_unit.cpp ) -ament_target_dependencies(test_docking_server_unit - ${dependencies} -) target_link_libraries(test_docking_server_unit + ${geometry_msgs_TARGETS} ${library_name} + nav2_util::nav2_util_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test docking server (smoke) diff --git a/nav2_docking/opennav_docking_bt/CMakeLists.txt b/nav2_docking/opennav_docking_bt/CMakeLists.txt index c89330ee38a..a852c4124b7 100644 --- a/nav2_docking/opennav_docking_bt/CMakeLists.txt +++ b/nav2_docking/opennav_docking_bt/CMakeLists.txt @@ -2,56 +2,56 @@ cmake_minimum_required(VERSION 3.5) project(opennav_docking_bt) find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(std_msgs REQUIRED) -find_package(nav2_util REQUIRED) -find_package(nav2_msgs REQUIRED) -find_package(nav2_core REQUIRED) +find_package(behaviortree_cpp REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(nav2_behavior_tree REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_core REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav2_util REQUIRED) find_package(nav_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(behaviortree_cpp REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) -# potentially replace with nav2_common, nav2_package() -set(CMAKE_CXX_STANDARD 17) -add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wdeprecated -fPIC -Wshadow -Wnull-dereference) +nav2_package() -include_directories( - include +add_library(opennav_dock_action_bt_node SHARED src/dock_robot.cpp) +target_include_directories(opennav_dock_action_bt_node + PUBLIC + "$" + "$" ) - -set(dependencies - rclcpp - rclcpp_action - std_msgs - nav2_util - nav2_msgs - nav2_core - nav2_behavior_tree - nav_msgs - geometry_msgs - behaviortree_cpp +target_compile_definitions(opennav_dock_action_bt_node PRIVATE BT_PLUGIN_EXPORT) +target_link_libraries(opennav_dock_action_bt_node PUBLIC + behaviortree_cpp::behaviortree_cpp + ${geometry_msgs_TARGETS} + nav2_behavior_tree::nav2_behavior_tree + ${nav2_msgs_TARGETS} ) -add_library(opennav_dock_action_bt_node SHARED src/dock_robot.cpp) -list(APPEND plugin_libs opennav_dock_action_bt_node) add_library(opennav_undock_action_bt_node SHARED src/undock_robot.cpp) -list(APPEND plugin_libs opennav_undock_action_bt_node) - -foreach(bt_plugin ${plugin_libs}) - ament_target_dependencies(${bt_plugin} ${dependencies}) - target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) -endforeach() +target_include_directories(opennav_undock_action_bt_node + PUBLIC + "$" + "$" +) +target_compile_definitions(opennav_undock_action_bt_node PRIVATE BT_PLUGIN_EXPORT) +target_link_libraries(opennav_undock_action_bt_node PUBLIC + behaviortree_cpp::behaviortree_cpp + ${geometry_msgs_TARGETS} + nav2_behavior_tree::nav2_behavior_tree + ${nav2_msgs_TARGETS} +) -install(TARGETS ${plugin_libs} +install(TARGETS opennav_dock_action_bt_node opennav_undock_action_bt_node + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY behavior_trees DESTINATION share/${PROJECT_NAME}) @@ -63,7 +63,13 @@ if(BUILD_TESTING) add_subdirectory(test) endif() -ament_export_include_directories(include) -ament_export_libraries(${plugin_libs}) -ament_export_dependencies(${dependencies}) +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_libraries(opennav_dock_action_bt_node opennav_undock_action_bt_node) +ament_export_dependencies( + behaviortree_cpp + geometry_msgs + nav2_behavior_tree + nav2_msgs +) +ament_export_targets(${PROJECT_NAME}) ament_package() diff --git a/nav2_docking/opennav_docking_bt/package.xml b/nav2_docking/opennav_docking_bt/package.xml index 85b90b12070..37785551165 100644 --- a/nav2_docking/opennav_docking_bt/package.xml +++ b/nav2_docking/opennav_docking_bt/package.xml @@ -8,16 +8,17 @@ Apache-2.0 ament_cmake + nav2_common - rclcpp - rclcpp_action + behaviortree_cpp + geometry_msgs nav2_behavior_tree - nav2_util nav2_core nav2_msgs + nav2_util nav_msgs - geometry_msgs - behaviortree_cpp + rclcpp + rclcpp_action ament_lint_common ament_lint_auto diff --git a/nav2_docking/opennav_docking_bt/test/CMakeLists.txt b/nav2_docking/opennav_docking_bt/test/CMakeLists.txt index ba0c374f8b8..077e3a8581e 100644 --- a/nav2_docking/opennav_docking_bt/test/CMakeLists.txt +++ b/nav2_docking/opennav_docking_bt/test/CMakeLists.txt @@ -1,9 +1,21 @@ # Cancel test ament_add_gtest(test_dock_robot test_dock_robot.cpp) -target_link_libraries(test_dock_robot opennav_dock_action_bt_node) -ament_target_dependencies(test_dock_robot ${dependencies}) +target_link_libraries(test_dock_robot + behaviortree_cpp::behaviortree_cpp + ${geometry_msgs_TARGETS} + ${nav2_msgs_TARGETS} + opennav_dock_action_bt_node + rclcpp::rclcpp + rclcpp_action::rclcpp_action +) # Make command test ament_add_gtest(test_undock_robot test_undock_robot.cpp) -target_link_libraries(test_undock_robot opennav_undock_action_bt_node) -ament_target_dependencies(test_undock_robot ${dependencies}) +target_link_libraries(test_undock_robot + behaviortree_cpp::behaviortree_cpp + ${geometry_msgs_TARGETS} + ${nav2_msgs_TARGETS} + opennav_undock_action_bt_node + rclcpp::rclcpp + rclcpp_action::rclcpp_action +) diff --git a/nav2_docking/opennav_docking_core/CMakeLists.txt b/nav2_docking/opennav_docking_core/CMakeLists.txt index 639319bc5ee..3d62476effc 100644 --- a/nav2_docking/opennav_docking_core/CMakeLists.txt +++ b/nav2_docking/opennav_docking_core/CMakeLists.txt @@ -2,23 +2,34 @@ cmake_minimum_required(VERSION 3.5) project(opennav_docking_core) find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) -find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) -find_package(nav2_util REQUIRED) -find_package(nav2_msgs REQUIRED) +find_package(tf2_ros REQUIRED) nav2_package() -set(dependencies - rclcpp - rclcpp_lifecycle - nav2_msgs - nav2_util +add_library(opennav_docking_core INTERFACE) +target_include_directories(opennav_docking_core + INTERFACE + "$" + "$" +) +target_link_libraries(opennav_docking_core INTERFACE + ${geometry_msgs_TARGETS} + rclcpp_lifecycle::rclcpp_lifecycle + tf2_ros::tf2_ros +) + +install(TARGETS opennav_docking_core + EXPORT opennav_docking_core + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -28,7 +39,12 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_export_include_directories(include) -ament_export_dependencies(${dependencies}) +ament_export_include_directories(include/${PROJECT_NAME}) +ament_export_dependencies( + geometry_msgs + rclcpp_lifecycle + tf2_ros +) +ament_export_targets(opennav_docking_core) ament_package() diff --git a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp index 695ab6e2cbc..886fde2fa7a 100644 --- a/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp +++ b/nav2_docking/opennav_docking_core/include/opennav_docking_core/charging_dock.hpp @@ -18,9 +18,8 @@ #include #include -#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_util/lifecycle_node.hpp" #include "tf2_ros/buffer.h" diff --git a/nav2_docking/opennav_docking_core/package.xml b/nav2_docking/opennav_docking_core/package.xml index bcd03d786b9..2df639d5161 100644 --- a/nav2_docking/opennav_docking_core/package.xml +++ b/nav2_docking/opennav_docking_core/package.xml @@ -8,16 +8,14 @@ Apache-2.0 ament_cmake + nav2_common - rclcpp + geometry_msgs rclcpp_lifecycle - nav2_util - nav2_msgs + tf2_ros ament_lint_common ament_lint_auto - ament_cmake_gtest - ament_cmake_pytest ament_cmake diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt index a1806c524a5..75a00b9b7f7 100644 --- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt @@ -2,38 +2,41 @@ cmake_minimum_required(VERSION 3.5) project(dwb_plugins) find_package(ament_cmake REQUIRED) -find_package(nav2_common REQUIRED) -find_package(angles REQUIRED) find_package(dwb_core REQUIRED) +find_package(dwb_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav2_common REQUIRED) +find_package(nav2_util REQUIRED) find_package(nav_2d_msgs REQUIRED) find_package(nav_2d_utils REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) -find_package(nav2_util REQUIRED) +find_package(rcl_interfaces REQUIRED) nav2_package() -set(dependencies - angles - dwb_core - nav_2d_msgs - nav_2d_utils - pluginlib - rclcpp - nav2_util +add_library(standard_traj_generator SHARED + src/standard_traj_generator.cpp + src/limited_accel_generator.cpp + src/kinematic_parameters.cpp + src/xy_theta_iterator.cpp) +target_include_directories(standard_traj_generator PUBLIC + "$" + "$" ) - -include_directories( - include +target_link_libraries(standard_traj_generator PUBLIC + dwb_core::dwb_core + ${dwb_msgs_TARGETS} + ${geometry_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_2d_msgs_TARGETS} + rclcpp::rclcpp + ${rcl_interfaces_TARGETS} +) +target_link_libraries(standard_traj_generator PRIVATE + nav_2d_utils::conversions + pluginlib::pluginlib ) - -add_library(standard_traj_generator SHARED - src/standard_traj_generator.cpp - src/limited_accel_generator.cpp - src/kinematic_parameters.cpp - src/xy_theta_iterator.cpp) -ament_target_dependencies(standard_traj_generator ${dependencies}) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -42,23 +45,39 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + ament_find_gtest() + + find_package(nav2_costmap_2d REQUIRED) + find_package(rclcpp_lifecycle REQUIRED) add_subdirectory(test) endif() install(TARGETS standard_traj_generator - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) install(FILES plugins.xml - DESTINATION share/${PROJECT_NAME} + DESTINATION share/${PROJECT_NAME} ) -ament_export_include_directories(include) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(standard_traj_generator) +ament_export_dependencies( + dwb_core + dwb_msgs + geometry_msgs + nav2_util + nav_2d_msgs + rclcpp + rcl_interfaces +) +ament_export_targets(${PROJECT_NAME}) + pluginlib_export_plugin_description_file(dwb_core plugins.xml) ament_package() diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp index 55a663fddbc..e5feefc9faa 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp @@ -38,7 +38,6 @@ #include #include -#include "rclcpp/rclcpp.hpp" #include "nav_2d_msgs/msg/twist2_d.hpp" #include "dwb_plugins/kinematic_parameters.hpp" #include "nav2_util/lifecycle_node.hpp" diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 86311cfb563..3e0b0c941ec 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -12,17 +12,21 @@ ament_cmake nav2_common - angles dwb_core + dwb_msgs + geometry_msgs + nav2_util nav_2d_msgs nav_2d_utils pluginlib rclcpp - nav2_util + rcl_interfaces + ament_cmake_gtest ament_lint_common ament_lint_auto - ament_cmake_gtest + nav2_costmap_2d + rclcpp_lifecycle ament_cmake diff --git a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt index c5b9308e8dc..a7853938435 100644 --- a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt @@ -1,10 +1,29 @@ ament_add_gtest(vtest velocity_iterator_test.cpp) +target_link_libraries(vtest standard_traj_generator) ament_add_gtest(twist_gen_test twist_gen.cpp) -target_link_libraries(twist_gen_test standard_traj_generator) +target_link_libraries(twist_gen_test + standard_traj_generator + dwb_core::dwb_core + ${dwb_msgs_TARGETS} + ${geometry_msgs_TARGETS} + nav2_util::nav2_util_core + ${nav_2d_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) ament_add_gtest(kinematic_parameters_test kinematic_parameters_test.cpp) -target_link_libraries(kinematic_parameters_test standard_traj_generator) +target_link_libraries(kinematic_parameters_test + standard_traj_generator + rclcpp::rclcpp + ${rcl_interfaces_TARGETS} +) ament_add_gtest(speed_limit_test speed_limit_test.cpp) -target_link_libraries(speed_limit_test standard_traj_generator) +target_link_libraries(speed_limit_test + standard_traj_generator + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core + rclcpp::rclcpp +) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index dd476aa9869..885a6725f5a 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py +++ b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py @@ -17,17 +17,24 @@ from geometry_msgs.msg import PoseWithCovarianceStamped, Twist from geometry_msgs.msg import Quaternion, TransformStamped, Vector3 +from nav2_simple_commander.line_iterator import LineIterator from nav_msgs.msg import Odometry +from nav_msgs.srv import GetMap import rclpy from rclpy.duration import Duration from rclpy.node import Node from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy from sensor_msgs.msg import LaserScan -from tf2_ros import TransformBroadcaster +from tf2_ros import Buffer, TransformBroadcaster, TransformListener import tf_transformations -from .utils import addYawToQuat, matrixToTransform, transformStampedToMatrix - +from .utils import ( + addYawToQuat, + getMapOccupancy, + matrixToTransform, + transformStampedToMatrix, + worldToMap, +) """ This is a loopback simulator that replaces a physics simulator to create a @@ -48,6 +55,8 @@ def __init__(self): self.initial_pose = None self.timer = None self.setupTimer = None + self.map = None + self.mat_base_to_laser = None self.declare_parameter('update_duration', 0.01) self.update_dur = self.get_parameter('update_duration').get_parameter_value().double_value @@ -64,6 +73,14 @@ def __init__(self): self.declare_parameter('scan_frame_id', 'base_scan') self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value + self.declare_parameter('scan_publish_dur', 0.1) + self.scan_publish_dur = self.get_parameter( + 'scan_publish_dur').get_parameter_value().double_value + + self.declare_parameter('publish_map_odom_tf', True) + self.publish_map_odom_tf = self.get_parameter( + 'publish_map_odom_tf').get_parameter_value().bool_value + self.t_map_to_odom = TransformStamped() self.t_map_to_odom.header.frame_id = self.map_frame_id self.t_map_to_odom.child_frame_id = self.odom_frame_id @@ -88,12 +105,31 @@ def __init__(self): self.scan_pub = self.create_publisher(LaserScan, 'scan', sensor_qos) self.setupTimer = self.create_timer(0.1, self.setupTimerCallback) + + self.map_client = self.create_client(GetMap, '/map_server/map') + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.getMap() + self.info('Loopback simulator initialized') + def getBaseToLaserTf(self): + try: + # Wait for transform and lookup + transform = self.tf_buffer.lookup_transform( + self.base_frame_id, self.scan_frame_id, rclpy.time.Time()) + self.mat_base_to_laser = transformStampedToMatrix(transform) + + except Exception as ex: + self.get_logger().error('Transform lookup failed: %s' % str(ex)) + def setupTimerCallback(self): # Publish initial identity odom transform & laser scan to warm up system self.tf_broadcaster.sendTransform(self.t_odom_to_base_link) - self.publishLaserScan() + if self.mat_base_to_laser is None: + self.getBaseToLaserTf() def cmdVelCallback(self, msg): self.debug('Received cmd_vel') @@ -122,6 +158,7 @@ def initialPoseCallback(self, msg): self.setupTimer.destroy() self.setupTimer = None self.timer = self.create_timer(self.update_dur, self.timerCallback) + self.timer_laser = self.create_timer(self.scan_publish_dur, self.publishLaserScan) return self.initial_pose = msg.pose.pose @@ -129,7 +166,7 @@ def initialPoseCallback(self, msg): # Adjust map->odom transform based on new initial pose, keeping odom->base_link the same t_map_to_base_link = TransformStamped() t_map_to_base_link.header = msg.header - t_map_to_base_link.child_frame_id = 'base_link' + t_map_to_base_link.child_frame_id = self.base_frame_id t_map_to_base_link.transform.translation.x = self.initial_pose.position.x t_map_to_base_link.transform.translation.y = self.initial_pose.position.y t_map_to_base_link.transform.translation.z = 0.0 @@ -165,29 +202,33 @@ def timerCallback(self): self.publishTransforms(self.t_map_to_odom, self.t_odom_to_base_link) self.publishOdometry(self.t_odom_to_base_link) - self.publishLaserScan() def publishLaserScan(self): # Publish a bogus laser scan for collision monitor - scan = LaserScan() - # scan.header.stamp = (self.get_clock().now()).to_msg() - scan.header.frame_id = self.scan_frame_id - scan.angle_min = -math.pi - scan.angle_max = math.pi - scan.angle_increment = 0.005817705996 # 0.333 degrees - scan.time_increment = 0.0 - scan.scan_time = 0.1 - scan.range_min = 0.1 - scan.range_max = 100.0 - num_samples = int((scan.angle_max - scan.angle_min) / scan.angle_increment) - scan.ranges = [scan.range_max - 0.01] * num_samples - self.scan_pub.publish(scan) + self.scan_msg = LaserScan() + self.scan_msg.header.stamp = (self.get_clock().now()).to_msg() + self.scan_msg.header.frame_id = self.scan_frame_id + self.scan_msg.angle_min = -math.pi + self.scan_msg.angle_max = math.pi + # 1.5 degrees + self.scan_msg.angle_increment = 0.0261799 + self.scan_msg.time_increment = 0.0 + self.scan_msg.scan_time = 0.1 + self.scan_msg.range_min = 0.05 + self.scan_msg.range_max = 30.0 + num_samples = int( + (self.scan_msg.angle_max - self.scan_msg.angle_min) / + self.scan_msg.angle_increment) + self.scan_msg.ranges = [0.0] * num_samples + self.getLaserScan(num_samples) + self.scan_pub.publish(self.scan_msg) def publishTransforms(self, map_to_odom, odom_to_base_link): map_to_odom.header.stamp = \ (self.get_clock().now() + Duration(seconds=self.update_dur)).to_msg() odom_to_base_link.header.stamp = self.get_clock().now().to_msg() - self.tf_broadcaster.sendTransform(map_to_odom) + if self.publish_map_odom_tf: + self.tf_broadcaster.sendTransform(map_to_odom) self.tf_broadcaster.sendTransform(odom_to_base_link) def publishOdometry(self, odom_to_base_link): @@ -209,6 +250,89 @@ def debug(self, msg): self.get_logger().debug(msg) return + def getMap(self): + request = GetMap.Request() + if self.map_client.wait_for_service(timeout_sec=5.0): + # Request to get map + future = self.map_client.call_async(request) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + self.map = future.result().map + self.get_logger().info('Laser scan will be populated using map data') + else: + self.get_logger().warn( + 'Failed to get map, ' + 'Laser scan will be populated using max range' + ) + else: + self.get_logger().warn( + 'Failed to get map, ' + 'Laser scan will be populated using max range' + ) + + def getLaserPose(self): + mat_map_to_odom = transformStampedToMatrix(self.t_map_to_odom) + mat_odom_to_base = transformStampedToMatrix(self.t_odom_to_base_link) + + mat_map_to_laser = tf_transformations.concatenate_matrices( + mat_map_to_odom, + mat_odom_to_base, + self.mat_base_to_laser + ) + transform = matrixToTransform(mat_map_to_laser) + + x = transform.translation.x + y = transform.translation.y + theta = tf_transformations.euler_from_quaternion([ + transform.rotation.x, + transform.rotation.y, + transform.rotation.z, + transform.rotation.w + ])[2] + + return x, y, theta + + def getLaserScan(self, num_samples): + if self.map is None or self.initial_pose is None or self.mat_base_to_laser is None: + self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples + return + + x0, y0, theta = self.getLaserPose() + + mx0, my0 = worldToMap(x0, y0, self.map) + + if not 0 < mx0 < self.map.info.width or not 0 < my0 < self.map.info.height: + # outside map + self.scan_msg.ranges = [self.scan_msg.range_max - 0.1] * num_samples + return + + for i in range(num_samples): + curr_angle = theta + self.scan_msg.angle_min + i * self.scan_msg.angle_increment + x1 = x0 + self.scan_msg.range_max * math.cos(curr_angle) + y1 = y0 + self.scan_msg.range_max * math.sin(curr_angle) + + mx1, my1 = worldToMap(x1, y1, self.map) + + line_iterator = LineIterator(mx0, my0, mx1, my1, 0.5) + + while line_iterator.isValid(): + mx, my = int(line_iterator.getX()), int(line_iterator.getY()) + + if not 0 < mx < self.map.info.width or not 0 < my < self.map.info.height: + # if outside map then check next ray + break + + point_cost = getMapOccupancy(mx, my, self.map) + + if point_cost >= 60: + self.scan_msg.ranges[i] = math.sqrt( + (int(line_iterator.getX()) - mx0) ** 2 + + (int(line_iterator.getY()) - my0) ** 2 + ) * self.map.info.resolution + break + + line_iterator.advance() + def main(): rclpy.init() diff --git a/nav2_loopback_sim/nav2_loopback_sim/utils.py b/nav2_loopback_sim/nav2_loopback_sim/utils.py index 0ed85689ddf..468103e94e8 100644 --- a/nav2_loopback_sim/nav2_loopback_sim/utils.py +++ b/nav2_loopback_sim/nav2_loopback_sim/utils.py @@ -13,6 +13,8 @@ # limitations under the License. +import math + from geometry_msgs.msg import Quaternion, Transform import numpy as np import tf_transformations @@ -63,3 +65,13 @@ def matrixToTransform(matrix): transform.rotation.z = quaternion[2] transform.rotation.w = quaternion[3] return transform + + +def worldToMap(world_x, world_y, map_msg): + map_x = int(math.floor((world_x - map_msg.info.origin.position.x) / map_msg.info.resolution)) + map_y = int(math.floor((world_y - map_msg.info.origin.position.y) / map_msg.info.resolution)) + return map_x, map_y + + +def getMapOccupancy(x, y, map_msg): + return map_msg.data[y * map_msg.info.width + x] diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index d9da9fd1474..e2a90a06eff 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -4,29 +4,23 @@ project(nav2_smac_planner) set(CMAKE_BUILD_TYPE Release) #Debug, Release find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(angles REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(std_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(nav2_util REQUIRED) find_package(nav2_core REQUIRED) -find_package(nav2_msgs 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(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(angles REQUIRED) +find_package(nav2_util REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(nlohmann_json REQUIRED) find_package(ompl REQUIRED) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) if(MSVC) add_compile_definitions(_USE_MATH_DEFINES) @@ -34,107 +28,144 @@ else() add_compile_options(-O3 -Wextra -Wdeprecated -fPIC) endif() -include_directories( - include - ${OMPL_INCLUDE_DIRS} -) - set(library_name nav2_smac_planner) -set(dependencies - rclcpp - rclcpp_action - rclcpp_lifecycle - std_msgs - visualization_msgs - nav2_util - nav2_msgs - nav_msgs - geometry_msgs - builtin_interfaces - tf2_ros - nav2_costmap_2d - nav2_core - pluginlib - angles - eigen3_cmake_module -) - -# Hybrid plugin -add_library(${library_name} SHARED - src/smac_planner_hybrid.cpp +# Common library +add_library(${library_name}_common SHARED src/a_star.cpp - src/collision_checker.cpp - src/smoother.cpp src/analytic_expansion.cpp - src/node_hybrid.cpp - src/node_lattice.cpp + src/collision_checker.cpp src/costmap_downsampler.cpp src/node_2d.cpp src/node_basic.cpp + src/node_hybrid.cpp + src/node_lattice.cpp + src/smoother.cpp +) +target_include_directories(${library_name}_common + PUBLIC + "$" + "$" + ${OMPL_INCLUDE_DIRS} +) +target_link_libraries(${library_name}_common PUBLIC + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::layers + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + nlohmann_json::nlohmann_json + ${OMPL_LIBRARIES} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + tf2::tf2 + ${visualization_msgs_TARGETS} +) +target_link_libraries(${library_name}_common PRIVATE + angles::angles ) -target_link_libraries(${library_name} ${OMPL_LIBRARIES}) -target_include_directories(${library_name} PUBLIC ${Eigen3_INCLUDE_DIRS}) - -ament_target_dependencies(${library_name} - ${dependencies} +# Hybrid plugin +add_library(${library_name} SHARED + src/smac_planner_hybrid.cpp +) +target_include_directories(${library_name} + PUBLIC + "$" + "$" + ${OMPL_INCLUDE_DIRS} +) +target_link_libraries(${library_name} PUBLIC + ${library_name}_common + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + nlohmann_json::nlohmann_json + ${OMPL_LIBRARIES} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + tf2::tf2 + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} +) +target_link_libraries(${library_name} PRIVATE + pluginlib::pluginlib ) # 2D plugin add_library(${library_name}_2d SHARED src/smac_planner_2d.cpp - src/a_star.cpp - src/smoother.cpp - src/collision_checker.cpp - src/analytic_expansion.cpp - src/node_hybrid.cpp - src/node_lattice.cpp - src/costmap_downsampler.cpp - src/node_2d.cpp - src/node_basic.cpp ) - -target_link_libraries(${library_name}_2d ${OMPL_LIBRARIES}) -target_include_directories(${library_name}_2d PUBLIC ${Eigen3_INCLUDE_DIRS}) - -ament_target_dependencies(${library_name}_2d - ${dependencies} +target_include_directories(${library_name}_2d + PUBLIC + "$" + "$" + ${OMPL_INCLUDE_DIRS} +) +target_link_libraries(${library_name}_2d PUBLIC + ${library_name}_common + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + nlohmann_json::nlohmann_json + ${OMPL_LIBRARIES} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + tf2::tf2 + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} +) +target_link_libraries(${library_name}_2d PRIVATE + pluginlib::pluginlib ) # Lattice plugin add_library(${library_name}_lattice SHARED src/smac_planner_lattice.cpp - src/a_star.cpp - src/smoother.cpp - src/collision_checker.cpp - src/analytic_expansion.cpp - src/node_hybrid.cpp - src/node_lattice.cpp - src/costmap_downsampler.cpp - src/node_2d.cpp - src/node_basic.cpp ) - -target_link_libraries(${library_name}_lattice ${OMPL_LIBRARIES}) -target_include_directories(${library_name}_lattice PUBLIC ${Eigen3_INCLUDE_DIRS}) - -ament_target_dependencies(${library_name}_lattice - ${dependencies} +target_include_directories(${library_name}_lattice + PUBLIC + "$" + "$" + ${OMPL_INCLUDE_DIRS} +) +target_link_libraries(${library_name}_lattice PUBLIC + ${library_name}_common + ${geometry_msgs_TARGETS} + nav2_core::nav2_core + nav2_costmap_2d::nav2_costmap_2d_core + ${nav_msgs_TARGETS} + nlohmann_json::nlohmann_json + ${OMPL_LIBRARIES} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + ${rcl_interfaces_TARGETS} + tf2::tf2 + tf2_ros::tf2_ros + ${visualization_msgs_TARGETS} +) +target_link_libraries(${library_name}_lattice PRIVATE + ament_index_cpp::ament_index_cpp + pluginlib::pluginlib ) pluginlib_export_plugin_description_file(nav2_core smac_plugin_hybrid.xml) pluginlib_export_plugin_description_file(nav2_core smac_plugin_2d.xml) pluginlib_export_plugin_description_file(nav2_core smac_plugin_lattice.xml) -install(TARGETS ${library_name} ${library_name}_2d ${library_name}_lattice +install(TARGETS ${library_name}_common ${library_name} ${library_name}_2d ${library_name}_lattice + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) install(DIRECTORY lattice_primitives/sample_primitives DESTINATION share/${PROJECT_NAME}) @@ -144,10 +175,25 @@ if(BUILD_TESTING) set(AMENT_LINT_AUTO_FILE_EXCLUDE include/nav2_smac_planner/thirdparty/robin_hood.h) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) + ament_find_gtest() add_subdirectory(test) endif() -ament_export_include_directories(include ${OMPL_INCLUDE_DIRS}) +ament_export_include_directories(include/${PROJECT_NAME}) ament_export_libraries(${library_name} ${library_name}_2d ${library_name}_lattice) -ament_export_dependencies(${dependencies}) +ament_export_dependencies( + geometry_msgs + nav2_core + nav2_costmap_2d + nav_msgs + nlohmann_json + ompl + rclcpp + rclcpp_lifecycle + rcl_interfaces + tf2 + tf2_ros + visualization_msgs +) +ament_export_targets(${PROJECT_NAME}) ament_package() 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 c95d8c82cff..a3ae1f6c7b2 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -16,14 +16,12 @@ #ifndef NAV2_SMAC_PLANNER__A_STAR_HPP_ #define NAV2_SMAC_PLANNER__A_STAR_HPP_ -#include -#include -#include +#include #include #include -#include #include -#include "Eigen/Core" +#include +#include #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_core/planner_exceptions.hpp" 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 76719d72313..c4d3deccda7 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -15,10 +15,11 @@ #ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ #define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ -#include -#include +#include #include #include +#include +#include #include "nav2_smac_planner/node_2d.hpp" #include "nav2_smac_planner/node_hybrid.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp index 205c3f0471a..5b31ee87cd2 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include #include +#include #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp b/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp index 332507d7c2a..21189170298 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/costmap_downsampler.hpp @@ -15,9 +15,8 @@ #ifndef NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ #define NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_ -#include -#include #include +#include #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_smac_planner/constants.hpp" 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 3ccadefdedb..2ef090fe498 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -15,14 +15,10 @@ #ifndef NAV2_SMAC_PLANNER__NODE_2D_HPP_ #define NAV2_SMAC_PLANNER__NODE_2D_HPP_ -#include -#include -#include -#include -#include -#include -#include #include +#include +#include +#include #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" 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 dfb26c5ddaf..d7b7d72409f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_basic.hpp @@ -15,18 +15,6 @@ #ifndef NAV2_SMAC_PLANNER__NODE_BASIC_HPP_ #define NAV2_SMAC_PLANNER__NODE_BASIC_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "ompl/base/StateSpace.h" - #include "nav2_smac_planner/constants.hpp" #include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/node_lattice.hpp" 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 b67ed978ec8..cf6a9a6e89e 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -15,15 +15,10 @@ #ifndef NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ #define NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_ -#include -#include -#include -#include #include -#include #include #include -#include +#include #include "ompl/base/StateSpace.h" 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 f31ccb9b0d2..f3e3ff37f9a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -15,21 +15,12 @@ #ifndef NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_ #define NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_ -#include - -#include -#include -#include #include -#include #include -#include -#include #include +#include -#include "nlohmann/json.hpp" #include "ompl/base/StateSpace.h" -#include "angles/angles.h" #include "nav2_smac_planner/constants.hpp" #include "nav2_smac_planner/types.hpp" diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index d896cd90b5f..89ecc0b0926 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -15,22 +15,14 @@ #ifndef NAV2_SMAC_PLANNER__SMOOTHER_HPP_ #define NAV2_SMAC_PLANNER__SMOOTHER_HPP_ -#include #include -#include -#include -#include -#include #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav_msgs/msg/path.hpp" -#include "angles/angles.h" -#include "tf2/utils.h" #include "ompl/base/StateSpace.h" -#include "ompl/base/spaces/DubinsStateSpace.h" namespace nav2_smac_planner { diff --git a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp index fb020a729a1..89cb2f8dc77 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/utils.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/utils.hpp @@ -21,7 +21,6 @@ #include #include "nlohmann/json.hpp" -#include "Eigen/Core" #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/pose.hpp" #include "tf2/utils.h" diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 0a1303bd682..d73d553496b 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -8,31 +8,29 @@ Apache-2.0 ament_cmake + nav2_common - rclcpp - rclcpp_action - rclcpp_lifecycle - visualization_msgs - nav2_util - nav2_msgs - nav_msgs + ament_index_cpp + angles + eigen geometry_msgs - builtin_interfaces - nav2_common - tf2_ros - nav2_costmap_2d nav2_core - pluginlib - eigen3_cmake_module - eigen - ompl + nav2_costmap_2d + nav2_util + nav_msgs nlohmann-json-dev - angles + ompl + pluginlib + rclcpp + rclcpp_lifecycle + rcl_interfaces + tf2_ros + tf2 + visualization_msgs ament_lint_common ament_lint_auto ament_cmake_gtest - ament_cmake_pytest ament_cmake diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index f3ddc9adb80..6c794b9a987 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -12,16 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include +#include #include -#include +#include +#include +#include #include -#include #include -#include #include -#include -#include +#include + +#include "angles/angles.h" #include "ompl/base/ScopedState.h" #include "ompl/base/spaces/DubinsStateSpace.h" diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 5ba10d7d9a0..15491c051ea 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -19,7 +19,6 @@ #include #include -#include "Eigen/Core" #include "nav2_smac_planner/smac_planner_hybrid.hpp" // #define BENCHMARK_TESTING diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index c5a9ecdff60..ebeead9c5c4 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -18,7 +18,6 @@ #include #include -#include "Eigen/Core" #include "nav2_smac_planner/smac_planner_lattice.hpp" // #define BENCHMARK_TESTING diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index a69e14011f3..fb43f4a1215 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/nav2_smac_planner/src/smoother.cpp @@ -14,8 +14,15 @@ #include #include -#include + +#include #include +#include + +#include "angles/angles.h" + +#include "tf2/utils.h" + #include "nav2_smac_planner/smoother.hpp" namespace nav2_smac_planner diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt index 8039cf9c51e..fc4abf20c75 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -2,128 +2,136 @@ ament_add_gtest(test_utils test_utils.cpp ) -ament_target_dependencies(test_utils - ${dependencies} -) target_link_libraries(test_utils ${library_name} + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core ) # Test costmap downsampler ament_add_gtest(test_costmap_downsampler test_costmap_downsampler.cpp ) -ament_target_dependencies(test_costmap_downsampler - ${dependencies} -) target_link_libraries(test_costmap_downsampler ${library_name} + nav2_costmap_2d::nav2_costmap_2d_core + nav2_util::nav2_util_core + rclcpp::rclcpp ) # Test Node2D ament_add_gtest(test_node2d test_node2d.cpp ) -ament_target_dependencies(test_node2d - ${dependencies} -) target_link_libraries(test_node2d ${library_name} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test NodeHybrid ament_add_gtest(test_nodehybrid test_nodehybrid.cpp ) -ament_target_dependencies(test_nodehybrid - ${dependencies} -) target_link_libraries(test_nodehybrid ${library_name} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test NodeBasic ament_add_gtest(test_nodebasic test_nodebasic.cpp ) -ament_target_dependencies(test_nodebasic - ${dependencies} -) target_link_libraries(test_nodebasic ${library_name} + rclcpp::rclcpp ) # Test collision checker ament_add_gtest(test_collision_checker test_collision_checker.cpp ) -ament_target_dependencies(test_collision_checker - ${dependencies} -) target_link_libraries(test_collision_checker ${library_name} + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test A* ament_add_gtest(test_a_star test_a_star.cpp ) -ament_target_dependencies(test_a_star - ${dependencies} -) target_link_libraries(test_a_star ${library_name} + ament_index_cpp::ament_index_cpp + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test SMAC Hybrid ament_add_gtest(test_smac_hybrid test_smac_hybrid.cpp ) -ament_target_dependencies(test_smac_hybrid - ${dependencies} -) target_link_libraries(test_smac_hybrid ${library_name} + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test SMAC 2D ament_add_gtest(test_smac_2d test_smac_2d.cpp ) -ament_target_dependencies(test_smac_2d - ${dependencies} -) target_link_libraries(test_smac_2d ${library_name}_2d + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test SMAC lattice ament_add_gtest(test_smac_lattice test_smac_lattice.cpp ) -ament_target_dependencies(test_smac_lattice - ${dependencies} -) target_link_libraries(test_smac_lattice ${library_name}_lattice + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) # Test SMAC Smoother ament_add_gtest(test_smoother test_smoother.cpp ) -ament_target_dependencies(test_smoother - ${dependencies} -) target_link_libraries(test_smoother ${library_name}_lattice ${library_name} ${library_name}_2d + ament_index_cpp::ament_index_cpp + ${geometry_msgs_TARGETS} + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) -#Test Lattice node +# Test Lattice node ament_add_gtest(test_lattice_node test_nodelattice.cpp) - -ament_target_dependencies(test_lattice_node ${dependencies}) - -target_link_libraries(test_lattice_node ${library_name}) +target_link_libraries(test_lattice_node + ${library_name} + ament_index_cpp::ament_index_cpp + nav2_costmap_2d::nav2_costmap_2d_core + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/nav2_smac_planner/test/test_collision_checker.cpp b/nav2_smac_planner/test/test_collision_checker.cpp index dd4e032a51e..92590bd945f 100644 --- a/nav2_smac_planner/test/test_collision_checker.cpp +++ b/nav2_smac_planner/test/test_collision_checker.cpp @@ -19,7 +19,6 @@ #include "gtest/gtest.h" #include "nav2_smac_planner/collision_checker.hpp" -#include "nav2_util/lifecycle_node.hpp" using namespace nav2_costmap_2d; // NOLINT diff --git a/nav2_smac_planner/test/test_nodebasic.cpp b/nav2_smac_planner/test/test_nodebasic.cpp index 00aea824650..544842cec68 100644 --- a/nav2_smac_planner/test/test_nodebasic.cpp +++ b/nav2_smac_planner/test/test_nodebasic.cpp @@ -19,9 +19,6 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/costmap_subscriber.hpp" -#include "nav2_util/lifecycle_node.hpp" #include "nav2_smac_planner/node_basic.hpp" #include "nav2_smac_planner/node_2d.hpp" #include "nav2_smac_planner/node_hybrid.hpp"