From 19a66904c648a174220be1a1b98e81a9ef0e6758 Mon Sep 17 00:00:00 2001 From: Michael Jonathan Date: Sat, 29 Jun 2024 01:16:18 +0700 Subject: [PATCH] Humble min laser range parameters (#713) * ignore vscode folder in gitignore * add min_laser_range feature from parameters in laser_utils.cpp file * add info related to min_laser_range in README.MD and add sample parameters to all .yaml file in config folder * Update laser_utils.cpp, fix initial min_laser_range value from 25 to 0.0 --- .gitignore | 1 + README.md | 2 ++ config/mapper_params_lifelong.yaml | 1 + config/mapper_params_localization.yaml | 1 + config/mapper_params_offline.yaml | 1 + config/mapper_params_online_async.yaml | 1 + config/mapper_params_online_sync.yaml | 1 + src/laser_utils.cpp | 23 ++++++++++++++++++++++- 8 files changed, 30 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index fd13d280..d9d10a8a 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ snap_ws/* +.vscode/* diff --git a/README.md b/README.md index e7c155e3..b72c6dcc 100644 --- a/README.md +++ b/README.md @@ -248,6 +248,8 @@ The following settings and options are exposed to you. My default configuration `resolution` - Resolution of the 2D occupancy map to generate +`min_laser_range` - Minimum laser range to use for 2D occupancy map rasterizing + `max_laser_range` - Maximum laser range to use for 2D occupancy map rasterizing `minimum_time_interval` - The minimum duration of time between scans to be processed in synchronous mode diff --git a/config/mapper_params_lifelong.yaml b/config/mapper_params_lifelong.yaml index 995eed66..98824881 100644 --- a/config/mapper_params_lifelong.yaml +++ b/config/mapper_params_lifelong.yaml @@ -40,6 +40,7 @@ slam_toolbox: transform_publish_period: 0.02 #if 0 never publishes odometry map_update_interval: 5.0 resolution: 0.05 + min_laser_range: 0.0 #for rastering images max_laser_range: 20.0 #for rastering images minimum_time_interval: 0.5 transform_timeout: 0.2 diff --git a/config/mapper_params_localization.yaml b/config/mapper_params_localization.yaml index 22b9c99e..ff228440 100644 --- a/config/mapper_params_localization.yaml +++ b/config/mapper_params_localization.yaml @@ -23,6 +23,7 @@ slam_toolbox: transform_publish_period: 0.02 #if 0 never publishes odometry map_update_interval: 5.0 resolution: 0.05 + min_laser_range: 0.0 #for rastering images max_laser_range: 20.0 #for rastering images minimum_time_interval: 0.5 transform_timeout: 0.2 diff --git a/config/mapper_params_offline.yaml b/config/mapper_params_offline.yaml index c06591c3..2cb307f5 100644 --- a/config/mapper_params_offline.yaml +++ b/config/mapper_params_offline.yaml @@ -21,6 +21,7 @@ slam_toolbox: transform_publish_period: 0.02 #if 0 never publishes odometry map_update_interval: 10.0 resolution: 0.05 + min_laser_range: 0.0 #for rastering images max_laser_range: 20.0 #for rastering images minimum_time_interval: 0.5 transform_timeout: 0.2 diff --git a/config/mapper_params_online_async.yaml b/config/mapper_params_online_async.yaml index 26f019ff..d8ab98be 100644 --- a/config/mapper_params_online_async.yaml +++ b/config/mapper_params_online_async.yaml @@ -29,6 +29,7 @@ slam_toolbox: transform_publish_period: 0.02 #if 0 never publishes odometry map_update_interval: 5.0 resolution: 0.05 + min_laser_range: 0.0 #for rastering images max_laser_range: 20.0 #for rastering images minimum_time_interval: 0.5 transform_timeout: 0.2 diff --git a/config/mapper_params_online_sync.yaml b/config/mapper_params_online_sync.yaml index 5303edec..d9f130fc 100644 --- a/config/mapper_params_online_sync.yaml +++ b/config/mapper_params_online_sync.yaml @@ -29,6 +29,7 @@ slam_toolbox: transform_publish_period: 0.02 #if 0 never publishes odometry map_update_interval: 5.0 resolution: 0.05 + min_laser_range: 0.0 #for rastering images max_laser_range: 20.0 #for rastering images minimum_time_interval: 0.5 transform_timeout: 0.2 diff --git a/src/laser_utils.cpp b/src/laser_utils.cpp index 30e20230..1e8b2680 100644 --- a/src/laser_utils.cpp +++ b/src/laser_utils.cpp @@ -99,7 +99,28 @@ karto::LaserRangeFinder * LaserAssistant::makeLaser(const double & mountingYaw) karto::LaserRangeFinder_Custom, karto::Name("Custom Described Lidar")); laser->SetOffsetPose(karto::Pose2(laser_pose_.transform.translation.x, laser_pose_.transform.translation.y, mountingYaw)); - laser->SetMinimumRange(scan_.range_min); + + double min_laser_range = 0.0; + if (!node_->has_parameter("min_laser_range")) { + node_->declare_parameter("min_laser_range", min_laser_range); + } + node_->get_parameter("min_laser_range", min_laser_range); + + if (min_laser_range < 0) { + RCLCPP_WARN(node_->get_logger(), + "You've set minimum laser range to be negative," + "this isn't allowed so it will be set to (%.1f).", scan_.range_min); + min_laser_range = scan_.range_min; + } + + if (min_laser_range < scan_.range_min) { + RCLCPP_WARN(node_->get_logger(), + "minimum laser range setting (%.1f m) exceeds the capabilities " + "of the used Lidar (%.1f m)", min_laser_range, scan_.range_min); + min_laser_range = scan_.range_min; + } + + laser->SetMinimumRange(min_laser_range); laser->SetMaximumRange(scan_.range_max); laser->SetMinimumAngle(scan_.angle_min); laser->SetMaximumAngle(scan_.angle_max);