diff --git a/common/easynav_costmap_common/CMakeLists.txt b/common/easynav_costmap_common/CMakeLists.txt index de103729..f5482197 100644 --- a/common/easynav_costmap_common/CMakeLists.txt +++ b/common/easynav_costmap_common/CMakeLists.txt @@ -47,9 +47,20 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Files derived from ROS / Nav2 keep their original copyright and license + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) ament_lint_auto_find_test_dependencies() + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if(NOT "$ENV{ROS_DISTRO}" STREQUAL "humble") + find_package(ament_cmake_copyright REQUIRED) + ament_copyright(EXCLUDE + include/easynav_costmap_common/cost_values.hpp + include/easynav_costmap_common/costmap_2d.hpp + include/easynav_costmap_common/occ_grid_values.hpp + src/easynav_costmap_common/costmap_2d.cpp + tests/costmap_bresenham_2d.cpp + ) + endif() find_package(ament_cmake_gtest REQUIRED) add_subdirectory(tests) diff --git a/common/easynav_costmap_common/package.xml b/common/easynav_costmap_common/package.xml index 353d6c5d..c0173622 100644 --- a/common/easynav_costmap_common/package.xml +++ b/common/easynav_costmap_common/package.xml @@ -25,6 +25,7 @@ tf2_geometry_msgs ament_cmake_gtest + ament_cmake_copyright ament_lint_auto ament_lint_common diff --git a/common/easynav_simple_common/CMakeLists.txt b/common/easynav_simple_common/CMakeLists.txt index 742f2ce2..7b916b87 100644 --- a/common/easynav_simple_common/CMakeLists.txt +++ b/common/easynav_simple_common/CMakeLists.txt @@ -43,8 +43,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp b/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp index 4039c221..af7cbb0e 100644 --- a/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp +++ b/common/easynav_simple_common/include/easynav_simple_common/SimpleMap.hpp @@ -16,11 +16,13 @@ /// \file /// \brief Declaration of the SimpleMap type. -#ifndef EASYNAV_PLANNER__SIMPLEMAP_HPP_ -#define EASYNAV_PLANNER__SIMPLEMAP_HPP_ +#ifndef EASYNAV_SIMPLE_COMMON__SIMPLEMAP_HPP_ +#define EASYNAV_SIMPLE_COMMON__SIMPLEMAP_HPP_ #include #include +#include +#include #include "nav_msgs/msg/occupancy_grid.hpp" @@ -213,4 +215,4 @@ class SimpleMap } // namespace easynav -#endif // EASYNAV_PLANNER__SIMPLEMAP_HPP_ +#endif // EASYNAV_SIMPLE_COMMON__SIMPLEMAP_HPP_ diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt index 5c300e6e..984e4ad9 100644 --- a/controllers/easynav_mpc_controller/CMakeLists.txt +++ b/controllers/easynav_mpc_controller/CMakeLists.txt @@ -134,8 +134,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest 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 8c5ac0d9..f754ca4a 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -18,26 +18,22 @@ #define EASYNAV_MPC_CONTROLLER__MPCCONTROLLER_HPP_ #include -#include #include +#include -#include - +#include "Eigen/Core" +#include "tf2/LinearMath/Quaternion.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/path.hpp" - -#include -#include - +#include "pcl/io/pcd_io.h" +#include "pcl_conversions/pcl_conversions.h" #include "sensor_msgs/msg/point_cloud2.hpp" - #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" #include "easynav_sensors/types/PointPerception.hpp" - #include "easynav_mpc_controller/MPCOptimizer.hpp" namespace easynav @@ -93,12 +89,13 @@ class MPCController : public ControllerMethodBase std::unique_ptr optimizer_; ///< MPC optimizer - rclcpp::Publisher::SharedPtr mpc_path_pub_; ///< Publisher for MPC path. - rclcpp::Publisher::SharedPtr detection_pub_; ///< Publisher for MPC obstacles. + /// Publisher for MPC path. + rclcpp::Publisher::SharedPtr mpc_path_pub_; + /// Publisher for MPC obstacles. + rclcpp::Publisher::SharedPtr detection_pub_; private: geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. - }; } // namespace easynav diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp index 40ca6ce6..e6062e92 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCOptimizer.hpp @@ -17,6 +17,8 @@ #ifndef EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ #define EASYNAV_MPC_CONTROLLER__MPCOPTIMIZER_HPP_ +#include + #include "geometry_msgs/msg/pose.hpp" #include "nav_msgs/msg/path.hpp" #include "pcl/point_cloud.h" @@ -76,7 +78,6 @@ class MPCParameters Eigen::Matrix2d R_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Effort Cost Matrix Eigen::Matrix2d Rd_ {{0.1, 0.0}, {0.0, 0.1}}; ///< Smooth Cost Matrix double qtheta_ {3.0}; ///< Angular cost value. - }; /// \brief A MPC Optimizer class. @@ -101,7 +102,8 @@ class MPCOptimizer /// \brief Wrap for real cost function /// \param u std::vector Velocity vector to be optimized - /// \param grad std::vector gradient values for optimizer. It is NOT used for this implementation. + /// \param grad std::vector gradient values for optimizer. It is NOT used for this + /// implementation. /// \param data MPCParameter pointer with parameters used by optimizer /// \return double cost value used by nlOpt in internal callback double cost_function(const std::vector & u, [[maybe_unused]] @@ -109,13 +111,13 @@ class MPCOptimizer /// \brief Real cost function with static propierties /// \param u std::vector Velocity vector to be optimized - /// \param grad std::vector gradient values for optimizer. It is NOT used for this implementation. + /// \param grad std::vector gradient values for optimizer. It is NOT used for this + /// implementation. /// \param data MPCParameter pointer with parameters used by optimizer /// \return double cost value used by nlOpt in internal callback static double nlopt_cost_callback( const std::vector & x, std::vector & grad, void * data); - }; /// \brief Struct used as element in callback. 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 dea9ea4e..06e2f2a2 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -100,7 +100,6 @@ MPCController::publish_mpc_path( mpc_path_.poses.push_back(pose_stamped); } mpc_path_pub_->publish(mpc_path_); - } } @@ -297,7 +296,6 @@ MPCController::update_rt(NavState & nav_state) std::cerr << "Optimization Unsuccessful " << std::endl; } } - } catch (std::exception & e) { std::cerr << "Optimization Error: " << e.what() << std::endl; } diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp index 067b1495..635c5a47 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp @@ -134,7 +134,6 @@ MPCOptimizer::cost_function( } return cost; - } double diff --git a/controllers/easynav_mpc_controller/tests/mpc_controller_legacy_params_file_tests.cpp b/controllers/easynav_mpc_controller/tests/mpc_controller_legacy_params_file_tests.cpp index c4883dd9..24ca6be3 100644 --- a/controllers/easynav_mpc_controller/tests/mpc_controller_legacy_params_file_tests.cpp +++ b/controllers/easynav_mpc_controller/tests/mpc_controller_legacy_params_file_tests.cpp @@ -14,7 +14,8 @@ /// \file /// \brief A real parameter file from before robot_limits (easynav_indoor_testcase, -/// bonxai.amcl.params.urjc_alt_imu.yaml) keeps working: no limits of its own, and those of inactive controllers are ignored. +/// bonxai.amcl.params.urjc_alt_imu.yaml) keeps working: no limits of its own, and those of inactive +/// controllers are ignored. #include #include diff --git a/controllers/easynav_mppi_controller/CMakeLists.txt b/controllers/easynav_mppi_controller/CMakeLists.txt index 53185b68..55dd994b 100644 --- a/controllers/easynav_mppi_controller/CMakeLists.txt +++ b/controllers/easynav_mppi_controller/CMakeLists.txt @@ -61,8 +61,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest 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 6c9580d3..453ade99 100644 --- a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp +++ b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp @@ -18,14 +18,13 @@ #include #include +#include +#include #include "geometry_msgs/msg/twist_stamped.hpp" - #include "easynav_core/ControllerMethodBase.hpp" #include "easynav_common/types/NavState.hpp" - #include "easynav_mppi_controller/MPPIOptimizer.hpp" - #include "visualization_msgs/msg/marker_array.hpp" namespace easynav @@ -58,7 +57,7 @@ class MPPIController : public ControllerMethodBase double max_lin_acc_{0.5}; ///< Maximum linear acceleration for MPPI. double max_ang_acc_{1.0}; ///< Maximum angular acceleration for MPPI. double fov_{M_PI / 2.0}; ///< Field of view for MPPI. - double safety_radius_{0.6}; ///< Safety radius for obstacle avoidance. + double safety_radius_{0.6}; ///< Safety radius for obstacle avoidance. double obstacle_range_{2.0}; ///< Obstacles farther than this from the robot are ignored (m). double z_min_filter_{0.0}; ///< Points below this height (robot frame) are the ground (m). double robot_radius_{0.3}; ///< From robot_geometry. @@ -67,8 +66,10 @@ class MPPIController : public ControllerMethodBase geometry_msgs::msg::TwistStamped twist_stamped_; ///< Current velocity command. std::unique_ptr optimizer_; ///< MPPI optimizer - rclcpp::Publisher::SharedPtr mppi_candidates_pub_; ///< Publisher for MPPI candidates markers. - rclcpp::Publisher::SharedPtr mppi_optimal_pub_; ///< Publisher for MPPI optimal path markers. + /// Publisher for MPPI candidates markers. + rclcpp::Publisher::SharedPtr mppi_candidates_pub_; + /// Publisher for MPPI optimal path markers. + rclcpp::Publisher::SharedPtr mppi_optimal_pub_; /// \brief Obstacle points (map frame) within the robot's height, around it and towards where /// it moves (behind it when \p backward). @@ -80,7 +81,6 @@ class MPPIController : public ControllerMethodBase void publish_mppi_markers( const std::vector>> & all_trajs, const std::vector> & best_traj); - }; } // namespace easynav diff --git a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp index d900e166..9aa36f1a 100644 --- a/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp +++ b/controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIOptimizer.hpp @@ -32,10 +32,12 @@ namespace easynav /// \brief Result structure for MPPI optimization containing control commands and trajectories. struct MPPIResult { - double v; ///< Linear velocity command. - double w; ///< Angular velocity command. - std::vector>> all_trajectories; ///< All sampled trajectories. - std::vector> best_trajectory; ///< Best trajectory found during optimization. + double v; ///< Linear velocity command. + double w; ///< Angular velocity command. + /// All sampled trajectories. + std::vector>> all_trajectories; + /// Best trajectory found during optimization. + std::vector> best_trajectory; }; struct TrajectorySample @@ -88,10 +90,13 @@ class MPPIOptimizer double last_v_ = 0.0; ///< Last linear velocity command for smoothing. double last_w_ = 0.0; ///< Last angular velocity command for smoothing. - std::default_random_engine rng_; ///< Random number generator for sampling. - std::normal_distribution normal_ = std::normal_distribution(0.0, 0.5); ///< Normal distribution for noise in sampling. - std::normal_distribution v_noise_ = std::normal_distribution(0.0, 0.05); ///< Normal distribution for noise in linear velocity. - std::normal_distribution w_noise_ = std::normal_distribution(0.0, 0.02); ///< Normal distribution for noise in angular velocity. + std::default_random_engine rng_; ///< Random number generator for sampling. + /// Normal distribution for noise in sampling. + std::normal_distribution normal_ = std::normal_distribution(0.0, 0.5); + /// Normal distribution for noise in linear velocity. + std::normal_distribution v_noise_ = std::normal_distribution(0.0, 0.05); + /// Normal distribution for noise in angular velocity. + std::normal_distribution w_noise_ = std::normal_distribution(0.0, 0.02); /// \brief Computes the cost of a trajectory based on its distance to the path and heading error. /// \param trajectory The trajectory to evaluate. @@ -120,7 +125,8 @@ class MPPIOptimizer double x, double y, double yaw, double v, double w, const nav_msgs::msg::Path & path, int steps); - /// \brief Computes the heading error between the robot's current orientation and the target point. + /// \brief Computes the heading error between the robot's current orientation and the target + /// point. /// \param robot_yaw Current yaw orientation of the robot. /// \param target_x X coordinate of the target point. /// \param target_y Y coordinate of the target point. @@ -137,7 +143,6 @@ class MPPIOptimizer /// \param to Ending angle in radians. /// \return The shortest angular distance in radians. double shortest_angular_distance(double from, double to); - }; } // namespace easynav diff --git a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp index 2b03cca1..169c32da 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp +++ b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIOptimizer.cpp @@ -1,11 +1,27 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "easynav_mppi_controller/MPPIOptimizer.hpp" -#include "tf2/utils.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include #include #include +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + namespace easynav { @@ -260,7 +276,7 @@ MPPIResult MPPIOptimizer::compute_control( } if (min_v_sample < 0.05) { - lambda_ = std::min(5.0, lambda_ * 1.2); // increase lambda if tends to zero (stop) + lambda_ = std::min(5.0, lambda_ * 1.2); // increase lambda if tends to zero (stop) } // Softmin weighting of samples diff --git a/controllers/easynav_regulated_pp_controller/CMakeLists.txt b/controllers/easynav_regulated_pp_controller/CMakeLists.txt index e7a131ae..92bec03b 100644 --- a/controllers/easynav_regulated_pp_controller/CMakeLists.txt +++ b/controllers/easynav_regulated_pp_controller/CMakeLists.txt @@ -57,8 +57,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp index e82b506c..99d05e7e 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp @@ -73,15 +73,18 @@ class RegulatedPurePursuitController : public ControllerMethodBase // --- Velocity / acceleration limits --- double max_linear_vel_{0.5}; ///< Maximum linear velocity (m/s). - double min_linear_vel_{-0.5}; ///< Minimum linear velocity, used when use_dynamic_window is true (m/s). + /// Minimum linear velocity, used when use_dynamic_window is true (m/s). + double min_linear_vel_{-0.5}; double max_angular_vel_{2.5}; ///< Maximum angular velocity (rad/s). - double min_angular_vel_{-2.5}; ///< Minimum angular velocity, used when use_dynamic_window is true (rad/s). + /// Minimum angular velocity, used when use_dynamic_window is true (rad/s). + double min_angular_vel_{-2.5}; double max_linear_accel_{2.5}; ///< Maximum linear acceleration (m/s^2). double max_linear_decel_{2.5}; ///< Maximum linear deceleration (m/s^2). - double max_angular_accel_{3.2}; ///< Maximum angular acceleration (rad/s^2). - double max_angular_decel_{3.2}; ///< Maximum angular deceleration (rad/s^2). + double max_angular_accel_{3.2}; ///< Maximum angular acceleration (rad/s^2). + double max_angular_decel_{3.2}; ///< Maximum angular deceleration (rad/s^2). bool use_dynamic_window_{false}; ///< Use the Dynamic Window Pure Pursuit (DWPP) extension. - bool allow_reversing_{false}; ///< Allow driving backwards when the carrot is behind the robot. + /// Allow driving backwards when the carrot is behind the robot. + bool allow_reversing_{false}; // --- Rotate to heading --- bool use_rotate_to_heading_{true}; ///< Enable rotate-in-place behaviors. @@ -93,12 +96,16 @@ class RegulatedPurePursuitController : public ControllerMethodBase double regulated_linear_scaling_min_radius_{0.9}; ///< Turning radius that triggers regulation. double regulated_linear_scaling_min_speed_{0.25}; ///< Minimum speed kept under regulation. bool use_fixed_curvature_lookahead_{false}; ///< Use a separate lookahead for curvature. - double curvature_lookahead_dist_{1.0}; ///< Distance of the fixed curvature lookahead. - bool interpolate_curvature_after_goal_{false}; ///< Extrapolate curvature carrot past the goal. + /// Distance of the fixed curvature lookahead. + double curvature_lookahead_dist_{1.0}; + /// Extrapolate curvature carrot past the goal. + bool interpolate_curvature_after_goal_{false}; // --- Obstacle-proximity regulation (adapts the costmap-based term to EasyNav's perception) --- - bool use_obstacle_regulated_linear_velocity_scaling_{false}; ///< Enable obstacle-proximity regulation. - double obstacle_scaling_dist_{0.3}; ///< Distance below which obstacle regulation is triggered (m). + /// Enable obstacle-proximity regulation. + bool use_obstacle_regulated_linear_velocity_scaling_{false}; + /// Distance below which obstacle regulation is triggered (m). + double obstacle_scaling_dist_{0.3}; double obstacle_scaling_gain_{1.0}; ///< Gain (<=1.0) applied when scaling down the velocity. // Area where obstacles regulate the velocity (computeMinObstacleDistance()). double robot_radius_{0.35}; ///< Robot radius used when measuring obstacle distance (m). @@ -108,7 +115,8 @@ class RegulatedPurePursuitController : public ControllerMethodBase // --- Approach to goal --- double min_approach_linear_velocity_{0.05}; ///< Minimum linear velocity while approaching goal. - double approach_velocity_scaling_dist_{1.0}; ///< Remaining-path distance at which to start slowing. + /// Remaining-path distance at which to start slowing. + double approach_velocity_scaling_dist_{1.0}; // --- Goal tolerances (fallback values if "goal_tolerance.*" is not present in NavState) --- double xy_goal_tolerance_{0.25}; ///< Fallback positional tolerance to the goal (m). @@ -119,10 +127,13 @@ class RegulatedPurePursuitController : public ControllerMethodBase rclcpp::Publisher::SharedPtr is_rotating_to_heading_pub_; rclcpp::Time last_update_ts_; ///< Timestamp of the last control update. - double last_linear_vel_{0.0}; ///< Last commanded linear velocity (open-loop feedback). - double last_angular_vel_{0.0}; ///< Last commanded angular velocity (open-loop feedback). - bool is_rotating_to_heading_{false}; ///< Whether the controller is currently rotating in place. - geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. + /// Last commanded linear velocity (open-loop feedback). + double last_linear_vel_{0.0}; + /// Last commanded angular velocity (open-loop feedback). + double last_angular_vel_{0.0}; + /// Whether the controller is currently rotating in place. + bool is_rotating_to_heading_{false}; + geometry_msgs::msg::TwistStamped cmd_vel_; ///< Current velocity command. /// \brief Publishes a zero-velocity command and resets the open-loop velocity feedback. void stop(NavState & nav_state, const std_msgs::msg::Header & header); @@ -156,8 +167,10 @@ class RegulatedPurePursuitController : public ControllerMethodBase double lookahead_dist, bool interpolate_after_end = false); - /// \brief Intersection of the segment [p1, p2] with the circle of radius \p r centered at \p center. - /// \return The intersection point closer to \p p2, or \p p2 itself if there is no real intersection. + /// \brief Intersection of the segment [p1, p2] with the circle of radius \p r centered at + /// \p center. + /// \return The intersection point closer to \p p2, or \p p2 itself if there is no real + /// intersection. static geometry_msgs::msg::Point circleSegmentIntersection( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp index babc2859..40fb306d 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/regulation_functions.hpp @@ -26,7 +26,8 @@ /// Autonomous Robots, 2023 (https://arxiv.org/abs/2305.20026), ported from /// nav2_regulated_pure_pursuit_controller. Since EasyNav controllers have no costmap available, /// the cost-based regulation term is replaced by an equivalent one driven by the distance to the -/// nearest point in the fused point-cloud perception (see \ref easynav::RegulatedPurePursuitController). +/// nearest point in the fused point-cloud perception (see +/// \ref easynav::RegulatedPurePursuitController). #ifndef EASYNAV_REGULATED_PP_CONTROLLER__REGULATION_FUNCTIONS_HPP_ #define EASYNAV_REGULATED_PP_CONTROLLER__REGULATION_FUNCTIONS_HPP_ @@ -41,7 +42,8 @@ namespace easynav namespace heuristics { -/// \brief Curvature of the arc from the robot (at the local origin, facing +x) to a lookahead point. +/// \brief Curvature of the arc from the robot (at the local origin, facing +x) to a lookahead +/// point. /// \param lookahead_x X coordinate of the lookahead point in the robot frame. /// \param lookahead_y Y coordinate of the lookahead point in the robot frame. /// \return Signed curvature (1 / turning radius) of the arc. diff --git a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_legacy_params_file_tests.cpp b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_legacy_params_file_tests.cpp index 11d8aa49..7eadc6ac 100644 --- a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_legacy_params_file_tests.cpp +++ b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_legacy_params_file_tests.cpp @@ -11,6 +11,7 @@ // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief A real parameter file from before robot_limits (easynav_indoor_testcase, diff --git a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_robot_limits_tests.cpp b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_robot_limits_tests.cpp index e3a01ec3..d860c013 100644 --- a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_robot_limits_tests.cpp +++ b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_robot_limits_tests.cpp @@ -11,6 +11,7 @@ // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief The deprecated limit parameters of this controller still work, and "robot_limits.*" diff --git a/controllers/easynav_serest_controller/CMakeLists.txt b/controllers/easynav_serest_controller/CMakeLists.txt index a4669a75..c87834dc 100644 --- a/controllers/easynav_serest_controller/CMakeLists.txt +++ b/controllers/easynav_serest_controller/CMakeLists.txt @@ -53,8 +53,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest 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 c854e88c..4459b339 100644 --- a/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp +++ b/controllers/easynav_serest_controller/include/easynav_serest_controller/SerestController.hpp @@ -85,8 +85,8 @@ class SerestController : public ControllerMethodBase */ struct Vec2 { - double x; ///< X component. - double y; ///< Y component. + double x; ///< X component. + double y; ///< Y component. /// @brief Vector addition. inline Vec2 operator+(const Vec2 & b) const {return {x + b.x, y + b.y};} 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 5cd24d2a..e0356139 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp @@ -241,7 +241,7 @@ SerestController::ref_heading_and_curvature( if (i > 0 && (s - s_i) < b) { Vec2 Tim1 = seg_dir(i - 1); double psi_im1 = atan2dir(Tim1); - double w = 1.0 / (1.0 + std::exp(-( (s - s_i) / b ))); // sigmoid in [s_i, s_i+b] + double w = 1.0 / (1.0 + std::exp(-( (s - s_i) / b ))); // sigmoid in [s_i, s_i+b] // Circular interpolation of headings double sx = (1.0 - w) * std::cos(psi_im1) + w * std::cos(psi_i); double sy = (1.0 - w) * std::sin(psi_im1) + w * std::sin(psi_i); @@ -249,7 +249,7 @@ SerestController::ref_heading_and_curvature( // kappa surrogate ~ dpsi/ds ≈ (Δψ/b) σ'(·) double dpsi = std::atan2(std::sin(psi_i - psi_im1), std::cos(psi_i - psi_im1)); - double sigma_prime = (w * (1 - w)) / b; // derivada de logística escalada + double sigma_prime = (w * (1 - w)) / b; // derivada de logística escalada rk.kappa_hat = std::clamp(dpsi * sigma_prime, -kappa_max_, kappa_max_); } @@ -257,7 +257,7 @@ SerestController::ref_heading_and_curvature( if (i + 1 < pd.pts.size() - 1 && (s_ip1 - s) < b) { Vec2 Tip1 = seg_dir(i + 1); double psi_ip1 = atan2dir(Tip1); - double w = 1.0 / (1.0 + std::exp(-( (s_ip1 - s) / b ))); // symmetric + double w = 1.0 / (1.0 + std::exp(-( (s_ip1 - s) / b ))); // symmetric // Blend between i and i + 1 double sx = (1.0 - w) * std::cos(psi_i) + w * std::cos(psi_ip1); double sy = (1.0 - w) * std::sin(psi_i) + w * std::sin(psi_ip1); @@ -281,10 +281,10 @@ SerestController::frenet_errors( { // Normal vector to the right of the tangent Vec2 T = v2(std::cos(psi_ref), std::sin(psi_ref)); - Vec2 N = v2(-T.y, T.x); // 90º to the left (convention) + Vec2 N = v2(-T.y, T.x); // 90º to the left (convention) Vec2 err = robot_xy - prj.closest; - e_y = dot(err, N); // signed lateral distance + e_y = dot(err, N); // signed lateral distance e_theta = std::atan2(std::sin(robot_yaw - psi_ref), std::cos(robot_yaw - psi_ref)); } @@ -505,7 +505,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 * M_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, @@ -704,11 +704,10 @@ SerestController::update_rt(NavState & nav_state) } // Hysteresis on the TiP state + const bool tip_exit = std::fabs(e_theta) < thr_exit || prj.s_star > (near_start_s - 0.10); if (!tip_active_ && tip_request && std::fabs(e_theta) > thr_enter) { tip_active_ = true; - } else if (tip_active_ && - (std::fabs(e_theta) < thr_exit || prj.s_star > (near_start_s - 0.10))) - { + } else if (tip_active_ && tip_exit) { tip_active_ = false; } @@ -854,5 +853,4 @@ SerestController::update_rt(NavState & nav_state) } // namespace easynav -#include PLUGINLIB_EXPORT_CLASS(easynav::SerestController, easynav::ControllerMethodBase) diff --git a/controllers/easynav_simple_controller/CMakeLists.txt b/controllers/easynav_simple_controller/CMakeLists.txt index 71b22943..61853f80 100644 --- a/controllers/easynav_simple_controller/CMakeLists.txt +++ b/controllers/easynav_simple_controller/CMakeLists.txt @@ -52,8 +52,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/controllers/easynav_simple_controller/include/easynav_simple_controller/PIDController.hpp b/controllers/easynav_simple_controller/include/easynav_simple_controller/PIDController.hpp index 7ab46f87..bbe631f1 100644 --- a/controllers/easynav_simple_controller/include/easynav_simple_controller/PIDController.hpp +++ b/controllers/easynav_simple_controller/include/easynav_simple_controller/PIDController.hpp @@ -76,7 +76,7 @@ class PIDController double prev_error_; ///< Previous error value (for derivative term). double int_error_; ///< Accumulated integral error. - double integral_limit_ {0.0}; ///< If >0, clamps the integral term to ±integral_limit_. + double integral_limit_ {0.0}; ///< If >0, clamps the integral term to ±integral_limit_. }; } // namespace easynav diff --git a/controllers/easynav_simple_controller/src/easynav_simple_controller/PIDController.cpp b/controllers/easynav_simple_controller/src/easynav_simple_controller/PIDController.cpp index 0526ee39..a32d81f3 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/PIDController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/PIDController.cpp @@ -28,9 +28,9 @@ PIDController::PIDController(double min_ref, double max_ref, double min_output, prev_error_ = int_error_ = 0.0; KP_ = 0.7; - KI_ = 0.0; // start with no integral to avoid windup + KI_ = 0.0; // start with no integral to avoid windup KD_ = 0.0; - integral_limit_ = max_output_ * 2.0; // default integral clamp + integral_limit_ = max_output_ * 2.0; // default integral clamp } void diff --git a/controllers/easynav_simple_controller/tests/simple_controller_legacy_params_file_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_legacy_params_file_tests.cpp index 0093b39a..0df901b0 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_legacy_params_file_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_legacy_params_file_tests.cpp @@ -11,6 +11,7 @@ // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief A real parameter file from before robot_limits (easynav_indoor_testcase, diff --git a/controllers/easynav_simple_controller/tests/simple_controller_robot_limits_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_robot_limits_tests.cpp index a074e4b7..0d53eec4 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_robot_limits_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_robot_limits_tests.cpp @@ -11,6 +11,7 @@ // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and +// limitations under the License. /// \file /// \brief The deprecated limit parameters of this controller still work, and "robot_limits.*" diff --git a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp index a80d9a74..c35b20df 100644 --- a/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp +++ b/controllers/easynav_simple_controller/tests/simple_controller_tests.cpp @@ -13,19 +13,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include +#include +#include "gtest/gtest.h" #include "easynav_simple_common/SimpleMap.hpp" #include "easynav_simple_controller/SimpleController.hpp" - #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" - #include "std_srvs/srv/trigger.hpp" -#include -#include - /// \brief Fixture for SimpleController tests (minimal) class AMCLLocalizerTest : public ::testing::Test { diff --git a/controllers/easynav_vff_controller/CMakeLists.txt b/controllers/easynav_vff_controller/CMakeLists.txt index e3f64c33..815b609d 100644 --- a/controllers/easynav_vff_controller/CMakeLists.txt +++ b/controllers/easynav_vff_controller/CMakeLists.txt @@ -62,8 +62,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest 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 168b0c72..9712f3ea 100644 --- a/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp +++ b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp @@ -16,14 +16,15 @@ /// \file /// \brief Declaration of the VffController method. -#ifndef EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_ -#define EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_ +#ifndef EASYNAV_VFF_CONTROLLER__VFFCONTROLLER_HPP_ +#define EASYNAV_VFF_CONTROLLER__VFFCONTROLLER_HPP_ + +#include +#include #include "pcl/point_cloud.h" #include "pcl/point_types.h" - #include "easynav_core/ControllerMethodBase.hpp" - #include "geometry_msgs/msg/twist_stamped.hpp" #include "visualization_msgs/msg/marker.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -80,7 +81,7 @@ class VffController : public easynav::ControllerMethodBase * * @throws std::runtime_error on initialization error. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Updates the localization estimate based on the current navigation state. @@ -90,7 +91,7 @@ class VffController : public easynav::ControllerMethodBase * * @param nav_state The current navigation state of the system. */ - virtual void update_rt(NavState & nav_state) override; + void update_rt(NavState & nav_state) override; private: /** @@ -192,4 +193,4 @@ class VffController : public easynav::ControllerMethodBase } // namespace easynav -#endif // EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_ +#endif // EASYNAV_VFF_CONTROLLER__VFFCONTROLLER_HPP_ 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 d9dfc9c8..fa0592d7 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -145,9 +145,9 @@ VffController::make_marker( double VffController::normalize_angle(double angle) { - angle = fmod(angle + M_PI, 2.0 * M_PI); // range [0, 2π) + angle = fmod(angle + M_PI, 2.0 * M_PI); // range [0, 2π) if (angle < 0) { - angle += 2.0 * M_PI; // ensure positive + angle += 2.0 * M_PI; // ensure positive } return angle - M_PI; } @@ -242,7 +242,6 @@ void VffController::update_rt(NavState & nav_state) // If a goal is set if (!all_goals.goals.empty()) { - goal_.x = all_goals.goals[0].pose.position.x; goal_.y = all_goals.goals[0].pose.position.y; diff --git a/localizers/easynav_costmap_localizer/CMakeLists.txt b/localizers/easynav_costmap_localizer/CMakeLists.txt index 3ea812ca..c44afdb9 100644 --- a/localizers/easynav_costmap_localizer/CMakeLists.txt +++ b/localizers/easynav_costmap_localizer/CMakeLists.txt @@ -92,8 +92,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest 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 bf6dec98..75ba761b 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AMCLLocalizer.hpp @@ -21,15 +21,14 @@ #include #include -#include +#include +#include "Eigen/Geometry" #include "geometry_msgs/msg/pose_array.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" - #include "tf2/LinearMath/Transform.hpp" #include "tf2_ros/transform_broadcaster.hpp" - #include "easynav_core/LocalizerMethodBase.hpp" namespace easynav @@ -44,7 +43,8 @@ struct Particle double weight; ///< Normalized importance weight of the particle. }; -/// \brief A localization method implementing a simplified AMCL (Adaptive Monte Carlo Localization) approach. +/// \brief A localization method implementing a simplified AMCL (Adaptive Monte Carlo Localization) +/// approach. class AMCLLocalizer : public LocalizerMethodBase { public: @@ -65,7 +65,7 @@ class AMCLLocalizer : public LocalizerMethodBase * * @throws std::runtime_error on initialization error. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Real-time update of the localization state. @@ -172,7 +172,8 @@ class AMCLLocalizer : public LocalizerMethodBase */ void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); - /// @brief Starts from the last known pose after a reconfiguration ("initial_pose.use_last_known"). + /// @brief Starts from the last known pose after a reconfiguration + /// ("initial_pose.use_last_known"). void on_last_known_pose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) override; /// @brief Whether to start from the last known pose after a reconfiguration. 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 f8c3a51e..64b9522d 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -439,7 +439,7 @@ AMCLLocalizer::init_pose_callback( double var_y = cov[7]; // C(1,1) // Yaw variance - double var_yaw = cov[35]; // C(5,5) + double var_yaw = cov[35]; // C(5,5) // Ensure non-negative variances var_x = std::max(var_x, 0.0); @@ -569,8 +569,6 @@ AMCLLocalizer::update_odom_from_tf() last_input_time_ = tf_msg.header.stamp; initialized_odom_ = true; - - } void diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt index a684cee8..b6bf1b8e 100644 --- a/localizers/easynav_fusion_localizer/CMakeLists.txt +++ b/localizers/easynav_fusion_localizer/CMakeLists.txt @@ -116,9 +116,17 @@ install(DIRECTORY maps/ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Files derived from ROS / Nav2 keep their original copyright and license + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) ament_lint_auto_find_test_dependencies() + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if(NOT "$ENV{ROS_DISTRO}" STREQUAL "humble") + find_package(ament_cmake_copyright REQUIRED) + ament_copyright(EXCLUDE + include/easynav_fusion_localizer/ukf_wrapper.hpp + src/easynav_fusion_localizer/ukf_wrapper.cpp + ) + endif() find_package(ament_cmake_gtest REQUIRED) add_subdirectory(tests) 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 7ba99cd7..39c34410 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp @@ -1,13 +1,27 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #pragma once #include - #include #include +#include #include "easynav_core/LocalizerMethodBase.hpp" #include "easynav_fusion_localizer/ukf_wrapper.hpp" - #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" @@ -36,7 +50,8 @@ class FusionLocalizer : public easynav::LocalizerMethodBase /// Callback for /initialpose. void init_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - /// @brief Starts from the last known pose after a reconfiguration ("initial_pose.use_last_known"). + /// @brief Starts from the last known pose after a reconfiguration + /// ("initial_pose.use_last_known"). void on_last_known_pose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) override; /// @brief Whether to start from the last known pose after a reconfiguration. @@ -103,7 +118,6 @@ class FusionLocalizer : public easynav::LocalizerMethodBase std::string navsatfix_topic_; bool first_pose_received_{false}; - }; } // namespace easynav diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp index d85e945c..e675f11f 100644 --- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp +++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp @@ -868,9 +868,8 @@ class UkfWrapper std::string tf_prefix_; std::vector gps_callbackData_arr_; bool local_filter_; - }; } // namespace robot_localization -#endif // ROBOT_LOCALIZATION__ROS_FILTER_HPP_ +#endif // EASYNAV_FUSION_LOCALIZER__UKF_WRAPPER_HPP_ diff --git a/localizers/easynav_fusion_localizer/package.xml b/localizers/easynav_fusion_localizer/package.xml index 52d42b47..48ddb78e 100644 --- a/localizers/easynav_fusion_localizer/package.xml +++ b/localizers/easynav_fusion_localizer/package.xml @@ -33,6 +33,7 @@ yaets ament_cmake_gtest + ament_cmake_copyright ament_lint_auto ament_lint_common robot_localization 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 f4212ae2..6613943e 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -1,3 +1,18 @@ +// Copyright 2025 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + #include "easynav_common/Parameters.hpp" #include "easynav_fusion_localizer/FusionLocalizer.hpp" @@ -21,9 +36,7 @@ namespace easynav void FusionLocalizer::on_initialize() { - try { - auto node = get_node(); // Subscribe to initial pose @@ -103,13 +116,13 @@ void FusionLocalizer::on_initialize() if (has_global_filter_) { easynav::declare_parameter_if_absent( *localizer_node, plugin_name + ".latitude_origin", - double(0.0)); + 0.0); easynav::declare_parameter_if_absent( *localizer_node, plugin_name + ".longitude_origin", - double(0.0)); + 0.0); easynav::declare_parameter_if_absent( *localizer_node, plugin_name + ".altitude_origin", - double(0.0)); + 0.0); easynav::declare_parameter_if_absent( *localizer_node, plugin_name + ".navsatfix_topic", std::string("gps/filtered")); @@ -120,7 +133,6 @@ void FusionLocalizer::on_initialize() navsat_pub_ = localizer_node->create_publisher( navsatfix_topic_, rclcpp::QoS(10)); } - } catch (const std::exception & e) { RCLCPP_FATAL( get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s", @@ -252,7 +264,6 @@ void FusionLocalizer::update_rt(NavState & nav_state) void FusionLocalizer::update([[maybe_unused]] NavState & nav_state) { - } geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose( @@ -289,7 +300,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose pose_msg.pose.covariance.fill(0.0); - double default_var = 1.0; // 1 meter variance standard + double default_var = 1.0; // 1 meter variance standard if (navsat_msg.position_covariance_type == sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN) diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp index 7aa4a39b..0b80da65 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -29,9 +29,6 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#include "easynav_common/Parameters.hpp" -#include "easynav_fusion_localizer/ukf_wrapper.hpp" - #include #include #include @@ -45,6 +42,8 @@ #include #include +#include "easynav_common/Parameters.hpp" +#include "easynav_fusion_localizer/ukf_wrapper.hpp" #include "angles/angles.h" #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_updater/diagnostic_status_wrapper.hpp" @@ -74,9 +73,8 @@ #include "tf2/LinearMath/Transform.hpp" #include "tf2/LinearMath/Vector3.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include -#include - +#include "tf2_ros/buffer.hpp" +#include "tf2_ros/transform_broadcaster.hpp" #include "easynav_common/RTTFBuffer.hpp" namespace robot_localization @@ -2150,7 +2148,7 @@ void UkfWrapper::loadParams() RF_DEBUG("Initial estimate covariance is:\n" << initial_estimate_error_covariance_ << "\n"); filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_); } -} +} // NOLINT(readability/fn_size) void UkfWrapper::odometryCallback( const nav_msgs::msg::Odometry::SharedPtr msg, @@ -2384,8 +2382,9 @@ void UkfWrapper::initialize() // const std::chrono::duration timespan{1.0 / frequency_}; // timer_ = rclcpp::GenericTimer::make_shared( // parent_node_->get_clock(), std::chrono::duration_cast(timespan), - // std::bind(&UkfWrapper::periodicUpdate, this), parent_node_->get_node_base_interface()->get_context()); - // parent_node_->get_node_timers_interface()->add_timer(timer_, nullptr); + // std::bind(&UkfWrapper::periodicUpdate, this), + // parent_node_->get_node_base_interface()->get_context()); + // parent_node_->get_node_timers_interface()->add_timer(timer_, nullptr); } void UkfWrapper::periodicUpdate() diff --git a/localizers/easynav_mhamcl_localizer/CMakeLists.txt b/localizers/easynav_mhamcl_localizer/CMakeLists.txt index d414aec9..bea30731 100644 --- a/localizers/easynav_mhamcl_localizer/CMakeLists.txt +++ b/localizers/easynav_mhamcl_localizer/CMakeLists.txt @@ -62,8 +62,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/localizers/easynav_mhamcl_localizer/include/easynav_mhamcl_localizer/MHAMCLLocalizer.hpp b/localizers/easynav_mhamcl_localizer/include/easynav_mhamcl_localizer/MHAMCLLocalizer.hpp index 7a2d7f10..feb41df7 100644 --- a/localizers/easynav_mhamcl_localizer/include/easynav_mhamcl_localizer/MHAMCLLocalizer.hpp +++ b/localizers/easynav_mhamcl_localizer/include/easynav_mhamcl_localizer/MHAMCLLocalizer.hpp @@ -24,17 +24,15 @@ #include #include #include +#include #include "geometry_msgs/msg/pose_array.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "visualization_msgs/msg/marker_array.hpp" - #include "tf2/LinearMath/Transform.hpp" #include "tf2_ros/transform_broadcaster.hpp" - #include "easynav_core/LocalizerMethodBase.hpp" - #include "easynav_mhamcl_localizer/MapMatcher.hpp" #include "easynav_mhamcl_localizer/ParticlesDistribution.hpp" @@ -133,7 +131,8 @@ class MHAMCLLocalizer : public LocalizerMethodBase /// \brief Callback for the initial pose. Discards all the hypotheses and starts a new one. void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); - /// @brief Starts from the last known pose after a reconfiguration ("initial_pose.use_last_known"). + /// @brief Starts from the last known pose after a reconfiguration + /// ("initial_pose.use_last_known"). void on_last_known_pose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) override; /// @brief Whether to start from the last known pose after a reconfiguration. diff --git a/localizers/easynav_mhamcl_localizer/include/easynav_mhamcl_localizer/MapMatcher.hpp b/localizers/easynav_mhamcl_localizer/include/easynav_mhamcl_localizer/MapMatcher.hpp index 9b2483a2..699d0e38 100644 --- a/localizers/easynav_mhamcl_localizer/include/easynav_mhamcl_localizer/MapMatcher.hpp +++ b/localizers/easynav_mhamcl_localizer/include/easynav_mhamcl_localizer/MapMatcher.hpp @@ -60,7 +60,8 @@ class MapMatcher * @param num_levels Number of levels of the pyramid, including the original map. * @param angle_step Angular resolution of the search (rad). */ - MapMatcher(const easynav::Costmap2D & map, int num_levels = 4, double angle_step = M_PI / 8.0); + explicit MapMatcher( + const easynav::Costmap2D & map, int num_levels = 4, double angle_step = M_PI / 8.0); /** * @brief Get the candidate poses. diff --git a/localizers/easynav_mhamcl_localizer/src/easynav_mhamcl_localizer/MapMatcher.cpp b/localizers/easynav_mhamcl_localizer/src/easynav_mhamcl_localizer/MapMatcher.cpp index d9666971..82235d80 100644 --- a/localizers/easynav_mhamcl_localizer/src/easynav_mhamcl_localizer/MapMatcher.cpp +++ b/localizers/easynav_mhamcl_localizer/src/easynav_mhamcl_localizer/MapMatcher.cpp @@ -57,13 +57,13 @@ MapMatcher::half_scale(const easynav::Costmap2D & in) auto has = [&costs](unsigned char v) {return std::find(costs, costs + 4, v) != costs + 4;}; + const bool unknown = std::all_of( + costs, costs + 4, [](unsigned char c) {return c == easynav::NO_INFORMATION;}); if (has(easynav::LETHAL_OBSTACLE)) { out->setCost(i, j, easynav::LETHAL_OBSTACLE); } else if (has(easynav::FREE_SPACE)) { out->setCost(i, j, easynav::FREE_SPACE); - } else if (std::all_of( - costs, costs + 4, [](unsigned char c) {return c == easynav::NO_INFORMATION;})) - { + } else if (unknown) { out->setCost(i, j, easynav::NO_INFORMATION); } else { out->setCost(i, j, costs[0]); diff --git a/localizers/easynav_navmap_localizer/CMakeLists.txt b/localizers/easynav_navmap_localizer/CMakeLists.txt index 123df997..fd0d9650 100644 --- a/localizers/easynav_navmap_localizer/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/CMakeLists.txt @@ -64,8 +64,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest 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 298d80eb..55ef72ac 100644 --- a/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp @@ -21,18 +21,17 @@ #include #include -#include -#include +#include +#include +#include "Eigen/Geometry" +#include "bonxai/probabilistic_map.hpp" #include "geometry_msgs/msg/pose_array.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" - #include "tf2/LinearMath/Transform.hpp" #include "tf2_ros/transform_broadcaster.hpp" - #include "navmap_core/NavMap.hpp" - #include "easynav_core/LocalizerMethodBase.hpp" namespace easynav @@ -52,7 +51,8 @@ struct Particle std::size_t last_surface = 0; }; -/// \brief A localization method implementing a simplified AMCL (Adaptive Monte Carlo Localization) approach. +/// \brief A localization method implementing a simplified AMCL (Adaptive Monte Carlo Localization) +/// approach. class AMCLLocalizer : public LocalizerMethodBase { public: @@ -73,7 +73,7 @@ class AMCLLocalizer : public LocalizerMethodBase * * @throws std::runtime_error if initialization fails. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Real-time update of the localization state. @@ -180,7 +180,8 @@ class AMCLLocalizer : public LocalizerMethodBase */ void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); - /// @brief Starts from the last known pose after a reconfiguration ("initial_pose.use_last_known"). + /// @brief Starts from the last known pose after a reconfiguration + /// ("initial_pose.use_last_known"). void on_last_known_pose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) override; /// @brief Whether to start from the last known pose after a reconfiguration. @@ -221,7 +222,7 @@ class AMCLLocalizer : public LocalizerMethodBase double inflation_stddev_ {1.5}; // meters double inflation_prob_min_ {0.01}; // [0..1] min prob kept in inflated map - std::size_t correct_max_points_ {1500}; // hard cap of points per sensor cloud + std::size_t correct_max_points_ {1500}; // hard cap of points per sensor cloud double weights_tau_ {0.7}; // <1 sharpens, >1 flattens // Fraction of particles (0,1] used as "top" set for stats and reseed double top_keep_fraction_{0.2}; 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 0d9a6761..db27d392 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -24,22 +24,17 @@ #include #include -#include -#include - +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" #include "bonxai/bonxai.hpp" #include "bonxai/probabilistic_map.hpp" - #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/LinearMath/Vector3.hpp" - #include "easynav_common/Parameters.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "easynav_sensors/types/PointPerception.hpp" #include "easynav_sensors/types/IMUPerception.hpp" - #include "navmap_core/NavMap.hpp" - #include "easynav_navmap_localizer/AMCLLocalizer.hpp" #include "easynav_localizer/LocalizerNode.hpp" @@ -171,7 +166,7 @@ using ProgressCallback = std::function; /// Expect the fixed comma value returned by logods() [[nodiscard]] static constexpr float prob(int32_t logods_fixed) { - float logods = float(logods_fixed) * 1e-6; + float logods = static_cast(logods_fixed) * 1e-6; return 1.0 - 1.0 / (1.0 + std::exp(logods)); } @@ -180,7 +175,8 @@ inflate_map( const std::shared_ptr & src, double sigma, double p_min, ProgressCallback progress = nullptr) { - using namespace Bonxai; + using Bonxai::CoordT; + using Bonxai::ProbabilisticMap; if (!src || sigma <= 0.0) {return nullptr;} const double res = src->grid().voxelSize(); @@ -217,7 +213,8 @@ inflate_map( struct KeyHash { size_t operator()(const CoordT & c) const noexcept { - return (size_t)c.x * 73856093u ^ (size_t)c.y * 19349663u ^ (size_t)c.z * 83492791u; + return static_cast(c.x) * 73856093u ^ static_cast(c.y) * 19349663u ^ + static_cast(c.z) * 83492791u; } }; struct KeyEq { bool operator()(const CoordT & a, const CoordT & b) const noexcept @@ -303,17 +300,17 @@ inline bool ray_occluded_dda( } } if (dx >= dy && dx >= dz) { - x += sx; ey += dy; ez += dz; if (ey >= dx) {y += sy; ey -= dx;} if (ez >= dx) { - z += sz; ez -= dx; - } + x += sx; ey += dy; ez += dz; + if (ey >= dx) {y += sy; ey -= dx;} + if (ez >= dx) {z += sz; ez -= dx;} } else if (dy >= dx && dy >= dz) { - y += sy; ex += dx; ez += dz; if (ex >= dy) {x += sx; ex -= dy;} if (ez >= dy) { - z += sz; ez -= dy; - } + y += sy; ex += dx; ez += dz; + if (ex >= dy) {x += sx; ex -= dy;} + if (ez >= dy) {z += sz; ez -= dy;} } else { - z += sz; ex += dx; ey += dy; if (ex >= dz) {x += sx; ex -= dz;} if (ey >= dz) { - y += sy; ey -= dz; - } + z += sz; ex += dx; ey += dy; + if (ex >= dz) {x += sx; ex -= dz;} + if (ey >= dz) {y += sy; ey -= dz;} } } return false; @@ -331,7 +328,8 @@ struct CoordHash { size_t operator()(const Bonxai::CoordT & c) const noexcept { - return (size_t)c.x * 73856093u ^ (size_t)c.y * 19349663u ^ (size_t)c.z * 83492791u; + return static_cast(c.x) * 73856093u ^ static_cast(c.y) * 19349663u ^ + static_cast(c.z) * 83492791u; } }; struct CoordEq @@ -342,7 +340,7 @@ struct CoordEq } }; -} // namespace +} // namespace // ---------- AMCLLocalizer ---------- AMCLLocalizer::AMCLLocalizer() @@ -809,7 +807,7 @@ void AMCLLocalizer::predict(NavState & nav_state) struct SensorBundle { std::string frame_id; - pcl::PointCloud cloud; // in sensor frame + pcl::PointCloud cloud; // in sensor frame }; static inline std::string get_frame_id_from(const PointPerception & pp) @@ -953,7 +951,6 @@ void AMCLLocalizer::correct(NavState & nav_state) b.frame_id); // const tf2::Transform & t = T_bf_sensor_cache[b.frame_id]; - } catch (const tf2::TransformException & ex) { RCLCPP_WARN( get_node()->get_logger(), "TF bf->%s failed: %s", b.frame_id.c_str(), diff --git a/localizers/easynav_simple_localizer/CMakeLists.txt b/localizers/easynav_simple_localizer/CMakeLists.txt index d28759f0..d2d83fec 100644 --- a/localizers/easynav_simple_localizer/CMakeLists.txt +++ b/localizers/easynav_simple_localizer/CMakeLists.txt @@ -58,8 +58,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest 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 1b5eb2ff..9a9ddb51 100644 --- a/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp +++ b/localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp @@ -21,15 +21,14 @@ #include #include -#include +#include +#include "Eigen/Geometry" #include "geometry_msgs/msg/pose_array.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" - #include "tf2/LinearMath/Transform.hpp" #include "tf2_ros/transform_broadcaster.hpp" - #include "easynav_core/LocalizerMethodBase.hpp" namespace easynav @@ -44,7 +43,8 @@ struct Particle double weight; ///< Normalized importance weight of the particle. }; -/// \brief A localization method implementing a simplified AMCL (Adaptive Monte Carlo Localization) approach. +/// \brief A localization method implementing a simplified AMCL (Adaptive Monte Carlo Localization) +/// approach. class AMCLLocalizer : public LocalizerMethodBase { public: @@ -65,7 +65,7 @@ class AMCLLocalizer : public LocalizerMethodBase * * @throws std::runtime_error if initialization fails. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Real-time update of the localization state. @@ -172,7 +172,8 @@ class AMCLLocalizer : public LocalizerMethodBase */ void init_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); - /// @brief Starts from the last known pose after a reconfiguration ("initial_pose.use_last_known"). + /// @brief Starts from the last known pose after a reconfiguration + /// ("initial_pose.use_last_known"). void on_last_known_pose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) override; /// @brief Whether to start from the last known pose after a reconfiguration. @@ -226,4 +227,4 @@ class AMCLLocalizer : public LocalizerMethodBase } // namespace easynav -#endif // EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ +#endif // EASYNAV_SIMPLE_LOCALIZER__AMCLLOCALIZER_HPP_ diff --git a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt index 07fadda6..8a7679ac 100644 --- a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt @@ -96,12 +96,24 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - # Vendored third-party library: excluded from uncrustify instead of - # reformatting upstream code (same rationale as copyright/cpplint above). + # The vendored Bonxai library keeps its upstream code, copyright and license: + # the linters skip it instead of modifying it set(ament_cmake_uncrustify_ADDITIONAL_EXCLUDE include/bonxai/*) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright ament_cmake_cpplint) ament_lint_auto_find_test_dependencies() + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if(NOT "$ENV{ROS_DISTRO}" STREQUAL "humble") + find_package(ament_cmake_copyright REQUIRED) + # map_io.cpp and FindLZ4.cmake also keep their original copyright and license + ament_copyright(EXCLUDE + include/bonxai/* + src/bonxai/* + src/easynav_bonxai_maps_manager/map_io.cpp + cmake_modules/FindLZ4.cmake + ) + endif() + find_package(ament_cmake_cpplint REQUIRED) + ament_cpplint(EXCLUDE include/bonxai/* src/bonxai/*) find_package(ament_cmake_gtest REQUIRED) add_subdirectory(tests) 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 4130fccb..de9a50c9 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 @@ -16,15 +16,16 @@ /// \file /// \brief Declaration of the BonxaiMapsManager method. -#ifndef EASYNAV_BONXAI_MAPS_MANAGER__BONXAI_MAPS_MANAGER_HPP_ -#define EASYNAV_BONXAI_MAPS_MANAGER__BONXAI_MAPS_MANAGER_HPP_ +#ifndef EASYNAV_BONXAI_MAPS_MANAGER__BONXAIMAPSMANAGER_HPP_ +#define EASYNAV_BONXAI_MAPS_MANAGER__BONXAIMAPSMANAGER_HPP_ -#include "bonxai/probabilistic_map.hpp" +#include +#include +#include "bonxai/probabilistic_map.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" - #include "easynav_core/MapsManagerBase.hpp" namespace easynav_bonxai @@ -59,7 +60,7 @@ class BonxaiMapsManager : public easynav::MapsManagerBase * * @throws std::runtime_error if initialization fails. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. @@ -69,7 +70,7 @@ class BonxaiMapsManager : public easynav::MapsManagerBase * * @param nav_state Current state of the navigation system. */ - virtual void update(::easynav::NavState & nav_state) override; + void update(::easynav::NavState & nav_state) override; protected: /** @@ -117,4 +118,4 @@ class BonxaiMapsManager : public easynav::MapsManagerBase }; } // namespace easynav_bonxai -#endif // EASYNAV_BONXAI_MAPS_MANAGER__BONXAI_MAPS_MANAGER_HPP_ +#endif // EASYNAV_BONXAI_MAPS_MANAGER__BONXAIMAPSMANAGER_HPP_ diff --git a/maps_managers/easynav_bonxai_maps_manager/package.xml b/maps_managers/easynav_bonxai_maps_manager/package.xml index 76f9fad0..c25cdfc6 100644 --- a/maps_managers/easynav_bonxai_maps_manager/package.xml +++ b/maps_managers/easynav_bonxai_maps_manager/package.xml @@ -28,6 +28,8 @@ yaml_cpp_vendor ament_cmake_gtest + ament_cmake_copyright + ament_cmake_cpplint ament_lint_auto ament_lint_common rclcpp_lifecycle 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 af6a1551..d5f121a4 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 @@ -145,7 +145,6 @@ BonxaiMapsManager::on_initialize() "/incoming_occ_map", rclcpp::QoS(1).transient_local().reliable(), [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { - update_from_occ(*msg); publish_map(); }); @@ -309,7 +308,6 @@ BonxaiMapsManager::publish_map() bonxai_msg_.header.frame_id = tf_info.map_frame; bonxai_msg_.header.stamp = this->get_node()->now(); bonxai_pub_->publish(bonxai_msg_); - } } // namespace easynav_bonxai diff --git a/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt index 27d2ef94..e55a3e36 100644 --- a/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_costmap_maps_manager/CMakeLists.txt @@ -70,9 +70,18 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Files derived from ROS / Nav2 keep their original copyright and license + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) ament_lint_auto_find_test_dependencies() + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if(NOT "$ENV{ROS_DISTRO}" STREQUAL "humble") + find_package(ament_cmake_copyright REQUIRED) + ament_copyright(EXCLUDE + include/easynav_costmap_maps_manager/filters/InflationFilter.hpp + src/easynav_costmap_maps_manager/filters/InflationFilter.cpp + src/easynav_costmap_maps_manager/map_io.cpp + ) + endif() find_package(ament_cmake_gtest REQUIRED) add_subdirectory(tests) 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 cfef0727..cbb0aa96 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 @@ -16,17 +16,17 @@ /// \file /// \brief Declaration of the CostmapMapsManager method. -#ifndef EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ -#define EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ +#ifndef EASYNAV_COSTMAP_MAPS_MANAGER__COSTMAPMAPSMANAGER_HPP_ +#define EASYNAV_COSTMAP_MAPS_MANAGER__COSTMAPMAPSMANAGER_HPP_ #include +#include +#include #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_srvs/srv/trigger.hpp" - #include "easynav_core/MapsManagerBase.hpp" #include "easynav_costmap_common/costmap_2d.hpp" - #include "easynav_costmap_maps_manager/filters/CostmapFilter.hpp" #include "pluginlib/class_loader.hpp" @@ -61,7 +61,7 @@ class CostmapMapsManager : public easynav::MapsManagerBase * * @throws std::runtime_error if initialization fails. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. @@ -71,7 +71,7 @@ class CostmapMapsManager : public easynav::MapsManagerBase * * @param nav_state Current state of the navigation system. */ - virtual void update(NavState & nav_state) override; + void update(NavState & nav_state) override; /** * @brief Replaces the current static map. @@ -134,4 +134,4 @@ class CostmapMapsManager : public easynav::MapsManagerBase } // namespace easynav -#endif // EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ +#endif // EASYNAV_COSTMAP_MAPS_MANAGER__COSTMAPMAPSMANAGER_HPP_ 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 f0e76e20..65d0bcb1 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 @@ -14,13 +14,13 @@ // limitations under the License. -#ifndef EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_HPP_ -#define EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_HPP_ +#ifndef EASYNAV_COSTMAP_MAPS_MANAGER__FILTERS__COSTMAPFILTER_HPP_ +#define EASYNAV_COSTMAP_MAPS_MANAGER__FILTERS__COSTMAPFILTER_HPP_ #include +#include #include "easynav_common/types/NavState.hpp" - #include "rclcpp_lifecycle/lifecycle_node.hpp" namespace easynav @@ -56,4 +56,4 @@ class CostmapFilter }; } // namespace easynav -#endif // EASYNAV_PLANNER__FILTERS__COSTMAPFILTER_HPP_ +#endif // EASYNAV_COSTMAP_MAPS_MANAGER__FILTERS__COSTMAPFILTER_HPP_ 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 9a6f9b32..649bb773 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 @@ -39,8 +39,8 @@ *********************************************************************/ -#ifndef EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ -#define EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ +#ifndef EASYNAV_COSTMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ +#define EASYNAV_COSTMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ #include #include @@ -250,4 +250,4 @@ class InflationFilter : public CostmapFilter } // namespace easynav -#endif // EASYNAV_PLANNER__FILTERS__IINFLATIONFILTER_HPP_ +#endif // EASYNAV_COSTMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ 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 6adced67..b5c107e1 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 @@ -14,8 +14,8 @@ // limitations under the License. -#ifndef EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ -#define EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ +#ifndef EASYNAV_COSTMAP_MAPS_MANAGER__FILTERS__OBSTACLEFILTER_HPP_ +#define EASYNAV_COSTMAP_MAPS_MANAGER__FILTERS__OBSTACLEFILTER_HPP_ #include @@ -41,4 +41,4 @@ class ObstacleFilter : public CostmapFilter } // namespace easynav -#endif // EASYNAV_PLANNER__FILTERS__OBSTACLEFILTER_HPP_ +#endif // EASYNAV_COSTMAP_MAPS_MANAGER__FILTERS__OBSTACLEFILTER_HPP_ diff --git a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/map_io.hpp b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/map_io.hpp index a41bc423..f460c054 100644 --- a/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/map_io.hpp +++ b/maps_managers/easynav_costmap_maps_manager/include/easynav_costmap_maps_manager/map_io.hpp @@ -14,8 +14,8 @@ /* OccupancyGrid map input-output library */ -#ifndef EASYNAV_COSTMAP_COMMON__MAP_IO_HPP_ -#define EASYNAV_COSTMAP_COMMON__MAP_IO_HPP_ +#ifndef EASYNAV_COSTMAP_MAPS_MANAGER__MAP_IO_HPP_ +#define EASYNAV_COSTMAP_MAPS_MANAGER__MAP_IO_HPP_ #include #include @@ -158,4 +158,4 @@ std::string expand_user_home_dir_if_needed( } // namespace easynav -#endif // EASYNAV_COSTMAP_COMMON__MAP_IO_HPP_ +#endif // EASYNAV_COSTMAP_MAPS_MANAGER__MAP_IO_HPP_ diff --git a/maps_managers/easynav_costmap_maps_manager/package.xml b/maps_managers/easynav_costmap_maps_manager/package.xml index 1a270c49..0e324409 100644 --- a/maps_managers/easynav_costmap_maps_manager/package.xml +++ b/maps_managers/easynav_costmap_maps_manager/package.xml @@ -27,6 +27,7 @@ yaml_cpp_vendor ament_cmake_gtest + ament_cmake_copyright ament_lint_auto ament_lint_common 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 abf26552..30666d63 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 @@ -23,7 +23,6 @@ namespace easynav CostmapFilter::CostmapFilter() { - } void 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 385402dc..b771d4f9 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 @@ -46,7 +46,6 @@ #include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_common/types/NavState.hpp" -#include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" #include "easynav_costmap_maps_manager/filters/InflationFilter.hpp" @@ -94,7 +93,8 @@ InflationFilter::on_initialize() RCLCPP_INFO( node->get_logger(), - "InflationFilter with inflation_radius = %lf inscribed_radius = %lf cost_scaling_factor = %lf", + "InflationFilter with inflation_radius = %lf inscribed_radius = %lf " + "cost_scaling_factor = %lf", inflation_radius_, inscribed_radius_, cost_scaling_factor_); seen_.clear(); 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 ab52f1b1..7e3b37f6 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 @@ -20,7 +20,6 @@ #include "easynav_common/types/NavState.hpp" #include "easynav_sensors/types/PointPerception.hpp" -#include "easynav_costmap_common/costmap_2d.hpp" #include "easynav_costmap_common/cost_values.hpp" #include "easynav_common/Parameters.hpp" @@ -32,7 +31,6 @@ namespace easynav ObstacleFilter::ObstacleFilter() { - } void diff --git a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp index ec98ca7f..5509e071 100644 --- a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_tests.cpp @@ -198,7 +198,7 @@ TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) } ///// \brief Dynamic map update test with point cloud -//TEST_F(CostmapMapsManagerTest, BasicDynamicUpdate) +// TEST_F(CostmapMapsManagerTest, BasicDynamicUpdate) //{ // auto node = std::make_shared("test_node"); // auto manager = std::make_shared(); @@ -238,7 +238,7 @@ TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) //} // ///// \brief OccupancyGrid updates map via subscription -//TEST_F(CostmapMapsManagerTest, IncomingOccupancyGridUpdatesMaps) +// TEST_F(CostmapMapsManagerTest, IncomingOccupancyGridUpdatesMaps) //{ // auto node = std::make_shared("test_node2"); // auto manager = std::make_shared(); @@ -277,14 +277,14 @@ TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) //} // ///// \brief Helper subclass to force map path for savemap -//class FriendCostmapMapsManager : public easynav::CostmapMapsManager +// class FriendCostmapMapsManager : public easynav::CostmapMapsManager //{ -//public: +// public: // void force_path(const std::string & path) {map_path_ = path;} //}; // ///// \brief Test that the savemap service correctly stores YAML and PGM files -//TEST_F(CostmapMapsManagerTest, SavemapServiceWorks) +// TEST_F(CostmapMapsManagerTest, SavemapServiceWorks) //{ // auto node = std::make_shared("test_savemap_node"); // auto manager = std::make_shared(); @@ -325,7 +325,8 @@ TEST_F(CostmapMapsManagerTest, IncomingMapTopicUpdatesInternalAndNavState) // // // Reload the map from the generated YAML + PGM files // nav_msgs::msg::OccupancyGrid loaded_grid; -// EXPECT_EQ(easynav::loadMapFromYaml(yaml_path + ".yaml", loaded_grid), easynav::LOAD_MAP_SUCCESS); +// EXPECT_EQ(easynav::loadMapFromYaml(yaml_path + ".yaml", loaded_grid), +// easynav::LOAD_MAP_SUCCESS); // // easynav::Costmap2D loaded_map(loaded_grid); // diff --git a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt index 775713ed..a353f281 100644 --- a/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_navmap_maps_manager/CMakeLists.txt @@ -80,9 +80,17 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Files derived from ROS / Nav2 keep their original copyright and license + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) ament_lint_auto_find_test_dependencies() + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if(NOT "$ENV{ROS_DISTRO}" STREQUAL "humble") + find_package(ament_cmake_copyright REQUIRED) + ament_copyright(EXCLUDE + src/easynav_navmap_maps_manager/filters/InflationFilter.cpp + src/easynav_navmap_maps_manager/map_io.cpp + ) + endif() find_package(ament_cmake_gtest REQUIRED) add_subdirectory(tests) 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 ed3b393c..da7afbc2 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 @@ -16,20 +16,20 @@ /// \file /// \brief Declaration of the NavMapMapsManager method. -#ifndef EASYNAV_NAVMAP_MAPS_MANAGER__NAVMAP_MAPS_MANAGER_HPP_ -#define EASYNAV_NAVMAP_MAPS_MANAGER__NAVMAP_MAPS_MANAGER_HPP_ +#ifndef EASYNAV_NAVMAP_MAPS_MANAGER__NAVMAPMAPSMANAGER_HPP_ +#define EASYNAV_NAVMAP_MAPS_MANAGER__NAVMAPMAPSMANAGER_HPP_ #include +#include +#include #include "nav_msgs/msg/occupancy_grid.hpp" #include "navmap_ros_interfaces/msg/nav_map.hpp" #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" - #include "easynav_core/MapsManagerBase.hpp" #include "navmap_core/NavMap.hpp" - #include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" #include "pluginlib/class_loader.hpp" @@ -66,7 +66,7 @@ class NavMapMapsManager : public easynav::MapsManagerBase * * @throws std::runtime_error if initialization fails. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. @@ -76,7 +76,7 @@ class NavMapMapsManager : public easynav::MapsManagerBase * * @param nav_state Current state of the navigation system. */ - virtual void update(::easynav::NavState & nav_state) override; + void update(::easynav::NavState & nav_state) override; /** * @brief Replaces the current static map. @@ -136,4 +136,4 @@ class NavMapMapsManager : public easynav::MapsManagerBase } // namespace navmap } // namespace easynav -#endif // EASYNAV_NAVMAP_MAPS_MANAGER__NAVMAP_MAPS_MANAGER_HPP_ +#endif // EASYNAV_NAVMAP_MAPS_MANAGER__NAVMAPMAPSMANAGER_HPP_ 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 7fb10eab..f2b511dc 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 @@ -35,8 +35,8 @@ class InflationFilter : public NavMapFilter public: InflationFilter(); - virtual void on_initialize() override; - virtual void update(::easynav::NavState & nav_state) override; + void on_initialize() override; + void update(::easynav::NavState & nav_state) override; bool inflate_layer_u8( ::navmap::NavMap & nm, @@ -59,4 +59,4 @@ class InflationFilter : public NavMapFilter } // namespace easynav -#endif // EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__IINFLATIONFILTER_HPP_ +#endif // EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ 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 358e1cf0..32d7f6eb 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 @@ -14,10 +14,11 @@ // limitations under the License. -#ifndef EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ -#define EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ +#ifndef EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__NAVMAPFILTER_HPP_ +#define EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__NAVMAPFILTER_HPP_ #include +#include #include "easynav_common/types/NavState.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -59,4 +60,4 @@ class NavMapFilter }; } // namespace navmap } // namespace easynav -#endif // EASYNAV_PLANNER__FILTERS__NAVMAPFILTER_HPP_ +#endif // EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__NAVMAPFILTER_HPP_ 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 a90ac840..20006aa8 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 @@ -14,16 +14,14 @@ // limitations under the License. -#ifndef EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ -#define EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ +#ifndef EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__OBSTACLEFILTER_HPP_ +#define EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__OBSTACLEFILTER_HPP_ #include +#include #include "navmap_core/NavMap.hpp" #include "easynav_common/types/NavState.hpp" - -#include - #include "easynav_navmap_maps_manager/filters/NavMapFilter.hpp" namespace easynav @@ -45,11 +43,11 @@ class ObstacleFilter : public NavMapFilter public: ObstacleFilter(); - virtual void on_initialize() override; - virtual void update(::easynav::NavState & nav_state) override; + void on_initialize() override; + void update(::easynav::NavState & nav_state) override; - virtual bool is_adding_layer() override {return true;} - virtual std::string get_layer_name() override {return "obstacles";} + bool is_adding_layer() override {return true;} + std::string get_layer_name() override {return "obstacles";} private: ::navmap::NavMap navmap_; @@ -62,4 +60,4 @@ class ObstacleFilter : public NavMapFilter } // namespace navmap } // namespace easynav -#endif // EASYNAV_NAVMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ +#endif // EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__OBSTACLEFILTER_HPP_ diff --git a/maps_managers/easynav_navmap_maps_manager/package.xml b/maps_managers/easynav_navmap_maps_manager/package.xml index e8ee3752..1819c677 100644 --- a/maps_managers/easynav_navmap_maps_manager/package.xml +++ b/maps_managers/easynav_navmap_maps_manager/package.xml @@ -31,6 +31,7 @@ yaml_cpp_vendor ament_cmake_gtest + ament_cmake_copyright ament_lint_auto ament_lint_common geometry_msgs 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 e690a8f0..859db415 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 @@ -176,7 +176,6 @@ NavMapMapsManager::on_initialize() "/incoming_occ_map", rclcpp::QoS(1).transient_local().reliable(), [&](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { - resolution_ = msg->info.resolution; navmap_ = navmap_ros::from_occupancy_grid(*msg); @@ -191,7 +190,6 @@ NavMapMapsManager::on_initialize() "/incoming_pc2_map", rclcpp::QoS(100), [&](sensor_msgs::msg::PointCloud2::UniquePtr msg) { - navmap_ros::BuildParams params; navmap_ = navmap_ros::from_pointcloud2(*msg, navmap_msg_, params); 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 b2c96339..ac759c2b 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 @@ -72,7 +72,10 @@ bool InflationFilter::inflate_layer_u8( float inscribed_radius) { using ::navmap::NavCelId; - using namespace navmap_ros; // bring FREE_SPACE, LETHAL_OBSTACLE, etc. + using navmap_ros::FREE_SPACE; + using navmap_ros::INSCRIBED_INFLATED_OBSTACLE; + using navmap_ros::LETHAL_OBSTACLE; + using navmap_ros::NO_INFORMATION; if (nm.navcels.empty() || inflation_radius <= 0.0f || cost_scaling_factor <= 0.0f) { return false; @@ -173,7 +176,7 @@ bool InflationFilter::inflate_layer_u8( if (dst[u] != NO_INFORMATION && dst[u] != LETHAL_OBSTACLE) { const std::uint8_t c = cost_from_dist(du); - if (c > dst[u]) {dst[u] = c;} // monotonic accumulation + if (c > dst[u]) {dst[u] = c;} // monotonic accumulation } // Neighbor relaxation 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 8ad18b39..8508e014 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 @@ -25,7 +25,6 @@ namespace navmap NavMapFilter::NavMapFilter() { - } void 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 90c3b067..0e2c7411 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 @@ -38,7 +38,6 @@ namespace navmap ObstacleFilter::ObstacleFilter() { - } void @@ -112,8 +111,8 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) { std::size_t operator()(const Key & k) const noexcept { - std::size_t h1 = std::hash{}(static_cast(k.ix)); - std::size_t h2 = std::hash{}(static_cast(k.iy)); + std::size_t h1 = std::hash{}(static_cast(k.ix)); + std::size_t h2 = std::hash{}(static_cast(k.iy)); return h1 ^ (h2 + 0x9e3779b97f4a7c15ULL + (h1 << 6) + (h1 >> 2)); } }; diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp index 913234a8..6d6a4add 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_tests.cpp @@ -13,17 +13,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include +#include "gtest/gtest.h" #include "easynav_sensors/types/PointPerception.hpp" #include "easynav_common/types/NavState.hpp" - #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_srvs/srv/trigger.hpp" -#include - /// \brief Fixture for NavMapMapsManager tests class NavMapMapsManagerTest : public ::testing::Test { @@ -40,7 +38,7 @@ class NavMapMapsManagerTest : public ::testing::Test }; ///// \brief Dynamic map update test with point cloud -//TEST_F(NavMapMapsManagerTest, BasicDynamicUpdate) +// TEST_F(NavMapMapsManagerTest, BasicDynamicUpdate) //{ // auto node = std::make_shared("test_node"); // auto manager = std::make_shared(); @@ -80,7 +78,7 @@ class NavMapMapsManagerTest : public ::testing::Test //} // ///// \brief OccupancyGrid updates map via subscription -//TEST_F(NavMapMapsManagerTest, IncomingOccupancyGridUpdatesMaps) +// TEST_F(NavMapMapsManagerTest, IncomingOccupancyGridUpdatesMaps) //{ // auto node = std::make_shared("test_node2"); // auto manager = std::make_shared(); @@ -119,14 +117,14 @@ class NavMapMapsManagerTest : public ::testing::Test //} // ///// \brief Helper subclass to force map path for savemap -//class FriendNavMapMapsManager : public easynav::navmap::NavMapMapsManager +// class FriendNavMapMapsManager : public easynav::navmap::NavMapMapsManager //{ -//public: +// public: // void force_path(const std::string & path) {map_path_ = path;} //}; // ///// \brief Test that the savemap service correctly stores YAML and PGM files -//TEST_F(NavMapMapsManagerTest, SavemapServiceWorks) +// TEST_F(NavMapMapsManagerTest, SavemapServiceWorks) //{ // auto node = std::make_shared("test_savemap_node"); // auto manager = std::make_shared(); @@ -167,7 +165,8 @@ class NavMapMapsManagerTest : public ::testing::Test // // // Reload the map from the generated YAML + PGM files // nav_msgs::msg::OccupancyGrid loaded_grid; -// EXPECT_EQ(easynav::loadMapFromYaml(yaml_path + ".yaml", loaded_grid), easynav::LOAD_MAP_SUCCESS); +// EXPECT_EQ(easynav::loadMapFromYaml(yaml_path + ".yaml", loaded_grid), +// easynav::LOAD_MAP_SUCCESS); // // easynav::NavMap2D loaded_map(loaded_grid); // diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_filter_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_filter_tests.cpp index 0130b9bf..a27f644d 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_filter_tests.cpp +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_obstacle_filter_tests.cpp @@ -143,7 +143,8 @@ class NavmapObstacleFilterLimitsTest : public NavmapObstacleFilterTest return nav_state_.get<::navmap::NavMap>("map.navmap"); } - // Highest value of the two triangles of the cell at (x, y): a voxel center lies on their diagonal. + // Highest value of the two triangles of the cell at (x, y): a voxel center lies on their + // diagonal. static std::uint8_t cell_at(const ::navmap::NavMap & nm, float x, float y) { return std::max(obstacles_at(nm, x + 0.02f, y - 0.02f), obstacles_at(nm, x - 0.02f, y + 0.02f)); diff --git a/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt b/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt index 31d8a37a..a03a2d1d 100644 --- a/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_octomap_maps_manager/CMakeLists.txt @@ -81,9 +81,17 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Files derived from ROS / Nav2 keep their original copyright and license + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) ament_lint_auto_find_test_dependencies() + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if(NOT "$ENV{ROS_DISTRO}" STREQUAL "humble") + find_package(ament_cmake_copyright REQUIRED) + ament_copyright(EXCLUDE + src/easynav_octomap_maps_manager/filters/InflationFilter.cpp + src/easynav_octomap_maps_manager/map_io.cpp + ) + endif() endif() ament_export_include_directories("include/${PROJECT_NAME}") 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 c7c3b0d3..bf67c672 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 @@ -16,19 +16,19 @@ /// \file /// \brief Declaration of the OctomapMapsManager method. -#ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__OCTOMAP_MAPS_MANAGER_HPP_ -#define EASYNAV_OCTOMAP_MAPS_MANAGER__OCTOMAP_MAPS_MANAGER_HPP_ +#ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__OCTOMAPMAPSMANAGER_HPP_ +#define EASYNAV_OCTOMAP_MAPS_MANAGER__OCTOMAPMAPSMANAGER_HPP_ #include +#include +#include #include "nav_msgs/msg/occupancy_grid.hpp" #include "octomap_msgs/msg/octomap.hpp" #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" - #include "easynav_core/MapsManagerBase.hpp" #include "easynav_octomap_maps_manager/octomap.hpp" - #include "easynav_octomap_maps_manager/filters/OctomapFilter.hpp" #include "pluginlib/class_loader.hpp" @@ -65,7 +65,7 @@ class OctomapMapsManager : public easynav::MapsManagerBase * * @throws std::runtime_error if initialization fails. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. @@ -75,7 +75,7 @@ class OctomapMapsManager : public easynav::MapsManagerBase * * @param nav_state Current state of the navigation system. */ - virtual void update(::easynav::NavState & nav_state) override; + void update(::easynav::NavState & nav_state) override; protected: /** @@ -123,4 +123,4 @@ class OctomapMapsManager : public easynav::MapsManagerBase } // namespace octomap } // namespace easynav -#endif // EASYNAV_OCTOMAP_MAPS_MANAGER__OCTOMAP_MAPS_MANAGER_HPP_ +#endif // EASYNAV_OCTOMAP_MAPS_MANAGER__OCTOMAPMAPSMANAGER_HPP_ 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 5f5a602f..2c305fc2 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 @@ -60,4 +60,4 @@ class InflationFilter : public OctomapFilter } // namespace easynav -#endif // EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__IINFLATIONFILTER_HPP_ +#endif // EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ 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 8e8465e8..68295485 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 @@ -14,8 +14,8 @@ // limitations under the License. -#ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ -#define EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ +#ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__OBSTACLEFILTER_HPP_ +#define EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__OBSTACLEFILTER_HPP_ #include @@ -48,4 +48,4 @@ class ObstacleFilter : public OctomapFilter } // namespace octomap } // namespace easynav -#endif // EASYNAV_OCTOMAP_MAPS_MANAGER__OBSTACLEFILTER_HPP_ +#endif // EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__OBSTACLEFILTER_HPP_ 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 0ee169de..97ccbabb 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 @@ -14,10 +14,11 @@ // limitations under the License. -#ifndef EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ -#define EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ +#ifndef EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__OCTOMAPFILTER_HPP_ +#define EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__OCTOMAPFILTER_HPP_ #include +#include #include "easynav_octomap_maps_manager/octomap.hpp" #include "easynav_common/types/NavState.hpp" @@ -57,4 +58,4 @@ class OctomapFilter }; } // namespace octomap } // namespace easynav -#endif // EASYNAV_PLANNER__FILTERS__OCTOMAPFILTER_HPP_ +#endif // EASYNAV_OCTOMAP_MAPS_MANAGER__FILTERS__OCTOMAPFILTER_HPP_ diff --git a/maps_managers/easynav_octomap_maps_manager/package.xml b/maps_managers/easynav_octomap_maps_manager/package.xml index d2696693..360729c8 100644 --- a/maps_managers/easynav_octomap_maps_manager/package.xml +++ b/maps_managers/easynav_octomap_maps_manager/package.xml @@ -31,6 +31,7 @@ yaml_cpp_vendor ament_cmake_gtest + ament_cmake_copyright ament_lint_auto ament_lint_common 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 2cf85632..ac63e753 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 @@ -154,8 +154,8 @@ OctomapMapsManager::on_initialize() // } // incoming_occ_map_sub_ = node->create_subscription( -// node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + "/incoming_occ_map", -// rclcpp::QoS(1).transient_local().reliable(), +// node->get_node_base_interface()->get_fully_qualified_name() + std::string("/") + plugin_name + +// "/incoming_occ_map", rclcpp::QoS(1).transient_local().reliable(), // [this](nav_msgs::msg::OccupancyGrid::UniquePtr msg) { // // resolution_ = msg->info.resolution; @@ -172,7 +172,6 @@ OctomapMapsManager::on_initialize() "/incoming_pc2_map", rclcpp::QoS(100), [&](sensor_msgs::msg::PointCloud2::UniquePtr msg) { - geometry_msgs::msg::TransformStamped tf_msg; try { tf_msg = RTTFBuffer::getInstance()->lookupTransform( 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 643651e8..eff993cd 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 @@ -77,7 +77,6 @@ bool InflationFilter::inflate_layer_u8( float cost_scaling_factor, float inscribed_radius) { - using namespace ::octomap; if (nm.navcels.empty() || inflation_radius <= 0.0f || cost_scaling_factor <= 0.0f) { return false; } @@ -114,7 +113,7 @@ bool InflationFilter::inflate_layer_u8( std::priority_queue pq; // Inicializar dst y semillas - auto & dst = dst_view->mutable_data(); // marca dirty + auto & dst = dst_view->mutable_data(); // marca dirty const auto & src = src_view->data(); bool any_seed = false; @@ -126,20 +125,20 @@ bool InflationFilter::inflate_layer_u8( dst[cid] = LETHAL_OBSTACLE; any_seed = true; } else if (s == NO_INFORMATION) { - dst[cid] = NO_INFORMATION; // mantener desconocido + dst[cid] = NO_INFORMATION; // mantener desconocido } else { // si dst==src (in-place), preserva el valor actual; si no, escribe 0 if (dst_layer != src_layer) {dst[cid] = FREE_SPACE;} } } - if (!any_seed) {return true;} // nada que inflar + if (!any_seed) {return true;} // nada que inflar auto cost_from_dist = [&](float d) -> uint8_t { if (d <= 0.0f) {return LETHAL_OBSTACLE;} // 254 - if (d <= r_ins) {return INSCRIBED_INFLATED_OBSTACLE;} // 253 + if (d <= r_ins) {return INSCRIBED_INFLATED_OBSTACLE;} // 253 if (d > R) {return FREE_SPACE;} // 0 - const float x = d - r_ins; // >= 0 + const float x = d - r_ins; // >= 0 const double factor = -1.0 * cost_scaling_factor * x; double c = std::exp(factor) * (static_cast(INSCRIBED_INFLATED_OBSTACLE) - 1); if (c < 0.0) {c = 0.0;} @@ -156,7 +155,7 @@ bool InflationFilter::inflate_layer_u8( // Escribir coste si no es desconocido ni letal if (dst[u] != NO_INFORMATION && dst[u] != LETHAL_OBSTACLE) { const uint8_t c = cost_from_dist(du); - if (c > dst[u]) {dst[u] = c;} // tomamos el máximo (monótono) + if (c > dst[u]) {dst[u] = c;} // tomamos el máximo (monótono) } // Relajar vecinos 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 08d5cd0a..e820557d 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 @@ -32,7 +32,6 @@ namespace octomap ObstacleFilter::ObstacleFilter() { - } void 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 d97e6359..11b48a01 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 @@ -27,7 +27,6 @@ namespace octomap OctomapFilter::OctomapFilter() { - } void diff --git a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt index d8262de7..06d57097 100644 --- a/maps_managers/easynav_routes_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_routes_maps_manager/CMakeLists.txt @@ -87,8 +87,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() add_subdirectory(tests) 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 d194e338..af46fbef 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 @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ -#define EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ +#ifndef EASYNAV_ROUTES_MAPS_MANAGER__ROUTESFILTER_HPP_ +#define EASYNAV_ROUTES_MAPS_MANAGER__ROUTESFILTER_HPP_ #include #include @@ -71,4 +71,4 @@ class RoutesFilter } // namespace easynav -#endif // EASYNAV_ROUTES_MAPS_MANAGER_ROUTES_FILTER_HPP_ +#endif // EASYNAV_ROUTES_MAPS_MANAGER__ROUTESFILTER_HPP_ 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 576cc09a..0c7a7442 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 @@ -16,25 +16,21 @@ /// \file /// \brief Declaration of the RoutesMapsManager class and related types. -#ifndef EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ -#define EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ +#ifndef EASYNAV_ROUTES_MAPS_MANAGER__ROUTESMAPSMANAGER_HPP_ +#define EASYNAV_ROUTES_MAPS_MANAGER__ROUTESMAPSMANAGER_HPP_ + +#include +#include +#include #include "geometry_msgs/msg/pose.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include "visualization_msgs/msg/interactive_marker.hpp" #include "visualization_msgs/msg/interactive_marker_feedback.hpp" - #include "interactive_markers/interactive_marker_server.hpp" - #include "std_srvs/srv/trigger.hpp" - -#include -#include - #include "pluginlib/class_loader.hpp" - #include "easynav_core/MapsManagerBase.hpp" - #include "easynav_routes_maps_manager/RoutesFilter.hpp" #include "easynav_routes_maps_manager/routes_map.hpp" #include "easynav_routes_maps_manager/msg/routes_map.hpp" @@ -71,7 +67,7 @@ class RoutesMapsManager : public easynav::MapsManagerBase * * @throws std::runtime_error if initialization fails. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. @@ -81,7 +77,7 @@ class RoutesMapsManager : public easynav::MapsManagerBase * * @param nav_state Current state of the navigation system. */ - virtual void update(NavState & nav_state) override; + void update(NavState & nav_state) override; /// @brief Access to the loaded routes (for testing and tools). const RoutesMap & get_routes() const {return routes_;} @@ -144,4 +140,4 @@ class RoutesMapsManager : public easynav::MapsManagerBase } // namespace easynav -#endif // EASYNAV_PLANNER__ROUTESMAPMANAGER_HPP_ +#endif // EASYNAV_ROUTES_MAPS_MANAGER__ROUTESMAPSMANAGER_HPP_ 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 975883ba..508ec0f3 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 @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ -#define EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ +#ifndef EASYNAV_ROUTES_MAPS_MANAGER__FILTERS__ROUTESCOSTMAPFILTER_HPP_ +#define EASYNAV_ROUTES_MAPS_MANAGER__FILTERS__ROUTESCOSTMAPFILTER_HPP_ #include @@ -91,4 +91,4 @@ class RoutesCostmapFilter : public RoutesFilter } // namespace easynav -#endif // EASYNAV_ROUTES_MAPS_MANAGER_FILTERS_ROUTES_COSTMAP_FILTER_HPP_ +#endif // EASYNAV_ROUTES_MAPS_MANAGER__FILTERS__ROUTESCOSTMAPFILTER_HPP_ 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 3ecfe57a..0ad7692f 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 @@ -460,12 +460,12 @@ void RoutesMapsManager::publish_interactive_markers() visualization_msgs::msg::Marker add_text; add_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; add_text.text = "add"; - add_text.scale.z = 0.1; // font height (half) + add_text.scale.z = 0.1; // font height (half) add_text.color.r = 1.0f; add_text.color.g = 1.0f; add_text.color.b = 1.0f; add_text.color.a = 1.0f; - add_text.pose.position.z = 0.4; // slightly above the sphere + add_text.pose.position.z = 0.4; // slightly above the sphere add_ctrl.markers.push_back(add_marker); add_ctrl.markers.push_back(add_text); @@ -496,12 +496,12 @@ void RoutesMapsManager::publish_interactive_markers() visualization_msgs::msg::Marker remove_text; remove_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; remove_text.text = "remove"; - remove_text.scale.z = 0.1; // font height (half) + remove_text.scale.z = 0.1; // font height (half) remove_text.color.r = 1.0f; remove_text.color.g = 1.0f; remove_text.color.b = 1.0f; remove_text.color.a = 1.0f; - remove_text.pose.position.z = 1.3; // slightly above the red sphere + remove_text.pose.position.z = 1.3; // slightly above the red sphere remove_ctrl.markers.push_back(remove_marker); remove_ctrl.markers.push_back(remove_text); 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 5dc50ed6..3f4836a7 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 @@ -14,19 +14,15 @@ // limitations under the License. -#include "easynav_common/Parameters.hpp" -#include "easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp" - #include #include #include +#include "easynav_common/Parameters.hpp" +#include "easynav_routes_maps_manager/filters/RoutesCostmapFilter.hpp" #include "easynav_common/types/NavState.hpp" - #include "easynav_costmap_common/costmap_2d.hpp" - #include "easynav_routes_maps_manager/RoutesMapsManager.hpp" - #include "easynav_common/RTTFBuffer.hpp" namespace easynav diff --git a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/route_io.cpp b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/route_io.cpp index f2074a26..a14a98e0 100644 --- a/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/route_io.cpp +++ b/maps_managers/easynav_routes_maps_manager/src/easynav_routes_maps_manager/route_io.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include "yaml-cpp/yaml.h" namespace easynav { diff --git a/maps_managers/easynav_simple_maps_manager/CMakeLists.txt b/maps_managers/easynav_simple_maps_manager/CMakeLists.txt index 74314ac9..2786110e 100644 --- a/maps_managers/easynav_simple_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_simple_maps_manager/CMakeLists.txt @@ -55,8 +55,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest 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 aeda05cd..c497d8fa 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 @@ -16,12 +16,13 @@ /// \file /// \brief Declaration of the SimpleMapsManager method. -#ifndef EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ -#define EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ +#ifndef EASYNAV_SIMPLE_MAPS_MANAGER__SIMPLEMAPSMANAGER_HPP_ +#define EASYNAV_SIMPLE_MAPS_MANAGER__SIMPLEMAPSMANAGER_HPP_ + +#include #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_srvs/srv/trigger.hpp" - #include "easynav_core/MapsManagerBase.hpp" #include "easynav_simple_common/SimpleMap.hpp" @@ -56,7 +57,7 @@ class SimpleMapsManager : public easynav::MapsManagerBase * * @throws std::runtime_error if initialization fails. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Updates the internal maps using the current navigation state. @@ -66,7 +67,7 @@ class SimpleMapsManager : public easynav::MapsManagerBase * * @param nav_state Current state of the navigation system. */ - virtual void update(NavState & nav_state) override; + void update(NavState & nav_state) override; /** * @brief Replaces the current static map. @@ -135,4 +136,4 @@ class SimpleMapsManager : public easynav::MapsManagerBase } // namespace easynav -#endif // EASYNAV_PLANNER__SIMPLEMAPMANAGER_HPP_ +#endif // EASYNAV_SIMPLE_MAPS_MANAGER__SIMPLEMAPSMANAGER_HPP_ diff --git a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp index 05c06253..61e688a4 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp +++ b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_tests.cpp @@ -15,23 +15,20 @@ #include -#include +#include +#include +#include "gtest/gtest.h" #include "easynav_simple_common/SimpleMap.hpp" #include "easynav_common/RTTFBuffer.hpp" #include "easynav_common/TransformListener.hpp" #include "easynav_sensors/types/PointPerception.hpp" #include "easynav_simple_maps_manager/SimpleMapsManager.hpp" - #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "tf2_ros/transform_listener.hpp" - #include "std_srvs/srv/trigger.hpp" -#include -#include - /// \brief Fixture for SimpleMapsManager tests (minimal) class SimpleMapsManagerTest : public ::testing::Test { diff --git a/planners/easynav_costmap_planner/CMakeLists.txt b/planners/easynav_costmap_planner/CMakeLists.txt index e6baa798..51845f41 100644 --- a/planners/easynav_costmap_planner/CMakeLists.txt +++ b/planners/easynav_costmap_planner/CMakeLists.txt @@ -47,8 +47,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest 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 d35d9090..3aceaa9f 100644 --- a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp +++ b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp @@ -42,7 +42,7 @@ class CostmapPlanner : public PlannerMethodBase * * Initializes internal parameters and configuration values. */ - explicit CostmapPlanner(); + CostmapPlanner(); /** * @brief Initializes the planner. @@ -52,7 +52,7 @@ class CostmapPlanner : public PlannerMethodBase * * @throws std::runtime_error if initialization fails. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Executes a planning cycle using the current navigation state. @@ -65,8 +65,8 @@ class CostmapPlanner : public PlannerMethodBase protected: double cost_factor_; ///< Scaling factor applied to cell cost values. - double inflation_penalty_; ///< Extra cost penalty for paths near inflated obstacles. - double heuristic_scale_ {1.0}; ///< Scaling factor applied to the heuristic term in A*. + double inflation_penalty_; ///< Extra cost penalty for paths near inflated obstacles. + double heuristic_scale_ {1.0}; ///< Scaling factor applied to the heuristic term in A*. bool continuous_replan_ {true}; ///< Wheter replan path at freq time nav_msgs::msg::Path current_path_; ///< Most recently computed path. geometry_msgs::msg::Pose current_goal_; ///< Current goal. diff --git a/planners/easynav_navmap_planner/CMakeLists.txt b/planners/easynav_navmap_planner/CMakeLists.txt index 4fbc7b99..8662a991 100644 --- a/planners/easynav_navmap_planner/CMakeLists.txt +++ b/planners/easynav_navmap_planner/CMakeLists.txt @@ -53,8 +53,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest 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 52aa5a88..7e38273d 100644 --- a/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp +++ b/planners/easynav_navmap_planner/include/easynav_navmap_planner/AStarPlanner.hpp @@ -14,18 +14,19 @@ // limitations under the License. /// \file -/// \brief Declaration of the AStarPlanner class implementing A* path planning using ::navmap::NavMap. +/// \brief Declaration of the AStarPlanner class implementing A* path planning using +/// ::navmap::NavMap. -#ifndef EASYNAV_NAVMAP_PLANNER__NAVMAPPLANNER_HPP_ -#define EASYNAV_NAVMAP_PLANNER__NAVMAPPLANNER_HPP_ +#ifndef EASYNAV_NAVMAP_PLANNER__ASTARPLANNER_HPP_ +#define EASYNAV_NAVMAP_PLANNER__ASTARPLANNER_HPP_ #include #include #include -#include +#include +#include "Eigen/Core" #include "nav_msgs/msg/path.hpp" - #include "easynav_core/PlannerMethodBase.hpp" #include "navmap_core/NavMap.hpp" #include "easynav_common/types/NavState.hpp" @@ -50,7 +51,7 @@ class AStarPlanner : public PlannerMethodBase * * Initializes internal parameters and configuration values. */ - explicit AStarPlanner(); + AStarPlanner(); /** * @brief Initializes the planner. @@ -60,7 +61,7 @@ class AStarPlanner : public PlannerMethodBase * * @throws std::runtime_error if initialization fails. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Executes a planning cycle using the current navigation state. @@ -194,4 +195,4 @@ class AStarPlanner : public PlannerMethodBase } // namespace easynav -#endif // EASYNAV_NAVMAP_PLANNER__NAVMAPPLANNER_HPP_ +#endif // EASYNAV_NAVMAP_PLANNER__ASTARPLANNER_HPP_ 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 f2dfe141..bcaebc91 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -517,7 +517,10 @@ std::vector AStarPlanner::a_star_path( const geometry_msgs::msg::Pose & goal) { using ::navmap::NavCelId; - using namespace navmap_ros; + using navmap_ros::FREE_SPACE; + using navmap_ros::INSCRIBED_INFLATED_OBSTACLE; + using navmap_ros::LETHAL_OBSTACLE; + using navmap_ros::NO_INFORMATION; if (nm.navcels.empty()) {return {};} @@ -597,8 +600,9 @@ std::vector AStarPlanner::a_star_path( } // Weighted step cost: - // base geometric cost (edge length) scaled by (cost_factor_ + cost_weight_ * norm_cost(target)). - // This preserves admissibility with heuristic h = Euclidean distance, since the minimal multiplier ≥ 1. + // base geometric cost (edge length) scaled by (cost_factor_ + cost_weight_ * + // norm_cost(target)). This preserves admissibility with heuristic h = Euclidean distance, since + // the minimal multiplier ≥ 1. auto step_cost = [&](NavCelId from, NavCelId to) -> double { const double base = euclid(from, to); if (!std::isfinite(base) || base <= 0.0) { diff --git a/planners/easynav_simple_planner/CMakeLists.txt b/planners/easynav_simple_planner/CMakeLists.txt index 1708ab08..317037f7 100644 --- a/planners/easynav_simple_planner/CMakeLists.txt +++ b/planners/easynav_simple_planner/CMakeLists.txt @@ -47,8 +47,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest 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 45a75ff6..400ad9bd 100644 --- a/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp +++ b/planners/easynav_simple_planner/include/easynav_simple_planner/SimplePlanner.hpp @@ -16,8 +16,8 @@ /// \file /// \brief Declaration of the SimplePlanner class implementing A* path planning. -#ifndef EASYNAV_PLANNER__SIMPLEPLANNER_HPP_ -#define EASYNAV_PLANNER__SIMPLEPLANNER_HPP_ +#ifndef EASYNAV_SIMPLE_PLANNER__SIMPLEPLANNER_HPP_ +#define EASYNAV_SIMPLE_PLANNER__SIMPLEPLANNER_HPP_ #include @@ -39,7 +39,7 @@ class SimplePlanner : public PlannerMethodBase * * Initializes the internal variables and parameters of the planner. */ - explicit SimplePlanner(); + SimplePlanner(); /** * @brief Initializes the planner. @@ -49,7 +49,7 @@ class SimplePlanner : public PlannerMethodBase * * @throws std::runtime_error if initialization fails. */ - virtual void on_initialize() override; + void on_initialize() override; /** * @brief Updates the planner by computing a new path. @@ -111,4 +111,4 @@ class SimplePlanner : public PlannerMethodBase } // namespace easynav -#endif // EASYNAV_PLANNER__SIMPLEPLANNER_HPP_ +#endif // EASYNAV_SIMPLE_PLANNER__SIMPLEPLANNER_HPP_ diff --git a/recoveries/easynav_diagnostic_recovery/CMakeLists.txt b/recoveries/easynav_diagnostic_recovery/CMakeLists.txt index f0e432b7..f719c717 100644 --- a/recoveries/easynav_diagnostic_recovery/CMakeLists.txt +++ b/recoveries/easynav_diagnostic_recovery/CMakeLists.txt @@ -92,8 +92,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/recoveries/easynav_diagnostic_recovery/include/easynav_diagnostic_recovery/evaluators/ControllerStuckEvaluator.hpp b/recoveries/easynav_diagnostic_recovery/include/easynav_diagnostic_recovery/evaluators/ControllerStuckEvaluator.hpp index 74a00278..c4f8cea0 100644 --- a/recoveries/easynav_diagnostic_recovery/include/easynav_diagnostic_recovery/evaluators/ControllerStuckEvaluator.hpp +++ b/recoveries/easynav_diagnostic_recovery/include/easynav_diagnostic_recovery/evaluators/ControllerStuckEvaluator.hpp @@ -19,9 +19,9 @@ #define EASYNAV_DIAGNOSTIC_RECOVERY__EVALUATORS__CONTROLLERSTUCKEVALUATOR_HPP_ #include +#include #include "rclcpp/time.hpp" - #include "easynav_diagnostic_recovery/RecoveryEvaluatorBase.hpp" namespace easynav diff --git a/recoveries/easynav_diagnostic_recovery/include/easynav_diagnostic_recovery/mitigations/AdvanceRecovery.hpp b/recoveries/easynav_diagnostic_recovery/include/easynav_diagnostic_recovery/mitigations/AdvanceRecovery.hpp index d477b792..986d9b8b 100644 --- a/recoveries/easynav_diagnostic_recovery/include/easynav_diagnostic_recovery/mitigations/AdvanceRecovery.hpp +++ b/recoveries/easynav_diagnostic_recovery/include/easynav_diagnostic_recovery/mitigations/AdvanceRecovery.hpp @@ -19,9 +19,9 @@ #define EASYNAV_DIAGNOSTIC_RECOVERY__MITIGATIONS__ADVANCERECOVERY_HPP_ #include +#include #include "rclcpp/time.hpp" - #include "easynav_diagnostic_recovery/RecoveryMitigationBase.hpp" namespace easynav diff --git a/recoveries/easynav_diagnostic_recovery/tests/diagnostic_recovery_bases_tests.cpp b/recoveries/easynav_diagnostic_recovery/tests/diagnostic_recovery_bases_tests.cpp index 272a3115..87f34ec0 100644 --- a/recoveries/easynav_diagnostic_recovery/tests/diagnostic_recovery_bases_tests.cpp +++ b/recoveries/easynav_diagnostic_recovery/tests/diagnostic_recovery_bases_tests.cpp @@ -13,8 +13,8 @@ // limitations under the License. /// \file -/// \brief Tests for the plugin interfaces DiagnosticRecoveryManager is composed of: safety reflexes, -/// evaluators and mitigations (exception safety, diagnostics, lifecycle forwarding). +/// \brief Tests for the plugin interfaces DiagnosticRecoveryManager is composed of: safety +/// reflexes, evaluators and mitigations (exception safety, diagnostics, lifecycle forwarding). #include #include diff --git a/recoveries/easynav_diagnostic_recovery/tests/mitigations/advance_recovery_tests.cpp b/recoveries/easynav_diagnostic_recovery/tests/mitigations/advance_recovery_tests.cpp index b3569c57..f9067462 100644 --- a/recoveries/easynav_diagnostic_recovery/tests/mitigations/advance_recovery_tests.cpp +++ b/recoveries/easynav_diagnostic_recovery/tests/mitigations/advance_recovery_tests.cpp @@ -163,7 +163,8 @@ TEST_F(AdvanceRecoveryTestCase, AccumulatesTotalTimeAcrossQuickReactivations) rec->internal_start(nav_state); auto first_status = rec->internal_cycle(nav_state); - ASSERT_EQ(first_status, easynav_diagnostic_recovery::RecoveryStatus::RUNNING); // too soon to escalate yet + // too soon to escalate yet + ASSERT_EQ(first_status, easynav_diagnostic_recovery::RecoveryStatus::RUNNING); rec->internal_stop(nav_state); std::this_thread::sleep_for(std::chrono::milliseconds(60)); // << episode_gap, same episode diff --git a/recoveries/easynav_simple_recovery/CMakeLists.txt b/recoveries/easynav_simple_recovery/CMakeLists.txt index 1dd6046d..ad931127 100644 --- a/recoveries/easynav_simple_recovery/CMakeLists.txt +++ b/recoveries/easynav_simple_recovery/CMakeLists.txt @@ -46,8 +46,10 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) + # Humble's ament_copyright does not recognize the license with the EasyNav line in the header + if("$ENV{ROS_DISTRO}" STREQUAL "humble") + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + endif() ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) diff --git a/recoveries/easynav_simple_recovery/include/easynav_simple_recovery/SimpleRecoveryManager.hpp b/recoveries/easynav_simple_recovery/include/easynav_simple_recovery/SimpleRecoveryManager.hpp index 6f84ad7e..f3f29008 100644 --- a/recoveries/easynav_simple_recovery/include/easynav_simple_recovery/SimpleRecoveryManager.hpp +++ b/recoveries/easynav_simple_recovery/include/easynav_simple_recovery/SimpleRecoveryManager.hpp @@ -112,7 +112,7 @@ class SimpleRecoveryManager : public RecoveryManagerBase double min_obstacle_z_ {0.05}; // Ignore points below (floor) [m] double max_obstacle_z_ {0.5}; // Ignore points above the robot [m] (geometry) double sensors_timeout_ {5.0}; // No sensor data for this long: sensors lost [s] - double max_position_variance_ {1.0}; // Above it (x or y): localization lost [m^2] + double max_position_variance_ {1.0}; // Above it (x or y): localization lost [m^2] double relocalize_timeout_ {20.0}; // Rotating longer than this: abort the mission [s] double rotate_speed_ {0.5}; // [rad/s] double stuck_time_ {10.0}; // Commanded but not moving for this long: stuck [s]