From c13c3b596ddc8265b642603af3337fbb07fa57ae Mon Sep 17 00:00:00 2001 From: Hao Li Date: Mon, 18 Dec 2023 15:31:10 +0100 Subject: [PATCH 1/2] 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. --- nav2_costmap_2d/src/costmap_2d.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index e72807da59..5d467c9987 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -102,6 +102,8 @@ void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y) { std::unique_lock lock(*access_); delete[] costmap_; + size_x_ = size_x; + size_y_ = size_y; costmap_ = new unsigned char[size_x * size_y]; } From eb4a02407b03f5775bce54151dd64445ad8faf9c Mon Sep 17 00:00:00 2001 From: Hao Li Date: Mon, 18 Dec 2023 22:23:59 +0100 Subject: [PATCH 2/2] 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 | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 5d467c9987..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(); } @@ -111,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; @@ -168,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(