From f6dd8af4f6d9c329bcfd47c6aeb3d0a8fe323990 Mon Sep 17 00:00:00 2001 From: Yong-Hao Zou Date: Thu, 12 Aug 2021 12:06:33 +0800 Subject: [PATCH 1/2] Fix null pointer in amcl on_cleanup --- nav2_amcl/src/amcl_node.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 0e4fc209119..f1c0a6e5561 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -335,8 +335,10 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) laser_scan_sub_.reset(); // Map - map_free(map_); - map_ = nullptr; + if (map_ != NULL) { + map_free(map_); + map_ = nullptr; + } first_map_received_ = false; free_space_indices.resize(0); From d82df59abef067f80770d9c1cef705f7745b66d9 Mon Sep 17 00:00:00 2001 From: zouyonghao Date: Tue, 24 Aug 2021 17:31:01 +0800 Subject: [PATCH 2/2] Add more semantic checks for amcl --- nav2_amcl/src/amcl_node.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 79e4efa5abf..16ab416fef3 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1107,6 +1107,25 @@ AmclNode::initParameters() last_time_printed_msg_ = now(); // Semantic checks + if (laser_likelihood_max_dist_ < 0) { + RCLCPP_WARN( + get_logger(), "You've set laser_likelihood_max_dist to be negtive," + " this isn't allowed so it will be set to default value 2.0."); + laser_likelihood_max_dist_ = 2.0; + } + if (max_particles_ < 0) { + RCLCPP_WARN( + get_logger(), "You've set max_particles to be negtive," + " this isn't allowed so it will be set to default value 2000."); + max_particles_ = 2000; + } + + if (min_particles_ < 0) { + RCLCPP_WARN( + get_logger(), "You've set min_particles to be negtive," + " this isn't allowed so it will be set to default value 500."); + min_particles_ = 500; + } if (min_particles_ > max_particles_) { RCLCPP_WARN(