add the option "freespace_downsampling" to save memory

Signed-off-by: Davide Faconti <[email protected]>

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -152,7 +152,7 @@ class AmclNode : public nav2_util::LifecycleNode
   std::recursive_mutex mutex_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::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<Point2D> free_space_indices;
 #endif
 

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<uint16_t>(i), static_cast<uint16_t>(j)};
+        AmclNode::Point2D point = {i, j};
         free_space_indices.push_back(point);
       }
     }