diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt
index dd9bdd5..2b2df16 100644
--- a/controllers/easynav_mpc_controller/CMakeLists.txt
+++ b/controllers/easynav_mpc_controller/CMakeLists.txt
@@ -20,18 +20,26 @@ find_package(sensor_msgs REQUIRED)
 find_package(pcl_conversions REQUIRED)
 find_package(Eigen3 REQUIRED NO_MODULE)
 find_package(PCL REQUIRED COMPONENTS common io)
+
+# Try to find NLopt package from system first, or get it through CMake FetchContent to build it
 find_package(NLopt QUIET)
 if(NOT NLopt_FOUND)
-  find_path(NLOPT_INCLUDE_DIR nlopt.hpp)
-  find_library(NLOPT_LIBRARY nlopt)
-  if(NLOPT_INCLUDE_DIR AND NLOPT_LIBRARY)
-    set(NLopt_FOUND TRUE)
-    set(NLopt_INCLUDE_DIRS ${NLOPT_INCLUDE_DIR})
-    set(NLopt_LIBRARIES ${NLOPT_LIBRARY})
-  endif()
-endif()
-if(NOT NLopt_FOUND)
-  message(FATAL_ERROR "NLopt not found. Install libnlopt-dev or set NLopt_DIR/CMAKE_PREFIX_PATH.")
+  include(FetchContent)
+  # Force FetchContent to download and build in the workspace build dir
+  set(FETCH_CONTENT_BASE_DIR "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/_deps>")
+
+  # Disable unneeded NLopt features
+  set(NLOPT_PYTHON OFF)
+  set(NLOPT_OCTAVE OFF)
+  set(NLOPT_GUILE OFF)
+  set(NLOPT_JAVA OFF)
+  fetchcontent_declare(nlopt
+    GIT_REPOSITORY  https://github.com/stevengj/nlopt
+    GIT_TAG         v2.11.0
+  )
+  fetchcontent_makeavailable(
+    nlopt
+  )
 endif()
 
 add_library(${PROJECT_NAME} SHARED
diff --git a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp
index 467b146..2c9c595 100644
--- a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp
+++ b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_tests.cpp
@@ -99,10 +99,10 @@ TEST(DynamicWindowPurePursuit, ComputeDynamicWindowClampsToAccelLimits)
   current_speed.angular.z = 0.0;
 
   const auto window = easynav::dynamic_window_pure_pursuit::computeDynamicWindow(
-    current_speed, /*max_linear_vel=*/1.0, /*min_linear_vel=*/-1.0,
-    /*max_angular_vel=*/1.0, /*min_angular_vel=*/-1.0,
-    /*max_linear_accel=*/2.0, /*max_linear_decel=*/2.0,
-    /*max_angular_accel=*/2.0, /*max_angular_decel=*/2.0, /*dt=*/0.1);
+    current_speed, /*max_linear_vel=*/ 1.0, /*min_linear_vel=*/ -1.0,
+    /*max_angular_vel=*/ 1.0, /*min_angular_vel=*/ -1.0,
+    /*max_linear_accel=*/ 2.0, /*max_linear_decel=*/ 2.0,
+    /*max_angular_accel=*/ 2.0, /*max_angular_decel=*/ 2.0, /*dt=*/ 0.1);
 
   EXPECT_NEAR(window.max_linear_vel, 0.2, 1e-9);
   EXPECT_NEAR(window.min_linear_vel, -0.2, 1e-9);
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..b33e450 100644
--- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp
+++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp
@@ -634,13 +634,11 @@ SerestController::update_rt(NavState & nav_state)
   if (!fetch_required_inputs(nav_state, path, odom)) {return;}
 
   // 1.5) Goal tolerances: prefer shared GoalManager values, fallback to local params
-  double goal_pos_tol = goal_pos_tol_;
-  double goal_yaw_tol = goal_yaw_tol_deg_ * (M_PI / 180.0);
   if (nav_state.has("goal_tolerance.position")) {
-    goal_pos_tol = nav_state.get<double>("goal_tolerance.position");
+    goal_pos_tol_ = nav_state.get<double>("goal_tolerance.position");
   }
   if (nav_state.has("goal_tolerance.yaw")) {
-    goal_yaw_tol = nav_state.get<double>("goal_tolerance.yaw");
+    goal_yaw_tol_deg_ = nav_state.get<double>("goal_tolerance.yaw") * (180.0 / M_PI);
   }
 
   // 2) Robot state (position + yaw)
