EasyNavigation · fmrico · Dec 16, 2025 · Dec 16, 2025
diff --git a/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp b/common/easynav_costmap_common/include/easynav_costmap_common/costmap_2d.hpp
@@ -41,6 +41,7 @@
 #include <cstring>
 #include <algorithm>
 #include <cmath>
+#include <climits>
 #include <vector>
 #include <mutex>
 #include "geometry_msgs/msg/point.hpp"

diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt
@@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-set(CMAKE_CXX_STANDARD 23)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
-
 find_package(ament_cmake REQUIRED)
 find_package(easynav_common REQUIRED)
 find_package(easynav_core REQUIRED)

diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp
@@ -45,8 +45,8 @@ class MPCController : public ControllerMethodBase
   ~MPCController() override;
 
   /// \brief Initializes parameters and MPC controller.
-  /// \return std::expected<void, std::string> success or error message.
-  std::expected<void, std::string> on_initialize() override;
+  /// \throws std::runtime_error if initialization fails.
+  void on_initialize() override;
 
   /// \brief Updates the controller using the given NavState.
   /// \param nav_state Current navigation state, including odometry and planned path.

diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp
@@ -32,7 +32,7 @@ MPCController::MPCController() {}
 
 MPCController::~MPCController() = default;
 
-std::expected<void, std::string>
+void
 MPCController::on_initialize()
 {
   auto node = get_node();
@@ -65,8 +65,6 @@ MPCController::on_initialize()
 
   detection_pub_ =
     node->create_publisher<sensor_msgs::msg::PointCloud2>("/mpc/detection", 10);
-
-  return {};
 }
 
 void

diff --git a/controllers/easynav_mppi_controller/CMakeLists.txt b/controllers/easynav_mppi_controller/CMakeLists.txt
@@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-set(CMAKE_CXX_STANDARD 23)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
-
 find_package(ament_cmake REQUIRED)
 find_package(easynav_common REQUIRED)
 find_package(easynav_core REQUIRED)

diff --git a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp
@@ -8,7 +8,6 @@
 #define EASYNAV_MPPI_CONTROLLER__MPPICONTROLLER_HPP_
 
 #include <memory>
-#include <expected>
 #include <string>
 
 #include "geometry_msgs/msg/twist_stamped.hpp"
@@ -33,8 +32,8 @@ class MPPIController : public ControllerMethodBase
   ~MPPIController() override;
 
   /// \brief Initializes parameters and MPPI controller.
-  /// \return std::expected<void, std::string> success or error message.
-  std::expected<void, std::string> on_initialize() override;
+  /// \throws std::runtime_error if initialization fails.
+  void on_initialize() override;
 
   /// \brief Updates the controller using the given NavState.
   /// \param nav_state Current navigation state, including odometry and planned path.

diff --git a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp
@@ -20,8 +20,6 @@
 /// \file
 /// \brief Implementation of the MPPIController class.
 
-#include <expected>
-
 #include "easynav_mppi_controller/MPPIController.hpp"
 #include "easynav_common/types/PointPerception.hpp"
 #include "easynav_common/RTTFBuffer.hpp"
@@ -37,7 +35,7 @@ MPPIController::MPPIController() {}
 
 MPPIController::~MPPIController() = default;
 
