From 8b343bd5f8f39a931b03ae8eb33639c425c1efad Mon Sep 17 00:00:00 2001 From: RyuYamamoto Date: Mon, 17 Jun 2024 10:09:53 +0900 Subject: [PATCH 1/3] add local costmap --- .../config/navyu_costmap_2d_params.yaml | 20 +++++++++- .../navyu_costmap_2d/navyu_costmap_2d.hpp | 11 +++++- .../plugins/dynamic_layer.hpp | 29 ++++++++------ .../plugins/inflation_layer.hpp | 31 ++++++++------- .../navyu_costmap_2d/plugins/layer.hpp | 2 +- .../navyu_costmap_2d/plugins/static_layer.hpp | 10 ++++- .../launch/navyu_costmap_2d.launch.py | 22 ++++------- navyu_costmap_2d/src/navyu_costmap_2d.cpp | 31 +++++++++++---- navyu_navigation/config/navyu_params.yaml | 20 +++++++++- .../launch/navyu_bringup.launch.py | 10 +++++ navyu_navigation/rviz/navyu.rviz | 39 +++++++++++++++---- 11 files changed, 161 insertions(+), 64 deletions(-) diff --git a/navyu_costmap_2d/config/navyu_costmap_2d_params.yaml b/navyu_costmap_2d/config/navyu_costmap_2d_params.yaml index 7aa46bb..d320a8d 100644 --- a/navyu_costmap_2d/config/navyu_costmap_2d_params.yaml +++ b/navyu_costmap_2d/config/navyu_costmap_2d_params.yaml @@ -1,8 +1,7 @@ global_costmap_node: ros__parameters: update_frequency: 1.0 - base_frame_id: base_link - map_frame_id: map + global_frame_id: map plugins: [static_layer, dynamic_layer, inflation_layer] inflation_layer: inflation_radius: 0.55 @@ -14,3 +13,20 @@ global_costmap_node: global_frame: map min_laser_range: 0.0 max_laser_range: 5.0 + +local_costmap_node: + ros__parameters: + update_frequency: 5.0 + global_frame_id: base_link + width: 3 + height: 3 + resolution: 0.05 + plugins: [dynamic_layer, inflation_layer] + inflation_layer: + inflation_radius: 0.55 + robot_radius: 0.22 + dynamic_layer: + scan_topic: scan + global_frame: base_link + min_laser_range: 0.0 + max_laser_range: 5.0 diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp index 617e85f..10af787 100644 --- a/navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp +++ b/navyu_costmap_2d/include/navyu_costmap_2d/navyu_costmap_2d.hpp @@ -40,9 +40,16 @@ class NavyuCostmap2D : public rclcpp::Node std::vector plugins_; std::map> layer_function_; + nav_msgs::msg::OccupancyGrid master_costmap_; + double update_frequency_; - std::string base_frame_id_; - std::string map_frame_id_; + std::string global_frame_id_; + + double resolution_; + double origin_x_; + double origin_y_; + int size_x_; + int size_y_; }; #endif // NAVYU_COSTMAP_2D__NAVYU_COSTMAP_2D_HPP_ diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/dynamic_layer.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/dynamic_layer.hpp index 416599e..59f5cbe 100644 --- a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/dynamic_layer.hpp +++ b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/dynamic_layer.hpp @@ -56,15 +56,11 @@ class DynamicLayer : public Layer void callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { scan_ = msg; } - void update(nav_msgs::msg::OccupancyGrid & master_costmap) override + bool update(nav_msgs::msg::OccupancyGrid & master_costmap) override { if (scan_ == nullptr) { RCLCPP_ERROR_STREAM(node_->get_logger(), "scan is empty."); - return; - } - if (master_costmap.data.empty()) { - RCLCPP_ERROR_STREAM(node_->get_logger(), "master costmap is empty."); - return; + return false; } // scan range filter @@ -81,7 +77,7 @@ class DynamicLayer : public Layer geometry_msgs::msg::TransformStamped map_to_sensor_frame; if (!get_transform(global_frame_, scan_->header.frame_id, map_to_sensor_frame)) { RCLCPP_ERROR_STREAM(node_->get_logger(), "Can not get frame."); - return; + return false; } pcl::PointCloud::Ptr laser_cloud(new pcl::PointCloud); @@ -89,9 +85,13 @@ class DynamicLayer : public Layer pcl::fromROSMsg(cloud_msg, *laser_cloud); // transform laser cloud - const Eigen::Affine3d affine = tf2::transformToEigen(map_to_sensor_frame); - const Eigen::Matrix4f matrix = affine.matrix().cast(); - pcl::transformPointCloud(*laser_cloud, *transform_cloud, matrix); + if (scan_->header.frame_id != global_frame_) { + const Eigen::Affine3d affine = tf2::transformToEigen(map_to_sensor_frame); + const Eigen::Matrix4f matrix = affine.matrix().cast(); + pcl::transformPointCloud(*laser_cloud, *transform_cloud, matrix); + } else { + transform_cloud = laser_cloud; + } // get costmap info const double resolution = master_costmap.info.resolution; @@ -99,15 +99,20 @@ class DynamicLayer : public Layer const int height = master_costmap.info.height; const double origin_x = master_costmap.info.origin.position.x; const double origin_y = master_costmap.info.origin.position.y; + master_costmap.data.resize(width * height); // calculation grid index for (auto cloud : transform_cloud->points) { const int ix = static_cast((cloud.x - origin_x) / resolution); const int iy = static_cast((cloud.y - origin_y) / resolution); - int index = ix + width * iy; - master_costmap.data[index] = LETHAL_COST; + if (0 <= ix and ix < width and 0 <= iy and iy < height) { + int index = ix + width * iy; + master_costmap.data[index] = LETHAL_COST; + } } + + return true; } bool get_transform( diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp index 6307dec..9636e39 100644 --- a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp +++ b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/inflation_layer.hpp @@ -30,11 +30,11 @@ class InflationLayer : public Layer node_->get_parameter("inflation_layer.robot_radius", robot_radius_); } - void update(nav_msgs::msg::OccupancyGrid & master_costmap) override + bool update(nav_msgs::msg::OccupancyGrid & master_costmap) override { if (master_costmap.data.empty()) { RCLCPP_ERROR_STREAM(node_->get_logger(), "costmap is empty."); - return; + return false; } const int size_x = master_costmap.info.width; @@ -49,11 +49,14 @@ class InflationLayer : public Layer } } } + + return true; } void expand_cost(nav_msgs::msg::OccupancyGrid & costmap, const int mx, const int my) { const int size_x = costmap.info.width; + const int size_y = costmap.info.height; const double resolution = costmap.info.resolution; const int cell_inflation_radius = std::ceil(inflation_radius_ / resolution); @@ -64,18 +67,20 @@ class InflationLayer : public Layer for (int y = min_y; y < max_y; y++) { for (int x = min_x; x < max_x; x++) { - const double distance = std::hypot(x - mx, y - my); - const int index = x + size_x * y; - const auto old_cost = costmap.data[index]; + if (0 <= x and x < size_x and 0 <= y and y < size_y) { + const double distance = std::hypot(x - mx, y - my); + const int index = x + size_x * y; + const auto old_cost = costmap.data[index]; - if (distance * resolution < robot_radius_) { - costmap.data[index] = std::max(INSCRIBED_COST, old_cost); - } else { - if (cell_inflation_radius < distance) continue; - if (costmap.data[index] == -1) continue; - const int8_t new_cost = - std::exp(-1 * 3.0 * (distance * resolution - robot_radius_)) * INSCRIBED_COST - 1; - costmap.data[index] = std::max(new_cost, old_cost); + if (distance * resolution < robot_radius_) { + costmap.data[index] = std::max(INSCRIBED_COST, old_cost); + } else { + if (cell_inflation_radius < distance) continue; + if (costmap.data[index] == -1) continue; + const int8_t new_cost = + std::exp(-1 * 3.0 * (distance * resolution - robot_radius_)) * INSCRIBED_COST - 1; + costmap.data[index] = std::max(new_cost, old_cost); + } } } } diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp index d7fa6fc..331ec80 100644 --- a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp +++ b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/layer.hpp @@ -36,7 +36,7 @@ class Layer listener_ = listener; }; virtual void configure() = 0; - virtual void update(nav_msgs::msg::OccupancyGrid & master_costmap) = 0; + virtual bool update(nav_msgs::msg::OccupancyGrid & master_costmap) = 0; protected: rclcpp::Node * node_; diff --git a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp index 3f1f75e..0f741dc 100644 --- a/navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp +++ b/navyu_costmap_2d/include/navyu_costmap_2d/plugins/static_layer.hpp @@ -36,7 +36,15 @@ class StaticLayer : public Layer void callback_map(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { map_ = *msg; } - void update(nav_msgs::msg::OccupancyGrid & master_costmap) override { master_costmap = map_; } + bool update(nav_msgs::msg::OccupancyGrid & master_costmap) override + { + if (map_.data.empty()) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "map is empty."); + return false; + } + master_costmap = map_; + return true; + } private: rclcpp::Subscription::SharedPtr map_subscriber_; diff --git a/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py b/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py index 5bf9e59..1e671bd 100644 --- a/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py +++ b/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py @@ -38,25 +38,17 @@ def generate_launch_description(): executable="navyu_costmap_2d_node", name="global_costmap_node", output="screen", + remappings=[("/costmap", "/global_costmap")], parameters=[costmap_map_config_path, {"use_sim_time": use_sim_time}], ), Node( - package="nav2_map_server", - executable="map_server", - name="map_server", - parameters=[{"yaml_filename": map_path}, {"frame_id": "map"}], - ), - Node( - package="nav2_lifecycle_manager", - executable="lifecycle_manager", - name="map_server_lifecycle_manager", + package="navyu_costmap_2d", + executable="navyu_costmap_2d_node", + name="local_costmap_node", output="screen", - emulate_tty=True, - parameters=[ - {"use_sim_time": True}, - {"autostart": True}, - {"node_names": ["map_server"]}, - ], + remappings=[("/costmap", "/local_costmap")], + parameters=[costmap_map_config_path, {"use_sim_time": use_sim_time}], ), + ] ) diff --git a/navyu_costmap_2d/src/navyu_costmap_2d.cpp b/navyu_costmap_2d/src/navyu_costmap_2d.cpp index c3777b0..224d040 100644 --- a/navyu_costmap_2d/src/navyu_costmap_2d.cpp +++ b/navyu_costmap_2d/src/navyu_costmap_2d.cpp @@ -25,8 +25,21 @@ NavyuCostmap2D::NavyuCostmap2D(const rclcpp::NodeOptions & node_options) : Node("navyu_costmap_2d", node_options) { update_frequency_ = declare_parameter("update_frequency"); - base_frame_id_ = declare_parameter("base_frame_id"); - map_frame_id_ = declare_parameter("map_frame_id"); + global_frame_id_ = declare_parameter("global_frame_id"); + + resolution_ = declare_parameter("resolution", 0.05); + origin_x_ = declare_parameter("origin_x", 0.0); + origin_y_ = declare_parameter("origin_y", 0.0); + size_x_ = declare_parameter("width", 5); + size_y_ = declare_parameter("height", 5); + + master_costmap_.header.frame_id = global_frame_id_; + master_costmap_.info.resolution = resolution_; + master_costmap_.info.width = static_cast(size_x_ / resolution_); + master_costmap_.info.height = static_cast(size_y_ / resolution_); + master_costmap_.info.origin.position.x = origin_x_ - static_cast(size_x_) / 2.0; + master_costmap_.info.origin.position.y = origin_y_ - static_cast(size_y_) / 2.0; + master_costmap_.data.resize(master_costmap_.info.width * master_costmap_.info.height); plugins_ = declare_parameter>("plugins"); @@ -61,22 +74,24 @@ NavyuCostmap2D::~NavyuCostmap2D() void NavyuCostmap2D::update() { - nav_msgs::msg::OccupancyGrid master_costmap; + master_costmap_.data.clear(); // update costmap for (auto & plugin : plugins_) { - layer_function_[plugin]->update(master_costmap); + if (!layer_function_[plugin]->update(master_costmap_)) { + RCLCPP_ERROR_STREAM(get_logger(), "Can not update costmap. " << plugin.c_str()); + return; + } } - if (master_costmap.data.empty()) { + if (master_costmap_.data.empty()) { RCLCPP_ERROR_STREAM(get_logger(), "master costmap is empty"); return; } - master_costmap.header.stamp = now(); - master_costmap.header.frame_id = map_frame_id_; + master_costmap_.header.stamp = now(); - costmap_publisher_->publish(master_costmap); + costmap_publisher_->publish(master_costmap_); } #include "rclcpp_components/register_node_macro.hpp" diff --git a/navyu_navigation/config/navyu_params.yaml b/navyu_navigation/config/navyu_params.yaml index 668aa89..d078df3 100644 --- a/navyu_navigation/config/navyu_params.yaml +++ b/navyu_navigation/config/navyu_params.yaml @@ -1,8 +1,7 @@ navyu_global_costmap_node: ros__parameters: update_frequency: 1.0 - base_frame_id: base_link - map_frame_id: map + global_frame_id: map plugins: [static_layer, dynamic_layer, inflation_layer] inflation_layer: inflation_radius: 0.55 @@ -15,6 +14,23 @@ navyu_global_costmap_node: min_laser_range: 0.0 max_laser_range: 5.0 +navyu_local_costmap_node: + ros__parameters: + update_frequency: 1.0 + global_frame_id: base_link + width: 5 + height: 5 + resolution: 0.05 + plugins: [dynamic_layer, inflation_layer] + inflation_layer: + inflation_radius: 0.55 + robot_radius: 0.22 + dynamic_layer: + scan_topic: scan + global_frame: base_link + min_laser_range: 0.0 + max_laser_range: 5.0 + navyu_global_planner_node: ros__parameters: map_frame: map diff --git a/navyu_navigation/launch/navyu_bringup.launch.py b/navyu_navigation/launch/navyu_bringup.launch.py index 4c94a1e..54a58b3 100644 --- a/navyu_navigation/launch/navyu_bringup.launch.py +++ b/navyu_navigation/launch/navyu_bringup.launch.py @@ -52,12 +52,22 @@ def generate_launch_description(): package="navyu_costmap_2d", executable="navyu_costmap_2d_node", name="navyu_global_costmap_node", + remappings=[("/costmap", "/global_costmap")], parameters=[navyu_config_path, {"use_sim_time": use_sim_time}], ), + Node( + package="navyu_costmap_2d", + executable="navyu_costmap_2d_node", + name="navyu_local_costmap_node", + remappings=[("/costmap", "/local_costmap")], + parameters=[navyu_config_path, {"use_sim_time": use_sim_time}], + ), + Node( package="navyu_path_planner", executable="navyu_global_planner_node", name="navyu_global_planner_node", + remappings=[("/costmap", "/global_costmap")], parameters=[navyu_config_path, {"use_sim_time": use_sim_time}], ), Node( diff --git a/navyu_navigation/rviz/navyu.rviz b/navyu_navigation/rviz/navyu.rviz index 3c1f366..4852261 100644 --- a/navyu_navigation/rviz/navyu.rviz +++ b/navyu_navigation/rviz/navyu.rviz @@ -6,6 +6,8 @@ Panels: Expanded: - /Global Options1 - /Grid1 + - /Local Costmap1/Status1 + - /Local Costmap1/Topic1 Splitter Ratio: 0.5 Tree Height: 1079 - Class: rviz_common/Selection @@ -257,13 +259,34 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /costmap + Value: /global_costmap Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /costmap_updates + Value: /global_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap_updates Use Timestamp: false Value: true - Alpha: 1 @@ -371,7 +394,7 @@ Visualization Manager: Enabled: true Name: Safety Status Namespaces: - text: true + {} Topic: Depth: 5 Durability Policy: Volatile @@ -435,11 +458,11 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 158.93878173828125 + Scale: 124.54267883300781 Target Frame: base_link Value: TopDownOrtho (rviz_default_plugins) - X: 0.2840563654899597 - Y: -0.15071021020412445 + X: 1.049617052078247 + Y: -0.3355798125267029 Saved: ~ Window Geometry: Displays: @@ -447,7 +470,7 @@ Window Geometry: Height: 1376 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006a10000003efc0100000002fb0000000800540069006d00650100000000000006a1000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006a1000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000003b200000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006a10000003efc0100000002fb0000000800540069006d00650100000000000006a1000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006a1000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -458,4 +481,4 @@ Window Geometry: collapsed: true Width: 1697 X: 46 - Y: 27 + Y: 973 From a154fe3649097c738b75bc65d93090a0f205d2ec Mon Sep 17 00:00:00 2001 From: RyuYamamoto Date: Mon, 17 Jun 2024 10:10:17 +0900 Subject: [PATCH 2/3] fix pre-commit --- navyu_costmap_2d/launch/navyu_costmap_2d.launch.py | 1 - navyu_navigation/launch/navyu_bringup.launch.py | 1 - 2 files changed, 2 deletions(-) diff --git a/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py b/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py index 1e671bd..06f8676 100644 --- a/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py +++ b/navyu_costmap_2d/launch/navyu_costmap_2d.launch.py @@ -49,6 +49,5 @@ def generate_launch_description(): remappings=[("/costmap", "/local_costmap")], parameters=[costmap_map_config_path, {"use_sim_time": use_sim_time}], ), - ] ) diff --git a/navyu_navigation/launch/navyu_bringup.launch.py b/navyu_navigation/launch/navyu_bringup.launch.py index 54a58b3..25511ba 100644 --- a/navyu_navigation/launch/navyu_bringup.launch.py +++ b/navyu_navigation/launch/navyu_bringup.launch.py @@ -62,7 +62,6 @@ def generate_launch_description(): remappings=[("/costmap", "/local_costmap")], parameters=[navyu_config_path, {"use_sim_time": use_sim_time}], ), - Node( package="navyu_path_planner", executable="navyu_global_planner_node", From 64a63f90da9aff527a35d6e5a12bcb93428f4696 Mon Sep 17 00:00:00 2001 From: RyuYamamoto Date: Mon, 17 Jun 2024 11:00:22 +0900 Subject: [PATCH 3/3] remove backward ros --- navyu_safety_limiter/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/navyu_safety_limiter/CMakeLists.txt b/navyu_safety_limiter/CMakeLists.txt index 6f1a316..d7d70a3 100644 --- a/navyu_safety_limiter/CMakeLists.txt +++ b/navyu_safety_limiter/CMakeLists.txt @@ -10,7 +10,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic ) endif() -find_package(backward_ros REQUIRED) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() @@ -23,8 +22,6 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_node ) -add_backward(${PROJECT_NAME}) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies()