From e65cf9ce8ece40db3536d27d3e66f6703f7659a4 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 18 Aug 2022 10:58:15 -0700 Subject: [PATCH] Fix Costmap Filters system tests (#3120) * Fix Costmap Filters system tests * Update map_io.cpp Co-authored-by: Alexey Merzlyakov --- nav2_map_server/src/map_io.cpp | 2 +- nav2_system_tests/src/costmap_filters/keepout_params.yaml | 1 + nav2_system_tests/src/costmap_filters/speed_global_params.yaml | 2 +- nav2_system_tests/src/costmap_filters/speed_local_params.yaml | 2 +- nav2_system_tests/src/costmap_filters/test_keepout_launch.py | 2 +- nav2_system_tests/src/costmap_filters/test_speed_launch.py | 2 +- 6 files changed, 6 insertions(+), 5 deletions(-) diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 050fc63d66e..4803b6d3f64 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -504,7 +504,7 @@ void tryWriteMapToFile( tf2::Matrix3x3 mat(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)); double yaw, pitch, roll; mat.getEulerYPR(yaw, pitch, roll); - + const int file_name_index = mapdatafile.find_last_of("/\\"); std::string image_name = mapdatafile.substr(file_name_index + 1); diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index b4f98cedea1..22b3b14e6be 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -124,6 +124,7 @@ controller_server: FollowPath: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: True + prune_distance: 1.0 min_vel_x: 0.0 min_vel_y: 0.0 max_vel_x: 0.26 diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index 1e021bb4f7d..ff0de1abdaa 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -248,7 +248,7 @@ map_server: map_saver: ros__parameters: use_sim_time: True - save_map_timeout: 5000 + save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 map_subscribe_transient_local: True diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index fe03fe138d5..a0a2024abb2 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -248,7 +248,7 @@ map_server: map_saver: ros__parameters: use_sim_time: True - save_map_timeout: 5000 + save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 map_subscribe_transient_local: True diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index d8ad1123921..796667c65d7 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -89,7 +89,7 @@ def generate_launch_description(): parameters=[{ 'node_names': [ - 'filter_mask_server', 'costmap_filter_info_server', 'bt_navigator' + 'filter_mask_server', 'costmap_filter_info_server' ] }, {'autostart': True}]), diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index 2603ea0df11..5b0c61ed9d7 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -87,7 +87,7 @@ def generate_launch_description(): parameters=[{ 'node_names': [ - 'filter_mask_server', 'costmap_filter_info_server', 'bt_navigator' + 'filter_mask_server', 'costmap_filter_info_server' ] }, {'autostart': True}]),