-std::expected<void, std::string>
+void
 MPPIController::on_initialize()
 {
   auto node = get_node();
@@ -72,8 +70,6 @@ MPPIController::on_initialize()
     node->create_publisher<visualization_msgs::msg::MarkerArray>("/mppi/candidates", 10);
   mppi_optimal_pub_ =
     node->create_publisher<visualization_msgs::msg::MarkerArray>("/mppi/optimal_path", 10);
-
-  return {};
 }
 
 void MPPIController::publish_mppi_markers(

diff --git a/controllers/easynav_serest_controller/CMakeLists.txt b/controllers/easynav_serest_controller/CMakeLists.txt
@@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-set(CMAKE_CXX_STANDARD 23)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
-
 find_package(ament_cmake REQUIRED)
 find_package(easynav_common REQUIRED)
 find_package(easynav_core REQUIRED)

diff --git a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp
@@ -20,7 +20,6 @@
 #ifndef EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_
 #define EASYNAV_SEREST_CONTROLLER__SERESTCONTROLLER_HPP_
 
-#include <expected>
 #include <vector>
 #include <string>
 
@@ -63,9 +62,9 @@ class SerestController : public ControllerMethodBase
 
   /**
    * @brief Initialize parameters and internal state.
-   * @return std::expected<void, std::string> Empty on success; error message otherwise.
+   * @throws std::runtime_error on initialization error.
    */
-  std::expected<void, std::string> on_initialize() override;
+  void on_initialize() override;
 
   /**
    * @brief Real-time control update (called ~20–30 Hz).

diff --git a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp
@@ -23,7 +23,6 @@
 #include <algorithm>
 #include <limits>
 #include <cmath>
-#include <numbers>
 
 #include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@@ -41,7 +40,7 @@ namespace easynav
 SerestController::SerestController() = default;
 SerestController::~SerestController() = default;
 
-std::expected<void, std::string>
+void
 SerestController::on_initialize()
 {
   auto node = get_node();
@@ -149,8 +148,6 @@ SerestController::on_initialize()
   last_vlin_ = 0.0;
   last_vrot_ = 0.0;
   last_update_ts_ = node->now();
-
-  return {};
 }
 
 SerestController::PathData
@@ -487,7 +484,7 @@ SerestController::should_turn_in_place(
 {
   // Keep compatibility with the signature, but ignore turn_in_place_thr
   // and use two internal thresholds without exposing parameters.
-  const double thr_enter = 60.0 * std::numbers::pi / 180.0; // enter TiP if |e_theta| > 60°
+  const double thr_enter = 60.0 * M_PI / 180.0; // enter TiP if |e_theta| > 60°
   // const double thr_exit = 35.0 * PI / 180.0; // exit TiP if |e_theta| < 35°
 
   // Do not allow reverse "shortcut" in this decision: if reverse is not allowed,
@@ -510,7 +507,7 @@ SerestController::maybe_final_align_and_publish(
   double dist_xy_goal, double stop_r, double e_theta_goal,
   double gamma_slow, double dt)
 {
-  const double goal_yaw_tol = goal_yaw_tol_deg_ * std::numbers::pi / 180.0;
+  const double goal_yaw_tol = goal_yaw_tol_deg_ * M_PI / 180.0;
 
   if (dist_xy_goal > stop_r) {
     return false;
@@ -625,7 +622,7 @@ SerestController::update_rt(NavState & nav_state)
 
   // 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_ * (std::numbers::pi / 180.0);
+  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");
   }
@@ -727,7 +724,7 @@ SerestController::update_rt(NavState & nav_state)
   double v_prog_ref = v_prog_ref_free * gamma_slow;
 
   // Maintain a small cruising speed when roughly aligned and outside the stop zone (no reverse)
-  const double align_thr = 30.0 * std::numbers::pi / 180.0;
+  const double align_thr = 30.0 * M_PI / 180.0;
   if (!allow_reverse_ && (dist_xy_goal > stop_r) && std::fabs(e_theta) < align_thr) {
     v_prog_ref = std::max(v_prog_ref, std::min(slow_min_speed_, v_prog_ref_free));
   }
@@ -778,7 +775,7 @@ SerestController::update_rt(NavState & nav_state)
 
   // Optional classic TiP safeguard using the same angular threshold
   {
-    const double turn_in_place_thr = (60.0 * std::numbers::pi / 180.0);
+    const double turn_in_place_thr = (60.0 * M_PI / 180.0);
     const double s_total = pd.s_acc.back();
     const double dist_to_end = s_total - prj.s_star;
 

diff --git a/controllers/easynav_simple_controller/CMakeLists.txt b/controllers/easynav_simple_controller/CMakeLists.txt
@@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-set(CMAKE_CXX_STANDARD 23)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
-
 find_package(ament_cmake REQUIRED)
 find_package(easynav_common REQUIRED)
 find_package(easynav_core REQUIRED)

diff --git a/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp b/controllers/easynav_simple_controller/include/easynav_simple_controller/SimpleController.hpp
@@ -8,7 +8,6 @@
 #define EASYNAV_SIMPLE_CONTROLLER__SIMPLECONTROLLER_HPP_
 
 #include <memory>
-#include <expected>
 #include <string>
 
 #include "geometry_msgs/msg/point.hpp"
@@ -32,8 +31,8 @@ class SimpleController : public ControllerMethodBase
   ~SimpleController() override;
 
   /// \brief Initializes parameters and PID controllers.
-  /// \return std::expected<void, std::string> success or error message.
-  std::expected<void, std::string> on_initialize() override;
+  /// \throws std::runtime_error on initialization error.
+  void on_initialize() override;
 
   /// \brief Updates the controller using the given NavState.
   /// \param nav_state Current navigation state, including odometry and planned path.

diff --git a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp
@@ -20,8 +20,6 @@
 /// \file
 /// \brief Implementation of the SimpleController class.
 
-#include <expected>
-
 #include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
@@ -39,7 +37,7 @@ SimpleController::SimpleController()
 
 SimpleController::~SimpleController() = default;
 
-std::expected<void, std::string>
+void
 SimpleController::on_initialize()
 {
   auto node = get_node();
@@ -87,8 +85,6 @@ SimpleController::on_initialize()
   last_vlin_ = 0.0;
   last_vrot_ = 0.0;
   last_update_ts_ = node->now();
-
-  return {};
 }
 
 

diff --git a/controllers/easynav_vff_controller/CMakeLists.txt b/controllers/easynav_vff_controller/CMakeLists.txt
@@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-set(CMAKE_CXX_STANDARD 23)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
-
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)

diff --git a/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp
@@ -23,8 +23,6 @@
 #ifndef EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_
 #define EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_
 
-#include <expected>
-
 #include "pcl/point_cloud.h"
 
 #include "easynav_core/ControllerMethodBase.hpp"
@@ -83,11 +81,9 @@ class VffController : public easynav::ControllerMethodBase
    * This method is called once during the configuration phase of the controller node,
    * and can be optionally overridden by derived classes to perform custom setup logic.
    *
-   * @return std::expected<void, std::string> Returns an expected object:
-   *         - `void` if initialization was successful,
-   *         - a `std::string` containing an error message if initialization failed.
+   * @throws std::runtime_error on initialization error.
    */
-  virtual std::expected<void, std::string> on_initialize() override;
+  virtual void on_initialize() override;
 
   /**
    * @brief Updates the localization estimate based on the current navigation state.

diff --git a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp
@@ -20,8 +20,6 @@
 /// \file
 /// \brief Implementation of the VffController class.
 
-#include <expected>
-
 #include "easynav_vff_controller/VffController.hpp"
 #include "easynav_common/types/NavState.hpp"
 #include "easynav_common/types/PointPerception.hpp"
@@ -35,7 +33,7 @@
 namespace easynav
 {
 
-std::expected<void, std::string> VffController::on_initialize()
+void VffController::on_initialize()
 {
   auto node = get_node();
   const auto & plugin_name = get_plugin_name();
@@ -77,8 +75,6 @@ std::expected<void, std::string> VffController::on_initialize()
   // Publisher for visualization markers
   marker_array_pub_ = node->create_publisher<visualization_msgs::msg::MarkerArray>(
     "vff/markers_vff", 10);
-
-  return {};
 }
 
 visualization_msgs::msg::MarkerArray

diff --git a/localizers/easynav_costmap_localizer/CMakeLists.txt b/localizers/easynav_costmap_localizer/CMakeLists.txt
@@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-set(CMAKE_CXX_STANDARD 23)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
-
 find_package(ament_cmake REQUIRED)
 find_package(easynav_common REQUIRED)
 find_package(easynav_costmap_common REQUIRED)

diff --git a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp
@@ -67,9 +67,9 @@ class AMCLLocalizer : public LocalizerMethodBase
    *
    * Sets up publishers, subscribers, and prepares the particle filter.
    *
-   * @return std::expected<void, std::string> Success or error message.
+   * @throws std::runtime_error on initialization error.
    */
-  virtual std::expected<void, std::string> on_initialize() override;
+  virtual void on_initialize() override;
 
   /**
    * @brief Real-time update of the localization state.

diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp
@@ -20,7 +20,6 @@
 /// \file
 /// \brief Implementation of the AMCLLocalizer class using Costmap2D.
 
-#include <expected>
 #include <random>
 
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
@@ -187,7 +186,7 @@ AMCLLocalizer::~AMCLLocalizer()
 {
 }
 
-std::expected<void, std::string>
+void
 AMCLLocalizer::on_initialize()
 {
   auto node = get_node();
@@ -272,8 +271,6 @@ AMCLLocalizer::on_initialize()
   last_reseed_ = get_node()->now();
 
   get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug);
-
-  return {};
 }
 
 void printTransform(const tf2::Transform & tf)

diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt
@@ -5,10 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-set(CMAKE_CXX_STANDARD 23)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
-
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)