Enable cpplint and copyright linters

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)

common/easynav_costmap_common/package.xml

@@ -25,6 +25,7 @@
   <depend>tf2_geometry_msgs</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_copyright</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

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)

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 <vector>
 #include <utility>
+#include <memory>
+#include <string>
 
 #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_

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)

controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp

@@ -18,26 +18,22 @@
 #define EASYNAV_MPC_CONTROLLER__MPCCONTROLLER_HPP_
 
 #include <vector>
-#include <Eigen/Core>
 #include <cmath>
+#include <memory>
 
-#include <tf2/LinearMath/Quaternion.hpp>
-
+#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 <pcl/io/pcd_io.h>
-#include <pcl_conversions/pcl_conversions.h>
-
+#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<MPCOptimizer> optimizer_;  ///< MPC optimizer
 
-  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr mpc_path_pub_;            ///< Publisher for MPC path.
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr detection_pub_; ///< Publisher for MPC obstacles.
+  /// Publisher for MPC path.
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr mpc_path_pub_;
+  /// Publisher for MPC obstacles.
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr detection_pub_;
 
 private:
   geometry_msgs::msg::TwistStamped cmd_vel_;  ///< Current velocity command.
-
 };
 
 }  // namespace easynav

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 <vector>
+
 #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,21 +102,22 @@ class MPCOptimizer
 
   /// \brief Wrap for real cost function
   /// \param u std::vector<double> Velocity vector to be optimized
-  /// \param grad std::vector<double> gradient values for optimizer. It is NOT used for this implementation.
+  /// \param grad std::vector<double> 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<double> & u, [[maybe_unused]]
     std::vector<double> & grad, void * data);
 
   /// \brief Real cost function with static propierties
   /// \param u std::vector<double> Velocity vector to be optimized
-  /// \param grad std::vector<double> gradient values for optimizer. It is NOT used for this implementation.
+  /// \param grad std::vector<double> 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<double> & x,
     std::vector<double> & grad, void * data);
-
 };
 
 /// \brief Struct used as element in callback.

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;
   }

controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCOptimizer.cpp

@@ -134,7 +134,6 @@ MPCOptimizer::cost_function(
   }
 
   return cost;
-
 }
 
 double

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 <memory>
 #include <string>

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)

controllers/easynav_mppi_controller/include/easynav_mppi_controller/MPPIController.hpp

@@ -18,14 +18,13 @@
 
 #include <memory>
 #include <string>
+#include <utility>
+#include <vector>
 
 #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<MPPIOptimizer> optimizer_;  ///< MPPI optimizer
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr mppi_candidates_pub_; ///< Publisher for MPPI candidates markers.
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr mppi_optimal_pub_;    ///< Publisher for MPPI optimal path markers.
+  /// Publisher for MPPI candidates markers.
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr mppi_candidates_pub_;
+  /// Publisher for MPPI optimal path markers.
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::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<std::vector<std::pair<double, double>>> & all_trajs,
     const std::vector<std::pair<double, double>> & best_traj);
-
 };
 
 }  // namespace easynav

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<std::vector<std::pair<double, double>>> all_trajectories; ///< All sampled trajectories.
-  std::vector<std::pair<double, double>> best_trajectory; ///< Best trajectory found during optimization.
+  double v;  ///< Linear velocity command.
+  double w;  ///< Angular velocity command.
+  /// All sampled trajectories.
+  std::vector<std::vector<std::pair<double, double>>> all_trajectories;
+  /// Best trajectory found during optimization.
+  std::vector<std::pair<double, double>> 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<double> normal_ = std::normal_distribution<double>(0.0, 0.5);    ///< Normal distribution for noise in sampling.
-  std::normal_distribution<double> v_noise_ = std::normal_distribution<double>(0.0, 0.05);  ///< Normal distribution for noise in linear velocity.
-  std::normal_distribution<double> w_noise_ = std::normal_distribution<double>(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<double> normal_ = std::normal_distribution<double>(0.0, 0.5);
+  /// Normal distribution for noise in linear velocity.
+  std::normal_distribution<double> v_noise_ = std::normal_distribution<double>(0.0, 0.05);
+  /// Normal distribution for noise in angular velocity.
+  std::normal_distribution<double> w_noise_ = std::normal_distribution<double>(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

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 <cmath>
 #include <limits>
 #include <algorithm>
 
+#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

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)