From bc588773fc689b03f34fad2611ccc13f156fa4cc Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 12 Jun 2024 19:07:04 +0200 Subject: [PATCH] add the option "freespace_downsampling" to save memory Signed-off-by: Davide Faconti --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 2 +- nav2_amcl/src/amcl_node.cpp | 13 ++++++++++--- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index c6e9400cc7d..950c839796b 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -152,7 +152,7 @@ class AmclNode : public nav2_util::LifecycleNode std::recursive_mutex mutex_; rclcpp::Subscription::ConstSharedPtr map_sub_; #if NEW_UNIFORM_SAMPLING - struct Point2D { uint16_t x; uint16_t y; }; + struct Point2D { int32_t x; int32_t y; }; static std::vector free_space_indices; #endif diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 7d67407fa61..e68625d23b3 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -227,6 +227,11 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) add_parameter( "first_map_only", rclcpp::ParameterValue(false), "Set this to true, when you want to load a new map published from the map_server"); + + add_parameter( + "freespace_downsampling", rclcpp::ParameterValue( + false), + "If true, downsample the free space used by the Pose Generator. This will save memory in VERY large maps"); } AmclNode::~AmclNode() @@ -1106,6 +1111,7 @@ AmclNode::initParameters() get_parameter("always_reset_initial_pose", always_reset_initial_pose_); get_parameter("scan_topic", scan_topic_); get_parameter("map_topic", map_topic_); + get_parameter("freespace_downsampling", freespace_downsampling_); save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); transform_tolerance_ = tf2::durationFromSec(tmp_tol); @@ -1435,12 +1441,13 @@ AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg) void AmclNode::createFreeSpaceVector() { + int delta = freespace_downsampling_ ? 2 : 1; // Index of free space free_space_indices.resize(0); - for (int i = 0; i < map_->size_x; i++) { - for (int j = 0; j < map_->size_y; j++) { + for (int i = 0; i < map_->size_x; i += delta) { + for (int j = 0; j < map_->size_y; j += delta) { if (map_->cells[MAP_INDEX(map_, i, j)].occ_state == -1) { - AmclNode::Point2D point = {static_cast(i), static_cast(j)}; + AmclNode::Point2D point = {i, j}; free_space_indices.push_back(point); } }