From 1d798cbb17119a248acab5ec1d06074332dee2fd Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Wed, 12 Jun 2024 18:29:12 +0200
Subject: [PATCH] [AMCL] reduce the amount of memory needed

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>
---
 nav2_amcl/include/nav2_amcl/amcl_node.hpp |  3 ++-
 nav2_amcl/include/nav2_amcl/map/map.hpp   |  9 +++++----
 nav2_amcl/src/amcl_node.cpp               | 11 ++++++-----
 3 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
index 030a85f38cd..c6e9400cc7d 100644
--- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp
+++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
@@ -152,7 +152,8 @@ class AmclNode : public nav2_util::LifecycleNode
   std::recursive_mutex mutex_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
 #if NEW_UNIFORM_SAMPLING
-  static std::vector<std::pair<int, int>> free_space_indices;
+  struct Point2D { uint16_t x; uint16_t y; };
+  static std::vector<Point2D> free_space_indices;
 #endif
 
   // Transforms
diff --git a/nav2_amcl/include/nav2_amcl/map/map.hpp b/nav2_amcl/include/nav2_amcl/map/map.hpp
index c9557efa0b0..3cf6f0d597b 100644
--- a/nav2_amcl/include/nav2_amcl/map/map.hpp
+++ b/nav2_amcl/include/nav2_amcl/map/map.hpp
@@ -41,20 +41,21 @@ struct _rtk_fig_t;
 // Limits
 #define MAP_WIFI_MAX_LEVELS 8
 
-
+// make sure that the sizeof(map_cell_t) == 5
+#pragma pack(push, 1)
 // Description for a single map cell.
 typedef struct
 {
   // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
-  int occ_state;
+  int8_t occ_state;
 
   // Distance to the nearest occupied cell
-  double occ_dist;
+  float occ_dist;
 
   // Wifi levels
   // int wifi_levels[MAP_WIFI_MAX_LEVELS];
 } map_cell_t;
-
+#pragma pack(pop)
 
 // Description for a map
 typedef struct
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index cdd4c5adbdb..7d67407fa61 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -404,7 +404,7 @@ AmclNode::checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time las
 }
 
 #if NEW_UNIFORM_SAMPLING
-std::vector<std::pair<int, int>> AmclNode::free_space_indices;
+std::vector<AmclNode::Point2D> AmclNode::free_space_indices;
 #endif
 
 bool
@@ -446,10 +446,10 @@ AmclNode::uniformPoseGenerator(void * arg)
 
 #if NEW_UNIFORM_SAMPLING
   unsigned int rand_index = drand48() * free_space_indices.size();
-  std::pair<int, int> free_point = free_space_indices[rand_index];
+  AmclNode::Point2D free_point = free_space_indices[rand_index];
   pf_vector_t p;
-  p.v[0] = MAP_WXGX(map, free_point.first);
-  p.v[1] = MAP_WYGY(map, free_point.second);
+  p.v[0] = MAP_WXGX(map, free_point.x);
+  p.v[1] = MAP_WYGY(map, free_point.y);
   p.v[2] = drand48() * 2 * M_PI - M_PI;
 #else
   double min_x, max_x, min_y, max_y;
@@ -1440,7 +1440,8 @@ AmclNode::createFreeSpaceVector()
   for (int i = 0; i < map_->size_x; i++) {
     for (int j = 0; j < map_->size_y; j++) {
       if (map_->cells[MAP_INDEX(map_, i, j)].occ_state == -1) {
-        free_space_indices.push_back(std::make_pair(i, j));
+        AmclNode::Point2D point = {static_cast<uint16_t>(i), static_cast<uint16_t>(j)};
+        free_space_indices.push_back(point);
       }
     }
   }