@@ -718,10 +716,10 @@ SerestController::update_rt(NavState & nav_state)
       publish_cmd_and_debug(
         nav_state, path, vlin, vrot,
         e_y, e_theta, rk.kappa_hat,
-        d_closest, v_safe, v_curv, /*alpha*/1.0,
+        d_closest, v_safe, v_curv, /*alpha*/ 1.0,
         allow_reverse_, dist_to_end,
         dist_xy_goal, gamma_slow,
-        /*in_final_align*/0, /*arrived*/0);
+        /*in_final_align*/ 0, /*arrived*/ 0);
       return;
     }
   }
@@ -830,7 +828,7 @@ SerestController::update_rt(NavState & nav_state)
     d_closest, v_safe, v_curv, alpha,
     allow_reverse_, dist_to_end,
     dist_xy_goal, gamma_slow,
-    /*in_final_align=*/0, /*arrived=*/0);
+    /*in_final_align=*/ 0, /*arrived=*/ 0);
 }
 
 }  // namespace easynav
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..b6ad318 100644
--- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp
+++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp
@@ -189,22 +189,28 @@ AMCLLocalizer::on_initialize()
   auto node = get_node();
   const auto & plugin_name = get_plugin_name();
 
-  int num_particles;
-  double x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw;
-
-  node->declare_parameter<int>(plugin_name + ".num_particles", 100);
-  node->declare_parameter<double>(plugin_name + ".initial_pose.x", 0.0);
-  node->declare_parameter<double>(plugin_name + ".initial_pose.y", 0.0);
-  node->declare_parameter<double>(plugin_name + ".initial_pose.yaw", 0.0);
-  node->declare_parameter<double>(plugin_name + ".initial_pose.std_dev_xy", 0.5);
-  node->declare_parameter<double>(plugin_name + ".initial_pose.std_dev_yaw", 0.5);
-  node->declare_parameter<double>(plugin_name + ".reseed_freq", 1.0);
-  node->declare_parameter<double>(plugin_name + ".noise_translation", 0.01);
-  node->declare_parameter<double>(plugin_name + ".noise_rotation", 0.01);
-  node->declare_parameter<double>(plugin_name + ".noise_translation_to_rotation", 0.01);
-  node->declare_parameter<double>(plugin_name + ".min_noise_xy", 0.05);
-  node->declare_parameter<double>(plugin_name + ".min_noise_yaw", 0.05);
-  node->declare_parameter<bool>(plugin_name + ".compute_odom_from_tf", false);
+  int num_particles = 100;
+  double x_init = 0.0;
+  double y_init = 0.0;
+  double yaw_init = 0.0;
+  double std_dev_xy = 0.5;
+  double std_dev_yaw = 0.5;
+  double reseed_freq = 1.0;
+
+  node->declare_parameter<int>(plugin_name + ".num_particles", num_particles);
+  node->declare_parameter<double>(plugin_name + ".initial_pose.x", x_init);
+  node->declare_parameter<double>(plugin_name + ".initial_pose.y", y_init);
+  node->declare_parameter<double>(plugin_name + ".initial_pose.yaw", yaw_init);
+  node->declare_parameter<double>(plugin_name + ".initial_pose.std_dev_xy", std_dev_xy);
+  node->declare_parameter<double>(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw);
+  node->declare_parameter<double>(plugin_name + ".reseed_freq", reseed_freq);
+  node->declare_parameter<double>(plugin_name + ".noise_translation", noise_translation_);
+  node->declare_parameter<double>(plugin_name + ".noise_rotation", noise_rotation_);
+  node->declare_parameter<double>(plugin_name + ".noise_translation_to_rotation",
+    noise_translation_to_rotation_);
+  node->declare_parameter<double>(plugin_name + ".min_noise_xy", min_noise_xy_);
+  node->declare_parameter<double>(plugin_name + ".min_noise_yaw", min_noise_yaw_);
+  node->declare_parameter<bool>(plugin_name + ".compute_odom_from_tf", compute_odom_from_tf_);
 
   node->get_parameter<int>(plugin_name + ".num_particles", num_particles);
   node->get_parameter<double>(plugin_name + ".initial_pose.x", x_init);
@@ -220,7 +226,6 @@ AMCLLocalizer::on_initialize()
   node->get_parameter<double>(plugin_name + ".min_noise_yaw", min_noise_yaw_);
   node->get_parameter<bool>(plugin_name + ".compute_odom_from_tf", compute_odom_from_tf_);
 
-  double reseed_freq;
   node->get_parameter<double>(plugin_name + ".reseed_freq", reseed_freq);
   reseed_time_ = 1.0 / reseed_freq;
 
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..a518267 100644
--- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp
+++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp
@@ -187,21 +187,27 @@ AMCLLocalizer::on_initialize()
   auto node = get_node();
   const auto & plugin_name = get_plugin_name();
 
-  int num_particles;
-  double x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw;
-
-  node->declare_parameter<int>(plugin_name + ".num_particles", 100);
-  node->declare_parameter<double>(plugin_name + ".initial_pose.x", 0.0);
-  node->declare_parameter<double>(plugin_name + ".initial_pose.y", 0.0);
-  node->declare_parameter<double>(plugin_name + ".initial_pose.yaw", 0.0);
-  node->declare_parameter<double>(plugin_name + ".initial_pose.std_dev_xy", 0.5);
-  node->declare_parameter<double>(plugin_name + ".initial_pose.std_dev_yaw", 0.5);
-  node->declare_parameter<double>(plugin_name + ".reseed_freq", 1.0);
-  node->declare_parameter<double>(plugin_name + ".noise_translation", 0.01);
-  node->declare_parameter<double>(plugin_name + ".noise_rotation", 0.01);
-  node->declare_parameter<double>(plugin_name + ".noise_translation_to_rotation", 0.01);
-  node->declare_parameter<double>(plugin_name + ".min_noise_xy", 0.05);
-  node->declare_parameter<double>(plugin_name + ".min_noise_yaw", 0.05);
+  int num_particles = 100;
+  double x_init = 0.0;
+  double y_init = 0.0;
+  double yaw_init = 0.0;
+  double std_dev_xy = 0.5;
+  double std_dev_yaw = 0.5;
+  double reseed_freq = 1.0;
+
+  node->declare_parameter<int>(plugin_name + ".num_particles", num_particles);
+  node->declare_parameter<double>(plugin_name + ".initial_pose.x", x_init);
+  node->declare_parameter<double>(plugin_name + ".initial_pose.y", y_init);
+  node->declare_parameter<double>(plugin_name + ".initial_pose.yaw", yaw_init);
+  node->declare_parameter<double>(plugin_name + ".initial_pose.std_dev_xy", std_dev_xy);
+  node->declare_parameter<double>(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw);
+  node->declare_parameter<double>(plugin_name + ".reseed_freq", reseed_freq);
+  node->declare_parameter<double>(plugin_name + ".noise_translation", noise_translation_);
+  node->declare_parameter<double>(plugin_name + ".noise_rotation", noise_rotation_);
+  node->declare_parameter<double>(plugin_name + ".noise_translation_to_rotation",
+    noise_translation_to_rotation_);
+  node->declare_parameter<double>(plugin_name + ".min_noise_xy", min_noise_xy_);
+  node->declare_parameter<double>(plugin_name + ".min_noise_yaw", min_noise_yaw_);
 
   node->get_parameter<int>(plugin_name + ".num_particles", num_particles);
   node->get_parameter<double>(plugin_name + ".initial_pose.x", x_init);
@@ -231,7 +237,6 @@ AMCLLocalizer::on_initialize()
   }
 
 
-  double reseed_freq;
   node->get_parameter<double>(plugin_name + ".reseed_freq", reseed_freq);
   reseed_time_ = 1.0 / reseed_freq;