From 666da8f5eb023fc4f528470d4065dc951a6a2ecb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 2 Aug 2026 11:27:22 +0200 Subject: [PATCH 1/4] Make effective changes in tolerances MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_serest_controller/SerestController.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index 52272e5..a617994 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -642,6 +642,10 @@ SerestController::update_rt(NavState & nav_state) if (nav_state.has("goal_tolerance.yaw")) { goal_yaw_tol = nav_state.get("goal_tolerance.yaw"); } + // Propagate the resolved tolerances to the members consumed by compute_goal_zone() + // and maybe_final_align_and_publish(), so a GoalManager override actually takes effect. + goal_pos_tol_ = goal_pos_tol; + goal_yaw_tol_deg_ = goal_yaw_tol * (180.0 / M_PI); // 2) Robot state (position + yaw) Vec2 robot_xy; double yaw = 0.0; From 33647fca35fb288045f33980b12c4d531102e617 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 2 Aug 2026 11:27:57 +0200 Subject: [PATCH 2/4] Initialize num_particles before using MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../src/easynav_costmap_localizer/AMCLLocalizer.cpp | 2 +- .../src/easynav_simple_localizer/AMCLLocalizer.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index b07dfc2..32598f1 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -189,7 +189,7 @@ AMCLLocalizer::on_initialize() auto node = get_node(); const auto & plugin_name = get_plugin_name(); - int num_particles; + int num_particles = 100; double x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw; node->declare_parameter(plugin_name + ".num_particles", 100); diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index 43c5eda..eeb7b6c 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -187,7 +187,7 @@ AMCLLocalizer::on_initialize() auto node = get_node(); const auto & plugin_name = get_plugin_name(); - int num_particles; + int num_particles = 100; double x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw; node->declare_parameter(plugin_name + ".num_particles", 100); From ba6363fdf39b98eec6d489ee8b971825104c7a5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 2 Aug 2026 11:28:11 +0200 Subject: [PATCH 3/4] Remove unused code MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_fusion_localizer/tests/fusion_localizer_tests.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp index d010b35..941463d 100644 --- a/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp @@ -61,10 +61,6 @@ class FusionLocalizerInitialPoseTest : public ::testing::Test TEST_F(FusionLocalizerInitialPoseTest, SubscribesToInitialPoseWithDefaultCallbackGroup) { - const double x0 = 1.5; - const double y0 = -0.25; - const double yaw0 = 0.7; - const double x1 = -0.8; const double y1 = 2.1; const double yaw1 = -1.2; From 44273dc7a32de7b74fcec76b25601c978868597a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 2 Aug 2026 11:29:02 +0200 Subject: [PATCH 4/4] Improve code by avoiding extra calls MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_costmap_maps_manager/filters/InflationFilter.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp index f03c72d..e2a8326 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp @@ -145,9 +145,8 @@ InflationFilter::update(NavState & nav_state) updateCosts(dynamic_map, min_i, min_j, max_i, max_j); - for (int i = 0; i < dynamic_map.getSizeInCellsX(); i++) { - for (int j = 0; j < dynamic_map.getSizeInCellsY(); j++) { - int index = static_cast(dynamic_map.getIndex(i, j)); + for (int i = 0; i < size_x; i++) { + for (int j = 0; j < size_y; j++) { unsigned char cost = std::max( dynamic_map.getCost(i, j), base_inflated_.getCost(i, j)); dynamic_map.setCost(i, j, cost);