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 index 3779bec..45394e9 100644 --- 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 #include #include +#include #include #include #include "geometry_msgs/msg/point.hpp" diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt index 48191bd..ef47e16 100644 --- 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 index e01b6e6..c8c3106 100644 --- 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 success or error message. - std::expected 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 index fde4992..939839c 100644 --- 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 MPCController::on_initialize() { auto node = get_node(); @@ -65,8 +65,6 @@ MPCController::on_initialize() detection_pub_ = node->create_publisher("/mpc/detection", 10); - - return {}; } void diff --git a/controllers/easynav_mppi_controller/CMakeLists.txt b/controllers/easynav_mppi_controller/CMakeLists.txt index 43a8650..4edc78f 100644 --- 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 index 6f57ce3..4e34017 100644 --- 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 -#include #include #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 success or error message. - std::expected 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 index 14d3200..b471b03 100644 --- 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 - #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 MPPIController::on_initialize() { auto node = get_node(); @@ -72,8 +70,6 @@ MPPIController::on_initialize() node->create_publisher("/mppi/candidates", 10); mppi_optimal_pub_ = node->create_publisher("/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 index 0067ae1..110edf6 100644 --- 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 index 0cbe7a4..2ca3680 100644 --- 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 #include #include @@ -63,9 +62,9 @@ class SerestController : public ControllerMethodBase /** * @brief Initialize parameters and internal state. - * @return std::expected Empty on success; error message otherwise. + * @throws std::runtime_error on initialization error. */ - std::expected 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 index 25c282e..48a7f46 100644 --- 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 #include #include -#include #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 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("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 index d3542ac..4bd163a 100644 --- 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 index 2dfd5c7..8820859 100644 --- 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 -#include #include #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 success or error message. - std::expected 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 index 101bba5..9438c50 100644 --- 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 - #include "tf2/utils.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -39,7 +37,7 @@ SimpleController::SimpleController() SimpleController::~SimpleController() = default; -std::expected +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 index 131de41..04a2da1 100644 --- 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 index 980549b..f9048de 100644 --- 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 - #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 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 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 index 9788ac4..04729e3 100644 --- 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 - #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 VffController::on_initialize() +void VffController::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); @@ -77,8 +75,6 @@ std::expected VffController::on_initialize() // Publisher for visualization markers marker_array_pub_ = node->create_publisher( "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 index 1ea83e6..aabb1b1 100644 --- 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 index d9d32e4..44b8352 100644 --- 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 Success or error message. + * @throws std::runtime_error on initialization error. */ - virtual std::expected 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 index 68f29bf..5e2da2f 100644 --- 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 #include #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -187,7 +186,7 @@ AMCLLocalizer::~AMCLLocalizer() { } -std::expected +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 index 23a79bd..4db3bed 100644 --- 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) diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp index c96bb97..c29ce6e 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -1,7 +1,6 @@ #pragma once #include -#include #include "easynav_core/LocalizerMethodBase.hpp" #include "easynav_fusion_localizer/ukf_wrapper.hpp" // Tu wrapper refactorizado @@ -28,10 +27,12 @@ class FusionLocalizer : public easynav::LocalizerMethodBase /** * @brief Hook de inicialización de MethodBase. * - * Crea e inicializa el UkfWrapper, que cargará parámetros - * y creará todos los suscriptores/publicadores de robot_localization. + * Loads and initializes the UkfWrapper, which will load parameters + * and create all robot_localization subscribers/publishers. + * + * @throws std::runtime_error if initialization fails. */ - std::expected on_initialize() override; + void on_initialize() override; /** * @brief Hook de actualización RT (alta frecuencia) de LocalizerMethodBase. diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index f117357..b212b73 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -1,5 +1,3 @@ -#include - #include "easynav_fusion_localizer/FusionLocalizer.hpp" #include "easynav_localizer/LocalizerNode.hpp" @@ -16,7 +14,7 @@ namespace easynav { -std::expected FusionLocalizer::on_initialize() +void FusionLocalizer::on_initialize() { try { @@ -50,8 +48,8 @@ std::expected FusionLocalizer::on_initialize() RCLCPP_FATAL( get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s", e.what()); - // Return an error to easynav - return std::unexpected(std::string("Failed to initialize UkfWrapper: ") + e.what()); + // Raise error + throw std::runtime_error(std::string("Failed to initialize UkfWrapper: ") + e.what()); } @@ -66,7 +64,6 @@ std::expected FusionLocalizer::on_initialize() n_gps_sensors_ = static_cast(ukf_wrapper_->getGpsCallbackDataArr().size()); RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully."); - return {}; } // 2. Hook de actualización RT (Tu "Timer" de alta frecuencia) diff --git a/localizers/easynav_gps_localizer/CMakeLists.txt b/localizers/easynav_gps_localizer/CMakeLists.txt index 6c3e2b6..f18114b 100644 --- a/localizers/easynav_gps_localizer/CMakeLists.txt +++ b/localizers/easynav_gps_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) diff --git a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp index fc731c9..cf0192d 100644 --- a/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp +++ b/localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp @@ -23,8 +23,6 @@ #ifndef EASYNAV_LOCALIZER__GPSLOCALIZER_HPP_ #define EASYNAV_LOCALIZER__GPSLOCALIZER_HPP_ -#include - #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "easynav_core/LocalizerMethodBase.hpp" @@ -61,9 +59,9 @@ class GpsLocalizer : public easynav::LocalizerMethodBase * * This override may be used to set up internal resources. By default, it simply succeeds. * - * @return std::expected Returns success or an error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the localization estimate based on the current navigation state. diff --git a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp index 5ed4393..3a852fd 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -20,7 +20,6 @@ /// \file /// \brief Implementation of the GpsLocalizer class. -#include #include "easynav_gps_localizer/GpsLocalizer.hpp" #include "easynav_common/RTTFBuffer.hpp" @@ -28,7 +27,7 @@ namespace easynav { -std::expected GpsLocalizer::on_initialize() +void GpsLocalizer::on_initialize() { auto node = get_node(); @@ -73,8 +72,6 @@ std::expected GpsLocalizer::on_initialize() time_1_ = get_node()->now().seconds(); alpha_ = 0.99; - - return {}; } void GpsLocalizer::gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) diff --git a/localizers/easynav_navmap_localizer/CMakeLists.txt b/localizers/easynav_navmap_localizer/CMakeLists.txt index b55471d..12e237f 100644 --- a/localizers/easynav_navmap_localizer/CMakeLists.txt +++ b/localizers/easynav_navmap_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(navmap_core REQUIRED) diff --git a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp index 954d927..23d2c45 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -75,9 +75,9 @@ class AMCLLocalizer : public LocalizerMethodBase * * Sets up publishers, subscribers, and prepares the particle filter. * - * @return std::expected Success or error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Real-time update of the localization state. diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index be979e2..f6edaf8 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -20,7 +20,6 @@ /// \file /// \brief AMCLLocalizer implementation (Bonxai + probabilistic inflation + ray casting). -#include #include #include #include @@ -366,7 +365,7 @@ AMCLLocalizer::AMCLLocalizer() AMCLLocalizer::~AMCLLocalizer() = default; -std::expected AMCLLocalizer::on_initialize() +void AMCLLocalizer::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); @@ -469,7 +468,6 @@ std::expected AMCLLocalizer::on_initialize() last_reseed_ = get_node()->now(); get_node()->get_logger().set_level(rclcpp::Logger::Level::Debug); - return {}; } void AMCLLocalizer::odom_callback(nav_msgs::msg::Odometry::UniquePtr msg) diff --git a/localizers/easynav_simple_localizer/CMakeLists.txt b/localizers/easynav_simple_localizer/CMakeLists.txt index 5423c28..5b61ef6 100644 --- a/localizers/easynav_simple_localizer/CMakeLists.txt +++ b/localizers/easynav_simple_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_simple_common REQUIRED) diff --git a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp index 2cb0c12..7bff423 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -67,9 +67,9 @@ class AMCLLocalizer : public LocalizerMethodBase * * Sets up publishers, subscribers, and prepares the particle filter. * - * @return std::expected Success or error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Real-time update of the localization state. 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 caa5125..dffc223 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -20,7 +20,6 @@ /// \file /// \brief Implementation of the AMCLLocalizer class. -#include #include #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -185,7 +184,7 @@ AMCLLocalizer::~AMCLLocalizer() { } -std::expected +void AMCLLocalizer::on_initialize() { auto node = get_node(); @@ -263,8 +262,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/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt index 994a44e..eea955e 100644 --- a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_bonxai_maps_manager/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) - list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) diff --git a/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp b/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp index d9b2694..8a81e0f 100644 --- a/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp +++ b/maps_managers/easynav_bonxai_maps_manager/include/easynav_bonxai_maps_manager/BonxaiMapsManager.hpp @@ -61,9 +61,9 @@ class BonxaiMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. diff --git a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp index d239986..bc8c3e4 100644 --- a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp +++ b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp @@ -17,7 +17,6 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include #include #include "easynav_bonxai_maps_manager/BonxaiMapsManager.hpp" @@ -53,7 +52,7 @@ BonxaiMapsManager::BonxaiMapsManager() BonxaiMapsManager::~BonxaiMapsManager() {} -std::expected +void BonxaiMapsManager::on_initialize() { auto node = get_node(); @@ -81,7 +80,7 @@ BonxaiMapsManager::on_initialize() const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + bonxai_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } std::vector bonxai_result; @@ -114,13 +113,13 @@ BonxaiMapsManager::on_initialize() const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + occmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } nav_msgs::msg::OccupancyGrid occ_msg; if (auto ret = loadMapFromYaml(map_path_, occ_msg) != LOAD_MAP_SUCCESS) { std::cerr << "loadMapFromYaml returned" << ret << std::endl; - return std::unexpected("YAML file [" + map_path_ + "] not found or invalid: "); + throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); } update_from_occ(occ_msg); @@ -159,8 +158,6 @@ BonxaiMapsManager::on_initialize() Bonxai::WritePointsFromPCD(map_path_, bonxai_result); // This can overwrite yaml occ maps // ToDo }); - - return {}; } void diff --git a/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt index 6ef5ac5..15c6c40 100644 --- a/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_costmap_maps_manager/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) - list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp index 4968746..f4d0fb5 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/CostmapMapsManager.hpp @@ -63,9 +63,9 @@ class CostmapMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/CostmapFilter.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/CostmapFilter.hpp index f830830..0ede2b6 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/CostmapFilter.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/CostmapFilter.hpp @@ -21,7 +21,6 @@ #ifndef EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_HPP_ -#include #include #include "easynav_common/types/NavState.hpp" @@ -42,12 +41,12 @@ class CostmapFilter public: CostmapFilter(); - std::expected + void initialize( const std::shared_ptr parent_node, const std::string & plugin_name); - virtual std::expected on_initialize() = 0; + virtual void on_initialize() = 0; virtual void update(NavState & nav_state) = 0; const std::string & get_plugin_name() const; diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp index 23dd347..20e9a08 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/InflationFilter.hpp @@ -42,7 +42,6 @@ #ifndef EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ -#include #include #include @@ -79,7 +78,7 @@ class InflationFilter : public CostmapFilter public: InflationFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(NavState & nav_state); /** diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/ObstacleFilter.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/ObstacleFilter.hpp index 7195ad0..940a2e1 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/ObstacleFilter.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/filters/ObstacleFilter.hpp @@ -21,7 +21,6 @@ #ifndef EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ -#include #include #include "easynav_common/types/NavState.hpp" @@ -36,7 +35,7 @@ class ObstacleFilter : public CostmapFilter public: ObstacleFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(NavState & nav_state); }; diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp index 9b2b037..e38c5a1 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp @@ -17,7 +17,7 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include +#include #include #include "easynav_costmap_maps_manager/CostmapMapsManager.hpp" @@ -52,7 +52,7 @@ CostmapMapsManager::CostmapMapsManager() CostmapMapsManager::~CostmapMapsManager() {} -std::expected +void CostmapMapsManager::on_initialize() { auto node = get_node(); @@ -81,13 +81,13 @@ CostmapMapsManager::on_initialize() std::shared_ptr instance; instance = costmap_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + costmap_filter); - - if (!result) { + try { + instance->initialize(node, plugin_name + "." + costmap_filter); + } catch (std::runtime_error & ex) { RCLCPP_ERROR(node->get_logger(), - "Unable to initialize [%s]. Error: %s", plugin.c_str(), result.error().c_str()); - return std::unexpected("Unable to initialize " + - plugin + " . Error: " + result.error()); + "Unable to initialize [%s]. Error: %s", plugin.c_str(), ex.what()); + throw std::runtime_error("Unable to initialize " + + plugin + " . Error: " + ex.what()); } costmap_filters_.push_back(instance); @@ -97,7 +97,7 @@ CostmapMapsManager::on_initialize() } catch (pluginlib::PluginlibException & ex) { RCLCPP_ERROR(node->get_logger(), "Unable to load plugin easynav::CostmapFilter. Error: %s", ex.what()); - return std::unexpected("Unable to load plugin easynav::CostmapFilter " + + throw std::runtime_error("Unable to load plugin easynav::CostmapFilter " + costmap_filter + " . Error: " + ex.what()); } } @@ -117,12 +117,12 @@ CostmapMapsManager::on_initialize() const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + map_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } if (auto ret = loadMapFromYaml(map_path_, static_grid_msg_) != LOAD_MAP_SUCCESS) { std::cerr << "loadMapFromYaml returned" << ret << std::endl; - return std::unexpected("YAML file [" + map_path_ + "] not found or invalid: "); + throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); } static_map_ = Costmap2D(static_grid_msg_); @@ -169,8 +169,6 @@ CostmapMapsManager::on_initialize() response->message = "Map successfully saved to: " + map_path_; } }); - - return {}; } void diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/CostmapFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/CostmapFilter.cpp index 7d2b8f2..b172a16 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/CostmapFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/CostmapFilter.cpp @@ -18,7 +18,6 @@ // along with this program. If not, see . -#include #include #include "easynav_costmap_maps_manager/filters/CostmapFilter.hpp" @@ -31,7 +30,7 @@ CostmapFilter::CostmapFilter() } -std::expected +void CostmapFilter::initialize( const std::shared_ptr parent_node, const std::string & plugin_name) @@ -39,7 +38,7 @@ CostmapFilter::initialize( parent_node_ = parent_node; plugin_name_ = plugin_name; - return on_initialize(); + on_initialize(); } std::shared_ptr 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 9e35b32..4aeffd9 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 @@ -39,7 +39,6 @@ *********************************************************************/ -#include #include #include "easynav_costmap_common/costmap_2d.hpp" @@ -71,7 +70,7 @@ InflationFilter::InflationFilter() { } -std::expected +void InflationFilter::on_initialize() { auto node = get_node(); @@ -95,8 +94,6 @@ InflationFilter::on_initialize() cached_distances_.clear(); cached_costs_.clear(); need_reinflation_ = false; - - return {}; } void diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp index b8a69cb..234803c 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/ObstacleFilter.cpp @@ -18,7 +18,6 @@ // along with this program. If not, see . -#include #include #include "easynav_costmap_common/costmap_2d.hpp" @@ -39,11 +38,9 @@ ObstacleFilter::ObstacleFilter() } -std::expected +void ObstacleFilter::on_initialize() -{ - return {}; -} +{} void ObstacleFilter::update(NavState & nav_state) diff --git a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt index f1e05f0..ba12d98 100644 --- a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_navmap_maps_manager/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) - list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp index 13f707a..6f7c2c1 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp @@ -68,9 +68,9 @@ class NavMapMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp index 13bbc9f..74f1725 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp @@ -39,7 +39,7 @@ class InflationFilter : public NavMapFilter public: InflationFilter(); - virtual std::expected on_initialize() override; + virtual void on_initialize() override; virtual void update(::easynav::NavState & nav_state) override; bool inflate_layer_u8( diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp index 9bda724..a561ac7 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp @@ -21,7 +21,6 @@ #ifndef EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ -#include #include #include "easynav_common/types/NavState.hpp" @@ -37,12 +36,12 @@ class NavMapFilter public: NavMapFilter(); - std::expected + void initialize( const std::shared_ptr parent_node, const std::string & plugin_name); - virtual std::expected on_initialize() = 0; + virtual void on_initialize() = 0; virtual void update(::easynav::NavState & nav_state) = 0; virtual bool is_adding_layer() {return false;} diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp index bd07c70..d4c8701 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp @@ -21,7 +21,6 @@ #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ #define EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ -#include #include #include "navmap_core/NavMap.hpp" @@ -39,7 +38,7 @@ class ObstacleFilter : public NavMapFilter public: ObstacleFilter(); - virtual std::expected on_initialize() override; + virtual void on_initialize() override; virtual void update(::easynav::NavState & nav_state) override; virtual bool is_adding_layer() override {return true;} diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp index c1cb22f..c230cb4 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp @@ -17,7 +17,7 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include +#include #include #include "easynav_navmap_maps_manager/NavMapMapsManager.hpp" @@ -63,7 +63,7 @@ NavMapMapsManager::NavMapMapsManager() NavMapMapsManager::~NavMapMapsManager() {} -std::expected +void NavMapMapsManager::on_initialize() { auto node = get_node(); @@ -93,13 +93,12 @@ NavMapMapsManager::on_initialize() std::shared_ptr instance; instance = navmap_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + navmap_filter); - - if (!result) { + try { + instance->initialize(node, plugin_name + "." + navmap_filter); + } catch (const std::runtime_error & ex) { RCLCPP_ERROR(node->get_logger(), - "Unable to initialize [%s]. Error: %s", plugin.c_str(), result.error().c_str()); - return std::unexpected("Unable to initialize " + - plugin + " . Error: " + result.error()); + "Unable to initialize [%s]. Error: %s", plugin.c_str(), ex.what()); + throw; } navmap_filters_.push_back(instance); @@ -109,7 +108,7 @@ NavMapMapsManager::on_initialize() } catch (pluginlib::PluginlibException & ex) { RCLCPP_ERROR(node->get_logger(), "Unable to load plugin easynav::navmap::NavMapFilter. Error: %s", ex.what()); - return std::unexpected("Unable to load plugin easynav::navmap::NavMapFilter " + + throw std::runtime_error("Unable to load plugin easynav::navmap::NavMapFilter " + navmap_filter + " . Error: " + ex.what()); } } @@ -130,13 +129,13 @@ NavMapMapsManager::on_initialize() const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + occmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } nav_msgs::msg::OccupancyGrid occ_msg; if (auto ret = loadMapFromYaml(map_path_, occ_msg) != LOAD_MAP_SUCCESS) { std::cerr << "loadMapFromYaml returned" << ret << std::endl; - return std::unexpected("YAML file [" + map_path_ + "] not found or invalid: "); + throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); } resolution_ = occ_msg.info.resolution; @@ -153,7 +152,7 @@ NavMapMapsManager::on_initialize() const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + std::string("/") + navmap_path_file; } catch (ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } if (navmap_ros::io::load_from_file(map_path_, navmap_)) { @@ -206,8 +205,6 @@ NavMapMapsManager::on_initialize() navmap_ros::io::save_to_file(navmap_, "/tmp/map.navmap"); // ToDo }); - - return {}; } void diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp index 2d9e8f7..33d6c95 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp @@ -39,7 +39,6 @@ *********************************************************************/ -#include #include #include @@ -196,7 +195,7 @@ bool InflationFilter::inflate_layer_u8( return true; } -std::expected +void InflationFilter::on_initialize() { auto node = get_node(); @@ -217,8 +216,6 @@ InflationFilter::on_initialize() RCLCPP_INFO(node->get_logger(), "InflationFilter (NavMap): radius=%.3f cost_scaling=%.3f inscribed=%.3f", inflation_radius_, cost_scaling_factor_, inscribed_radius_); - - return {}; } void InflationFilter::update(::easynav::NavState & nav_state) diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/NavMapFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/NavMapFilter.cpp index 970c8e1..139278b 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/NavMapFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/NavMapFilter.cpp @@ -18,7 +18,6 @@ // along with this program. If not, see . -#include #include #include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" @@ -33,7 +32,7 @@ NavMapFilter::NavMapFilter() } -std::expected +void NavMapFilter::initialize( const std::shared_ptr parent_node, const std::string & plugin_name) @@ -41,7 +40,7 @@ NavMapFilter::initialize( parent_node_ = parent_node; plugin_name_ = plugin_name; - return on_initialize(); + on_initialize(); } std::shared_ptr diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index c70dc61..cddda7b 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp @@ -18,7 +18,6 @@ // along with this program. If not, see . -#include #include #include @@ -42,11 +41,9 @@ ObstacleFilter::ObstacleFilter() } -std::expected +void ObstacleFilter::on_initialize() -{ - return {}; -} +{} void ObstacleFilter::update(::easynav::NavState & nav_state) { diff --git a/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt b/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt index ea292b8..fc7b4b2 100644 --- a/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_octomap_maps_manager/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) - list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake_modules) find_package(ament_cmake REQUIRED) diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp index ec358d5..8b4f79e 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/OctomapMapsManager.hpp @@ -67,9 +67,9 @@ class OctomapMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/InflationFilter.hpp index a2d42a7..c052472 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/InflationFilter.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/InflationFilter.hpp @@ -21,10 +21,8 @@ #ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ #define EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ -#include #include - #include "pluginlib/class_loader.hpp" #include "octomap_core/Octomap.hpp" @@ -42,7 +40,7 @@ class InflationFilter : public OctomapFilter public: InflationFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(::easynav::NavState & nav_state); bool inflate_layer_u8( diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/ObstacleFilter.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/ObstacleFilter.hpp index 91d57b7..571433c 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/ObstacleFilter.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/ObstacleFilter.hpp @@ -21,7 +21,6 @@ #ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ #define EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ -#include #include #include "pluginlib/class_loader.hpp" @@ -41,7 +40,7 @@ class ObstacleFilter : public OctomapFilter public: ObstacleFilter(); - virtual std::expected on_initialize(); + virtual void on_initialize(); virtual void update(::easynav::NavState & nav_state); bool is_adding_layer() override {return true;} diff --git a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/OctomapFilter.hpp b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/OctomapFilter.hpp index 6561d7e..efa8a98 100644 --- a/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/OctomapFilter.hpp +++ b/maps_managers/easynav_octomap_maps_manager/include/easynav_octomap_maps_manager/filters/OctomapFilter.hpp @@ -21,7 +21,6 @@ #ifndef EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ #define EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ -#include #include #include "octomap/octomap.h" @@ -38,12 +37,12 @@ class OctomapFilter public: OctomapFilter(); - std::expected + void initialize( const std::shared_ptr parent_node, const std::string & plugin_name); - virtual std::expected on_initialize() = 0; + virtual void on_initialize() = 0; virtual void update(::easynav::NavState & nav_state) = 0; float get_map_resolution() {return map_resolution_;} diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp index 347bd7e..a3fd6aa 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/OctomapMapsManager.cpp @@ -17,7 +17,6 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include #include #include "easynav_octomap_maps_manager/OctomapMapsManager.hpp" @@ -60,7 +59,7 @@ OctomapMapsManager::OctomapMapsManager() OctomapMapsManager::~OctomapMapsManager() {} -std::expected +void OctomapMapsManager::on_initialize() { auto node = get_node(); @@ -96,7 +95,7 @@ OctomapMapsManager::on_initialize() // if (!result) { // RCLCPP_ERROR(node->get_logger(), // "Unable to initialize [%s]. Error: %s", plugin.c_str(), result.error().c_str()); - // return std::unexpected("Unable to initialize " + + // throw std::runtime_error("Unable to initialize " + // plugin + " . Error: " + result.error()); // } // @@ -107,7 +106,7 @@ OctomapMapsManager::on_initialize() // } catch (pluginlib::PluginlibException & ex) { // RCLCPP_ERROR(node->get_logger(), // "Unable to load plugin easynav::octomap::OctomapFilter. Error: %s", ex.what()); - // return std::unexpected("Unable to load plugin easynav::octomap::OctomapFilter " + + // throw std::runtime_error("Unable to load plugin easynav::octomap::OctomapFilter " + // octomap_filter + " . Error: " + ex.what()); // } // } @@ -121,13 +120,13 @@ OctomapMapsManager::on_initialize() // const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); // map_path_ = pkgpath + std::string("/") + occmap_path_file; // } catch (ament_index_cpp::PackageNotFoundError & ex) { -// return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); +// throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); // } // // nav_msgs::msg::OccupancyGrid occ_msg; // if (auto ret = loadMapFromYaml(map_path_, occ_msg) != LOAD_MAP_SUCCESS) { // std::cerr << "loadMapFromYaml returned" << ret << std::endl; -// return std::unexpected("YAML file [" + map_path_ + "] not found or invalid: "); +// throw std::runtime_error("YAML file [" + map_path_ + "] not found or invalid: "); // } // // resolution_ = occ_msg.info.resolution; @@ -144,7 +143,7 @@ OctomapMapsManager::on_initialize() // const std::string pkgpath = ament_index_cpp::get_package_share_directory(package_name); // map_path_ = pkgpath + std::string("/") + occmap_path_file; // } catch (ament_index_cpp::PackageNotFoundError & ex) { -// return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); +// throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); // } // // if (octomap_ros::io::load_from_file(map_path_, octomap_)) { @@ -248,8 +247,6 @@ OctomapMapsManager::on_initialize() // ToDo }); - - return {}; } void diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp index c9cce30..0703a15 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp @@ -39,7 +39,6 @@ *********************************************************************/ -#include #include #include "easynav_common/types/NavState.hpp" @@ -178,7 +177,7 @@ bool InflationFilter::inflate_layer_u8( return true; } -std::expected +void InflationFilter::on_initialize() { auto node = get_node(); @@ -197,8 +196,6 @@ InflationFilter::on_initialize() RCLCPP_INFO(node->get_logger(), "InflationFilter with inflation_radius = %lf cost_scaling_factor = %lf", inflation_radius_, cost_scaling_factor_); - - return {}; } void diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp index df33154..ff276ef 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/ObstacleFilter.cpp @@ -18,7 +18,6 @@ // along with this program. If not, see . -#include #include #include "easynav_common/types/NavState.hpp" @@ -40,11 +39,9 @@ ObstacleFilter::ObstacleFilter() } -std::expected +void ObstacleFilter::on_initialize() -{ - return {}; -} +{} void ObstacleFilter::update(::easynav::NavState & nav_state) diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/OctomapFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/OctomapFilter.cpp index ebc222e..b25012c 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/OctomapFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/OctomapFilter.cpp @@ -18,7 +18,6 @@ // along with this program. If not, see . -#include #include #include "octomap/octomap.h" @@ -35,14 +34,14 @@ OctomapFilter::OctomapFilter() } -std::expected +void OctomapFilter::initialize( const std::shared_ptr parent_node, const std::string & plugin_name) { parent_node_ = parent_node; plugin_name_ = plugin_name; - return on_initialize(); + on_initialize(); } std::shared_ptr diff --git a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt index f817f36..18cd1a9 100644 --- a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_routes_maps_manager/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/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp index f3b1e1b..192af48 100644 --- a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesFilter.hpp @@ -20,7 +20,6 @@ #ifndef EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ #define EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ -#include #include #include #include @@ -58,9 +57,8 @@ class RoutesFilter /// @param plugin_ns Namespace under which this filter is configured /// (used as prefix for ROS parameters and topics). /// @param tf_info TF frame information used by the navigation stack. - /// @return std::expected Empty on success or - /// an error message describing the failure. - virtual std::expected initialize( + /// @throws std::runtime_error if initialization fails. + virtual void initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & plugin_ns) = 0; diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp index c2f4705..37fb17f 100644 --- a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/RoutesMapsManager.hpp @@ -94,9 +94,9 @@ class RoutesMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. diff --git a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp index 449bb74..c910a91 100644 --- a/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp +++ b/maps_managers/easynav_routes_maps_manager/include/easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp @@ -20,7 +20,6 @@ #ifndef EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ #define EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ -#include #include #include "easynav_routes_maps_manager/RoutesFilter.hpp" @@ -54,9 +53,8 @@ class RoutesCostmapFilter : public RoutesFilter /// @param plugin_ns Namespace used to resolve this filter's /// parameters and topics. /// @param tf_info TF frame information used by the navigation stack. - /// @return std::expected Empty on success or an - /// error message on failure. - std::expected initialize( + /// @throws std::runtime_error if initialization fails. + void initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & plugin_ns) override; diff --git a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp index 12f27b2..2601842 100644 --- a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/RoutesMapsManager.cpp @@ -21,7 +21,6 @@ #include "easynav_routes_maps_manager/RoutesMapsManager.hpp" #include "easynav_common/RTTFBuffer.hpp" -#include #include #include @@ -54,7 +53,7 @@ RoutesMapsManager::RoutesMapsManager() RoutesMapsManager::~RoutesMapsManager() = default; -std::expected RoutesMapsManager::on_initialize() +void RoutesMapsManager::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); @@ -91,7 +90,7 @@ std::expected RoutesMapsManager::on_initialize() const auto pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + "/" + map_path_file; } else { - return std::unexpected( + throw std::runtime_error( "Parameters '" + plugin_name + ".package' and '" + plugin_name + ".map_path_file' are not correctly set"); } @@ -182,7 +181,7 @@ std::expected RoutesMapsManager::on_initialize() publish_routes_markers(); publish_interactive_markers(); } catch (const std::exception & e) { - return std::unexpected(std::string{"Failed to load routes: "} + e.what()); + throw std::runtime_error(std::string{"Failed to load routes: "} + e.what()); } // Instantiate and initialize configured route filters @@ -206,13 +205,14 @@ std::expected RoutesMapsManager::on_initialize() std::shared_ptr instance = routes_filters_loader_->createSharedInstance(plugin); - auto result = instance->initialize(node, plugin_name + "." + filter_name); - if (!result) { + try { + instance->initialize(node, plugin_name + "." + filter_name); + } catch (const std::exception & e) { RCLCPP_ERROR(node->get_logger(), "Unable to initialize RoutesFilter %s [%s]. Error: %s", - filter_name.c_str(), plugin.c_str(), result.error().c_str()); - return std::unexpected("Unable to initialize RoutesFilter " + plugin + - " . Error: " + result.error()); + filter_name.c_str(), plugin.c_str(), e.what()); + throw std::runtime_error("Unable to initialize RoutesFilter " + plugin + + " . Error: " + e.what()); } routes_filters_.push_back(instance); @@ -222,12 +222,10 @@ std::expected RoutesMapsManager::on_initialize() } catch (pluginlib::PluginlibException & ex) { RCLCPP_ERROR(node->get_logger(), "Unable to load plugin easynav::RoutesFilter. Error: %s", ex.what()); - return std::unexpected("Unable to load plugin easynav::RoutesFilter " + + throw std::runtime_error("Unable to load plugin easynav::RoutesFilter " + filter_name + " . Error: " + ex.what()); } } - - return {}; } void RoutesMapsManager::update(NavState & nav_state) diff --git a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp index adf222e..83908e1 100644 --- a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/filters/RoutesCostmapFilter.cpp @@ -37,7 +37,7 @@ namespace easynav RoutesCostmapFilter::RoutesCostmapFilter() = default; -std::expected +void RoutesCostmapFilter::initialize( const rclcpp_lifecycle::LifecycleNode::SharedPtr & node, const std::string & plugin_ns) @@ -68,8 +68,6 @@ RoutesCostmapFilter::initialize( routes_occ_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/routes_map", rclcpp::QoS(1).reliable()); - - return {}; } void diff --git a/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp b/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp index a0e2d98..40d8c72 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_costmap_filter_tests.cpp @@ -59,8 +59,7 @@ TEST_F(RoutesCostmapFilterTest, DoesNothingWhenNavStateMissingKeys) easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = filter.initialize(node, "routes.routes_costmap"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); NavState nav_state; // No 'routes' and no 'map.dynamic.filtered' keys -> update should not crash @@ -75,8 +74,7 @@ TEST_F(RoutesCostmapFilterTest, RaisesCostOutsideRoutes) easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = filter.initialize(node, "routes.routes_costmap"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); // Simple costmap 10x1, resolution 1.0, origin at (0,0) Costmap2D map(10, 1, 1.0, 0.0, 0.0); @@ -126,8 +124,7 @@ TEST_F(RoutesCostmapFilterTest, IgnoresRoutePointsOutsideMap) easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = filter.initialize(node, "routes.routes_costmap"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(filter.initialize(node, "routes.routes_costmap")); // Costmap 5x1 from x=0..5 Costmap2D map(5, 1, 1.0, 0.0, 0.0); diff --git a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp index 6f84545..665a25a 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_tests.cpp @@ -92,8 +92,7 @@ TEST_F(RoutesMapsManagerTest, LoadsRoutesFromValidYaml) easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = manager->initialize(node, "routes"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(manager->initialize(node, "routes")); const auto & routes = manager->get_routes(); ASSERT_EQ(routes.size(), 2u); @@ -120,8 +119,7 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenMapPathEmpty) auto manager = std::make_shared(); easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = manager->initialize(node, "routes"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(manager->initialize(node, "routes")); const auto & routes = manager->get_routes(); ASSERT_EQ(routes.size(), 1u); @@ -147,8 +145,7 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenYamlMissing) auto manager = std::make_shared(); easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = manager->initialize(node, "routes"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(manager->initialize(node, "routes")); const auto & routes = manager->get_routes(); ASSERT_EQ(routes.size(), 1u); @@ -175,8 +172,7 @@ TEST_F(RoutesMapsManagerTest, DefaultRouteWhenNoRoutesKey) auto manager = std::make_shared(); easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = manager->initialize(node, "routes"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(manager->initialize(node, "routes")); const auto & routes = manager->get_routes(); ASSERT_EQ(routes.size(), 1u); @@ -209,8 +205,7 @@ TEST_F(RoutesMapsManagerTest, UpdateWritesRoutesIntoNavState) easynav::TFInfo tf_info; easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); - auto init_result = manager->initialize(node, "routes"); - ASSERT_TRUE(init_result.has_value()) << init_result.error(); + ASSERT_NO_THROW(manager->initialize(node, "routes")); easynav::NavState nav_state; manager->update(nav_state); diff --git a/maps_managers/easynav_simple_maps_manager/CMakeLists.txt b/maps_managers/easynav_simple_maps_manager/CMakeLists.txt index 8d7be65..b47cd6c 100644 --- a/maps_managers/easynav_simple_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_simple_maps_manager/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/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp b/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp index a7455b6..bd9a203 100644 --- a/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp +++ b/maps_managers/easynav_simple_maps_manager/include/easynav_simple_maps_manager/SimpleMapsManager.hpp @@ -58,9 +58,9 @@ class SimpleMapsManager : public easynav::MapsManagerBase * * Creates necessary publishers/subscribers and initializes the map instances. * - * @return std::expected Success or error string. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. diff --git a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp index 4410d40..411cdf8 100644 --- a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp +++ b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp @@ -20,8 +20,6 @@ /// \file /// \brief Implementation of the SimpleMapsManager class. -#include - #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" #include "easynav_common/types/PointPerception.hpp" @@ -51,7 +49,7 @@ SimpleMapsManager::~SimpleMapsManager() } -std::expected +void SimpleMapsManager::on_initialize() { auto node = get_node(); @@ -71,11 +69,11 @@ SimpleMapsManager::on_initialize() pkgpath = ament_index_cpp::get_package_share_directory(package_name); map_path_ = pkgpath + "/" + map_path_file; } catch(ament_index_cpp::PackageNotFoundError & ex) { - return std::unexpected("Package " + package_name + " not found. Error: " + ex.what()); + throw std::runtime_error("Package " + package_name + " not found. Error: " + ex.what()); } if (!static_map_.load_from_file(map_path_)) { - return std::unexpected("File [" + map_path_ + "] not found"); + throw std::runtime_error("File [" + map_path_ + "] not found"); } } @@ -123,8 +121,6 @@ SimpleMapsManager::on_initialize() static_grid_msg_.header.stamp = node->now(); static_occ_pub_->publish(static_grid_msg_); - - return {}; } void diff --git a/planners/easynav_costmap_planner/CMakeLists.txt b/planners/easynav_costmap_planner/CMakeLists.txt index 77ef543..580b6d7 100644 --- a/planners/easynav_costmap_planner/CMakeLists.txt +++ b/planners/easynav_costmap_planner/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/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp index c852a09..0b13806 100644 --- a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp +++ b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp @@ -54,9 +54,9 @@ class CostmapPlanner : public PlannerMethodBase * Loads planner parameters, sets up ROS publishers, * and prepares the costmap-based planning environment. * - * @return std::expected A success indicator or error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Executes a planning cycle using the current navigation state. diff --git a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp index 1998f9d..59d6d76 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -123,7 +123,7 @@ CostmapPlanner::CostmapPlanner() }); } -std::expected CostmapPlanner::on_initialize() +void CostmapPlanner::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); @@ -139,7 +139,6 @@ std::expected CostmapPlanner::on_initialize() path_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); - return {}; } void CostmapPlanner::update(NavState & nav_state) diff --git a/planners/easynav_navmap_planner/CMakeLists.txt b/planners/easynav_navmap_planner/CMakeLists.txt index 950f854..6e0ec15 100644 --- a/planners/easynav_navmap_planner/CMakeLists.txt +++ b/planners/easynav_navmap_planner/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/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp index c5fcb48..ce01f50 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -58,9 +58,9 @@ class AStarPlanner : public PlannerMethodBase * Loads planner parameters, sets up ROS publishers, * and prepares the NavMap-based planning environment. * - * @return std::expected A success indicator or error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Executes a planning cycle using the current navigation state. diff --git a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp index bc02c40..044ddee 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -63,7 +62,7 @@ AStarPlanner::AStarPlanner() }); } -std::expected AStarPlanner::on_initialize() +void AStarPlanner::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); @@ -76,7 +75,6 @@ std::expected AStarPlanner::on_initialize() path_pub_ = node->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); - return {}; } void AStarPlanner::update(NavState & nav_state) diff --git a/planners/easynav_simple_planner/CMakeLists.txt b/planners/easynav_simple_planner/CMakeLists.txt index 5d6975c..0282ad7 100644 --- a/planners/easynav_simple_planner/CMakeLists.txt +++ b/planners/easynav_simple_planner/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_simple_common REQUIRED) diff --git a/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp b/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp index f54b912..48bda79 100644 --- a/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp +++ b/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp @@ -51,9 +51,9 @@ class SimplePlanner : public PlannerMethodBase * Configures publishers, retrieves parameters, and prepares the planner * for path generation using the available map data. * - * @return std::expected Success or an error message. + * @throws std::runtime_error if initialization fails. */ - virtual std::expected on_initialize() override; + virtual void on_initialize() override; /** * @brief Updates the planner by computing a new path. diff --git a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp index 899e29c..3315d17 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -86,7 +86,7 @@ SimplePlanner::SimplePlanner() }); } -std::expected +void SimplePlanner::on_initialize() { auto node = get_node(); @@ -99,8 +99,6 @@ SimplePlanner::on_initialize() path_pub_ = get_node()->create_publisher( node->get_fully_qualified_name() + std::string("/") + plugin_name + "/path", 10); - - return {}; } void @@ -247,7 +245,7 @@ SimplePlanner::a_star_path( double new_cost = cost_so_far[idx(current.x, current.y)] + hypot(dx, dy); int nid = idx(nx, ny); - if (!cost_so_far.contains(nid) || new_cost < cost_so_far[nid]) { + if (cost_so_far.find(nid) == cost_so_far.end() || new_cost < cost_so_far[nid]) { cost_so_far[nid] = new_cost; double priority = new_cost + heuristic(nx, ny, gx, gy); open.push({nx, ny, new_cost, priority}); @@ -258,7 +256,7 @@ SimplePlanner::a_star_path( std::vector path; int cx = gx, cy = gy; - while (came_from.contains(idx(cx, cy))) { + while (came_from.find(idx(cx, cy) ) != came_from.end()) { geometry_msgs::msg::Pose pose; auto [px, py] = map.cell_to_metric(cx, cy);