From af08021065f9cce8ff761859a595afcecb64e2ce Mon Sep 17 00:00:00 2001
From: Hao-Li-Bachelorarbeit
 <141755843+Hao-Li-Bachelorarbeit@users.noreply.github.com>
Date: Mon, 18 Dec 2023 23:22:08 +0100
Subject: [PATCH] Synchronize map size information during map initialization
 (#4015)

* Update costmap size configuration

This commit updates the costmap_2d.cpp file to fix a bug where the costmap size wasn't appropriately updated. Two new lines of code have been added to ensure the size of the costmap is correctly configured each time it's instantiated.

* Refactor costmap size assignment in Costmap2D class

The code refactor eliminates the direct mutation of the size_x_ and size_y_ attributes in the Costmap2D class. Instead, the class uses the size of cells provided during initialization and calculation from map coordinates for better encapsulation and clarity.
---
 nav2_costmap_2d/src/costmap_2d.cpp | 13 +++++--------
 1 file changed, 5 insertions(+), 8 deletions(-)

diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp
index e72807da59..8914166a5e 100644
--- a/nav2_costmap_2d/src/costmap_2d.cpp
+++ b/nav2_costmap_2d/src/costmap_2d.cpp
@@ -49,13 +49,13 @@ namespace nav2_costmap_2d
 Costmap2D::Costmap2D(
   unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
   double origin_x, double origin_y, unsigned char default_value)
-: size_x_(cells_size_x), size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x),
+: resolution_(resolution), origin_x_(origin_x),
   origin_y_(origin_y), costmap_(NULL), default_value_(default_value)
 {
   access_ = new mutex_t();
 
   // create the costmap
-  initMaps(size_x_, size_y_);
+  initMaps(cells_size_x, cells_size_y);
   resetMaps();
 }
 
@@ -102,6 +102,8 @@ void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
 {
   std::unique_lock<mutex_t> lock(*access_);
   delete[] costmap_;
+  size_x_ = size_x;
+  size_y_ = size_y;
   costmap_ = new unsigned char[size_x * size_y];
 }
 
@@ -109,8 +111,6 @@ void Costmap2D::resizeMap(
   unsigned int size_x, unsigned int size_y, double resolution,
   double origin_x, double origin_y)
 {
-  size_x_ = size_x;
-  size_y_ = size_y;
   resolution_ = resolution;
   origin_x_ = origin_x;
   origin_y_ = origin_y;
@@ -166,15 +166,12 @@ bool Costmap2D::copyCostmapWindow(
     // ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
     return false;
   }
-
-  size_x_ = upper_right_x - lower_left_x;
-  size_y_ = upper_right_y - lower_left_y;
   resolution_ = map.resolution_;
   origin_x_ = win_origin_x;
   origin_y_ = win_origin_y;
 
   // initialize our various maps and reset markers for inflation
-  initMaps(size_x_, size_y_);
+  initMaps(upper_right_x - lower_left_x, upper_right_y - lower_left_y);
 
   // copy the window of the static map and the costmap that we're taking
   copyMapRegion(