cpplint

Signed-off-by: methylDragon <methylDragon@gmail.com>

fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp

@@ -99,8 +99,9 @@ class Unicycle2DStateCostFunction : public benchmark::Fixture
 };
 
 // Cost function process noise and covariance
-const double Unicycle2DStateCostFunction::process_noise_diagonal[] =
-{1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9};
+const double Unicycle2DStateCostFunction::process_noise_diagonal[] = {
+  1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9
+};
 
 const fuse_core::Matrix8d Unicycle2DStateCostFunction::covariance =
   fuse_core::Vector8d(process_noise_diagonal).asDiagonal();
@@ -149,8 +150,9 @@ BENCHMARK_F(
   AutoDiffUnicycle2DStateCostFunction)(benchmark::State & state)
 {
   // Create cost function using automatic differentiation on the cost functor
-  ceres::AutoDiffCostFunction<fuse_models::Unicycle2DStateCostFunctor, 8, 2, 1, 2, 1, 2, 2, 1, 2, 1,
-    2>
+  ceres::AutoDiffCostFunction<
+    fuse_models::Unicycle2DStateCostFunctor, 8, 2, 1, 2, 1, 2, 2, 1, 2, 1, 2
+  >
   cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information));
 
   for (auto _ : state) {

fuse_models/include/fuse_models/acceleration_2d.hpp

@@ -34,6 +34,12 @@
 #ifndef FUSE_MODELS__ACCELERATION_2D_HPP_
 #define FUSE_MODELS__ACCELERATION_2D_HPP_
 
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <string>
+
 #include <fuse_models/parameters/acceleration_2d_params.hpp>
 
 #include <fuse_core/async_sensor_model.hpp>
@@ -42,8 +48,6 @@
 
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
 
 
 namespace fuse_models

fuse_models/include/fuse_models/common/sensor_config.hpp

@@ -34,6 +34,12 @@
 #ifndef FUSE_MODELS__COMMON__SENSOR_CONFIG_HPP_
 #define FUSE_MODELS__COMMON__SENSOR_CONFIG_HPP_
 
+#include <algorithm>
+#include <functional>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
 #include <fuse_models/common/variable_traits.hpp>
 
 #include <boost/algorithm/string/case_conv.hpp>
@@ -44,12 +50,6 @@
 #include <fuse_variables/velocity_linear_2d_stamped.hpp>
 #include <rclcpp/logging.hpp>
 
-#include <algorithm>
-#include <functional>
-#include <stdexcept>
-#include <string>
-#include <vector>
-
 
 namespace fuse_models
 {

fuse_models/include/fuse_models/common/sensor_proc.hpp

@@ -34,6 +34,17 @@
 #ifndef FUSE_MODELS__COMMON__SENSOR_PROC_HPP_
 #define FUSE_MODELS__COMMON__SENSOR_PROC_HPP_
 
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/LinearMath/Vector3.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <algorithm>
+#include <functional>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
 #include <fuse_constraints/absolute_pose_2d_stamped_constraint.hpp>
 #include <fuse_constraints/relative_pose_2d_stamped_constraint.hpp>
 #include <fuse_constraints/absolute_constraint.hpp>
@@ -54,21 +65,12 @@
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <rclcpp/clock.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <tf2/LinearMath/Transform.h>
-#include <tf2/LinearMath/Vector3.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
 #include <tf2_2d/tf2_2d.hpp>
 #include <tf2_2d/transform.hpp>
 
 #include <boost/range/join.hpp>
 
-#include <algorithm>
-#include <functional>
-#include <stdexcept>
-#include <string>
-#include <vector>
 
 static auto sensor_proc_clock = rclcpp::Clock();
 
@@ -532,8 +534,8 @@ inline bool processDifferentialPoseWithCovariance(
     //
     // C2 = J_p1 * C1 * J_p1^T + J_p12 * C12 * J_p12^T
     //
-    // where C1, C2, C12 are the covariance matrices of p1, p2 and dp, respectively, and J_p1 and J_p12 are the
-    // jacobians of the equation wrt p1 and p12, respectively.
+    // where C1, C2, C12 are the covariance matrices of p1, p2 and dp, respectively, and J_p1 and
+    // J_p12 are the jacobians of the equation wrt p1 and p12, respectively.
     //
     // Therefore, the covariance C12 of the relative pose p12 is:
     //
@@ -603,15 +605,15 @@ inline bool processDifferentialPoseWithCovariance(
     //
     //
     //
-    // At this point we could go one step further since p12 = t12 * dt and include the jacobian of this additional
-    // equation:
+    // At this point we could go one step further since p12 = t12 * dt and include the jacobian of
+    // this additional equation:
     //
     // J_t12 = dt * Id
     //
     // where Id is a 3x3 identity matrix.
     //
-    // However, that would give us the covariance of the twist t12, and here we simply need the one of the relative
-    // pose p12.
+    // However, that would give us the covariance of the twist t12, and here we simply need the one
+    // of the relative pose p12.
     //
     //
     //
@@ -642,8 +644,9 @@ inline bool processDifferentialPoseWithCovariance(
     //
     //
     //
-    // Note that the covariance propagation expression derived here for dependent pose measurements gives more accurate
-    // results than simply changing the sign in the expression for independent pose measurements, which would be:
+    // Note that the covariance propagation expression derived here for dependent pose measurements
+    // gives more accurate results than simply changing the sign in the expression for independent
+    // pose measurements, which would be:
     //
     // C12 = J_p2 * C2 * J_p2^T - J_p1 * C1 * J_p1^T
     //
@@ -657,21 +660,23 @@ inline bool processDifferentialPoseWithCovariance(
     // J_p2 = (------) = (sin(yaw1)  cos(yaw1) 0)
     //        ( 0 | 1)   (        0          0 1)
     //
-    // which are the j_pose1 and j_pose2 jacobians used above for the covariance propagation expresion for independent
-    // pose measurements.
+    // which are the j_pose1 and j_pose2 jacobians used above for the covariance propagation
+    // expression for independent pose measurements.
     //
-    // This seems to be the approach adviced in https://github.com/cra-ros-pkg/robot_localization/issues/356, but after
-    // comparing the resulting relative pose covariance C12 and the twist covariance, we can conclude that the approach
-    // proposed here is the only one that allow us to get results that match.
+    // This seems to be the approach adviced in
+    // https://github.com/cra-ros-pkg/robot_localization/issues/356, but after comparing the
+    // resulting relative pose covariance C12 and the twist covariance, we can conclude that the
+    // approach proposed here is the only one that allow us to get results that match.
     //
     // The relative pose covariance C12 and the twist covariance T12 can be compared with:
     //
     // T12 = J_t12 * C12 * J_t12^T
     //
     //
     //
-    // In some cases the difference between the C1 and C2 covariance matrices is very small and it could yield to an
-    // ill-conditioned C12 covariance. For that reason a minimum covariance is added to [2].
+    // In some cases the difference between the C1 and C2 covariance matrices is very small and it
+    // could yield to an ill-conditioned C12 covariance. For that reason a minimum covariance is
+    //    added to [2].
     fuse_core::Matrix3d j_pose1;
     /* *INDENT-OFF* */
     j_pose1 << 1, 0, sy * pose_relative_mean(0) - cy * pose_relative_mean(1),

fuse_models/include/fuse_models/graph_ignition.hpp

@@ -35,6 +35,10 @@
 #ifndef FUSE_MODELS__GRAPH_IGNITION_HPP_
 #define FUSE_MODELS__GRAPH_IGNITION_HPP_
 
+#include <atomic>
+#include <memory>
+#include <string>
+
 #include <fuse_msgs/srv/set_graph.hpp>
 #include <fuse_models/parameters/graph_ignition_params.hpp>
 
@@ -46,7 +50,6 @@
 #include <rclcpp/rclcpp.hpp>
 #include <std_srvs/srv/empty.hpp>
 
-#include <atomic>
 
 namespace fuse_models
 {
@@ -173,7 +176,8 @@ class GraphIgnition : public fuse_core::AsyncSensorModel
 
   ParameterType params_;  //!< Object containing all of the configuration parameters
 
-  rclcpp::Client<std_srvs::srv::Empty>::SharedPtr reset_client_;  //!< Service client used to call the "reset" service on the optimizer
+  //!< Service client used to call the "reset" service on the optimizer
+  rclcpp::Client<std_srvs::srv::Empty>::SharedPtr reset_client_;
   rclcpp::Service<fuse_msgs::srv::SetGraph>::SharedPtr set_graph_service_;
   rclcpp::Subscription<fuse_msgs::msg::SerializedGraph>::SharedPtr sub_;
 

fuse_models/include/fuse_models/imu_2d.hpp

@@ -34,6 +34,12 @@
 #ifndef FUSE_MODELS__IMU_2D_HPP_
 #define FUSE_MODELS__IMU_2D_HPP_
 
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <string>
+
 #include <fuse_models/parameters/imu_2d_params.hpp>
 #include <fuse_core/throttled_callback.hpp>
 
@@ -43,10 +49,6 @@
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/imu.hpp>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
-
-#include <memory>
 
 
 namespace fuse_models

fuse_models/include/fuse_models/odometry_2d.hpp

@@ -34,6 +34,12 @@
 #ifndef FUSE_MODELS__ODOMETRY_2D_HPP_
 #define FUSE_MODELS__ODOMETRY_2D_HPP_
 
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <string>
+
 #include <fuse_models/parameters/odometry_2d_params.hpp>
 #include <fuse_core/throttled_callback.hpp>
 
@@ -43,10 +49,6 @@
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
-
-#include <memory>
 
 
 namespace fuse_models

fuse_models/include/fuse_models/odometry_2d_publisher.hpp

@@ -34,6 +34,14 @@
 #ifndef FUSE_MODELS__ODOMETRY_2D_PUBLISHER_HPP_
 #define FUSE_MODELS__ODOMETRY_2D_PUBLISHER_HPP_
 
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <memory>
+#include <mutex>
+#include <string>
+
 #include <fuse_models/parameters/odometry_2d_publisher_params.hpp>
 
 #include <fuse_core/async_publisher.hpp>
@@ -47,13 +55,7 @@
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
 
-#include <memory>
-#include <mutex>
-#include <string>
 
 namespace fuse_models
 {
@@ -191,11 +193,12 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher
   /**
    * @brief Object that searches for the most recent common timestamp for a set of variables
    */
-  using Synchronizer = fuse_publishers::StampedVariableSynchronizer<fuse_variables::Orientation2DStamped,
-      fuse_variables::Position2DStamped,
-      fuse_variables::VelocityLinear2DStamped,
-      fuse_variables::VelocityAngular2DStamped,
-      fuse_variables::AccelerationLinear2DStamped>;
+  using Synchronizer = fuse_publishers::StampedVariableSynchronizer<
+    fuse_variables::Orientation2DStamped,
+    fuse_variables::Position2DStamped,
+    fuse_variables::VelocityLinear2DStamped,
+    fuse_variables::VelocityAngular2DStamped,
+    fuse_variables::AccelerationLinear2DStamped>;
 
   fuse_core::node_interfaces::NodeInterfaces<
     fuse_core::node_interfaces::Base,
@@ -215,11 +218,12 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher
 
   rclcpp::Time latest_stamp_;
   rclcpp::Time latest_covariance_stamp_;
-  bool latest_covariance_valid_{false};    //!< Whether the latest covariance computed is valid or not
+  bool latest_covariance_valid_{false};  //!< Whether the latest covariance computed is valid or not
   nav_msgs::msg::Odometry odom_output_;
   geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output_;
 
-  Synchronizer synchronizer_;  //!< Object that tracks the latest common timestamp of multiple variables
+  //!< Object that tracks the latest common timestamp of multiple variables
+  Synchronizer synchronizer_;
 
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
 
@@ -229,16 +233,17 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher
   std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ = nullptr;
   std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 
-  fuse_core::DelayedThrottleFilter delayed_throttle_filter_{10.0};    //!< A ros::console filter to print delayed
-                                                                      //!< throttle messages, that
-                                                                      //!< can be reset on start
+  fuse_core::DelayedThrottleFilter delayed_throttle_filter_{10.0};  //!< A ros::console filter to
+                                                                    //!< print delayed throttle
+                                                                    //!< messages, that can be reset
+                                                                    //!< on start
 
   rclcpp::TimerBase::SharedPtr publish_timer_;
 
-  std::mutex mutex_;  //!< A mutex to protect the access to the attributes used concurrently by the notifyCallback and
-                      //!< publishTimerCallback methods:
-                      //!< latest_stamp_, latest_covariance_stamp_, odom_output_ and
-                      //!< acceleration_output_
+  std::mutex mutex_;  //!< A mutex to protect the access to the attributes used concurrently by the
+                      //!< notifyCallback and publishTimerCallback methods:
+                      //!<   latest_stamp_, latest_covariance_stamp_, odom_output_ and
+                      //!<   acceleration_output_
 };
 
 }  // namespace fuse_models

fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp

@@ -34,14 +34,14 @@
 #ifndef FUSE_MODELS__PARAMETERS__ACCELERATION_2D_PARAMS_HPP_
 #define FUSE_MODELS__PARAMETERS__ACCELERATION_2D_PARAMS_HPP_
 
+#include <string>
+#include <vector>
+
 #include <fuse_core/loss.hpp>
 #include <fuse_core/parameter.hpp>
 #include <fuse_variables/acceleration_linear_2d_stamped.hpp>
 #include <fuse_models/parameters/parameter_base.hpp>
 
-#include <string>
-#include <vector>
-
 
 namespace fuse_models
 {
@@ -115,8 +115,9 @@ struct Acceleration2DParams : public ParameterBase
 
   bool disable_checks {false};
   int queue_size {10};
-  rclcpp::Duration tf_timeout {0, 0};      //!< The maximum time to wait for a transform to become available
-  rclcpp::Duration throttle_period {0, 0};      //!< The throttle period duration in seconds
+  rclcpp::Duration tf_timeout {0, 0};  //!< The maximum time to wait for a transform to become
+                                       //!< available
+  rclcpp::Duration throttle_period {0, 0};  //!< The throttle period duration in seconds
   bool throttle_use_wall_time {false};      //!< Whether to throttle using ros::WallTime or not
   std::string topic {};
   std::string target_frame {};

fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp

@@ -35,9 +35,10 @@
 #ifndef FUSE_MODELS__PARAMETERS__GRAPH_IGNITION_PARAMS_HPP_
 #define FUSE_MODELS__PARAMETERS__GRAPH_IGNITION_PARAMS_HPP_
 
+#include <string>
+
 #include <fuse_models/parameters/parameter_base.hpp>
 
-#include <string>
 
 namespace fuse_models
 {