fuse -> ROS 2 fuse_models: Linting

fuse_models/CMakeLists.txt

@@ -83,6 +83,13 @@ target_link_libraries(${PROJECT_NAME}
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+
+  # This is because these two files have unsuitable copyrights
+  set(_linter_excludes
+    test/test_sensor_proc.cpp
+    test/test_unicycle_2d.cpp
+  )
+  set(AMENT_LINT_AUTO_FILE_EXCLUDE ${_linter_excludes})
   ament_lint_auto_find_test_dependencies()
   add_subdirectory(test)
   add_subdirectory(benchmark)

fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp

@@ -31,51 +31,49 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#include <fuse_models/unicycle_2d_state_cost_function.h>
-#include <fuse_models/unicycle_2d_state_cost_functor.h>
-
 #include <benchmark/benchmark.h>
-
 #include <ceres/autodiff_cost_function.h>
 #include <Eigen/Dense>
 
 #include <vector>
 
+#include <fuse_models/unicycle_2d_state_cost_function.hpp>
+#include <fuse_models/unicycle_2d_state_cost_functor.hpp>
+
 class Unicycle2DStateCostFunction : public benchmark::Fixture
 {
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   Unicycle2DStateCostFunction()
-    : jacobians(num_parameter_blocks)
+  : jacobians(num_parameter_blocks)
     , J(num_parameter_blocks)
   {
-    for (size_t i = 0; i < num_parameter_blocks; ++i)
-    {
+    for (size_t i = 0; i < num_parameter_blocks; ++i) {
       J[i].resize(num_residuals, block_sizes[i]);
       jacobians[i] = J[i].data();
     }
   }
 
   // Analytic cost function
-  static constexpr double dt{ 0.1 };
+  static constexpr double dt{0.1};
   static const fuse_core::Matrix8d sqrt_information;
 
   static const fuse_models::Unicycle2DStateCostFunction cost_function;
 
   // Parameters
-  static const double* parameters[];
+  static const double * parameters[];
 
   // Residuals
   fuse_core::Vector8d residuals;
 
-  static const std::vector<int32_t>& block_sizes;
+  static const std::vector<int32_t> & block_sizes;
   static const size_t num_parameter_blocks;
 
   static const size_t num_residuals;
 
   // Jacobians
-  std::vector<double*> jacobians;
+  std::vector<double *> jacobians;
 
 private:
   // Cost function process noise and covariance
@@ -101,13 +99,15 @@ 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();
+  fuse_core::Vector8d(process_noise_diagonal).asDiagonal();
 
 // Parameter blocks
-const double Unicycle2DStateCostFunction::position1[] = { 0.0, 0.0 };
+const double Unicycle2DStateCostFunction::position1[] = {0.0, 0.0};
 const double Unicycle2DStateCostFunction::yaw1[] = {0.0};
 const double Unicycle2DStateCostFunction::vel_linear1[] = {1.0, 0.0};
 const double Unicycle2DStateCostFunction::vel_yaw1[] = {1.570796327};
@@ -120,36 +120,42 @@ const double Unicycle2DStateCostFunction::vel_yaw2[] = {1.570796327};
 const double Unicycle2DStateCostFunction::acc_linear2[] = {1.0, 0.0};
 
 // Analytic cost function
-const fuse_core::Matrix8d Unicycle2DStateCostFunction::sqrt_information(covariance.inverse().llt().matrixU());
+const fuse_core::Matrix8d Unicycle2DStateCostFunction::sqrt_information(covariance.inverse().llt().
+  matrixU());
 
-const fuse_models::Unicycle2DStateCostFunction Unicycle2DStateCostFunction::cost_function{ dt, sqrt_information };
+const fuse_models::Unicycle2DStateCostFunction Unicycle2DStateCostFunction::cost_function{dt,
+  sqrt_information};
 
 // Parameters
-const double* Unicycle2DStateCostFunction::parameters[] = {  // NOLINT(whitespace/braces)
-  position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, position2, yaw2, vel_linear2, vel_yaw2, acc_linear2
+const double * Unicycle2DStateCostFunction::parameters[] = {  // NOLINT(whitespace/braces)
+  position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, position2, yaw2, vel_linear2, vel_yaw2,
+  acc_linear2
 };
 
-const std::vector<int32_t>& Unicycle2DStateCostFunction::block_sizes = cost_function.parameter_block_sizes();
+const std::vector<int32_t> & Unicycle2DStateCostFunction::block_sizes =
+  cost_function.parameter_block_sizes();
 const size_t Unicycle2DStateCostFunction::num_parameter_blocks = block_sizes.size();
 
 const size_t Unicycle2DStateCostFunction::num_residuals = cost_function.num_residuals();
 
-BENCHMARK_F(Unicycle2DStateCostFunction, AnalyticUnicycle2DCostFunction)(benchmark::State& state)
+BENCHMARK_F(Unicycle2DStateCostFunction, AnalyticUnicycle2DCostFunction)(benchmark::State & state)
 {
-  for (auto _ : state)
-  {
+  for (auto _ : state) {
     cost_function.Evaluate(parameters, residuals.data(), jacobians.data());
   }
 }
 
-BENCHMARK_F(Unicycle2DStateCostFunction, AutoDiffUnicycle2DStateCostFunction)(benchmark::State& state)
+BENCHMARK_F(
+  Unicycle2DStateCostFunction,
+  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>
-      cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information));
+  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)
-  {
+  for (auto _ : state) {
     cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians.data());
   }
 }

fuse_models/include/fuse_models/acceleration_2d.h

@@ -1,7 +1,7 @@
 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2018, Locus Robotics
+ *  Copyright (c) 2022, Locus Robotics
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -31,118 +31,12 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef FUSE_MODELS_ACCELERATION_2D_H
-#define FUSE_MODELS_ACCELERATION_2D_H
 
-#include <fuse_models/parameters/acceleration_2d_params.h>
+#ifndef FUSE_MODELS__ACCELERATION_2D_H_
+#define FUSE_MODELS__ACCELERATION_2D_H_
 
-#include <fuse_core/async_sensor_model.hpp>
-#include <fuse_core/throttled_callback.hpp>
-#include <fuse_core/uuid.hpp>
+#warning This header is obsolete, please include fuse_models/acceleration_2d.hpp instead
 
-#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <tf2_ros/buffer.h>
-#include <tf2_ros/transform_listener.h>
+#include <fuse_models/acceleration_2d.hpp>
 
-
-namespace fuse_models
-{
-
-/**
- * @brief An adapter-type sensor that produces 2D linear acceleration constraints from information published by another
- * node
- *
- * This sensor subscribes to a geometry_msgs::msg::AccelWithCovarianceStamped topic and converts each received message
- * into a 2D linear acceleration variable and constraint.
- *
- * Parameters:
- *  - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to publish
- *  - device_name (string) Used to generate the device/robot ID if the device_id is not provided
- *  - queue_size (int, default: 10) The subscriber queue size for the twist messages
- *  - target_frame (string) The target frame_id to transform the data into before using it
- *  - topic (string) The topic to which to subscribe for the twist messages
- *
- * Subscribes:
- *  - \p topic (geometry_msgs::msg::AccelWithCovarianceStamped) Acceleration information at a given timestamp
- */
-class Acceleration2D : public fuse_core::AsyncSensorModel
-{
-public:
-  FUSE_SMART_PTR_DEFINITIONS(Acceleration2D)
-  using ParameterType = parameters::Acceleration2DParams;
-
-  /**
-   * @brief Default constructor
-   */
-  Acceleration2D();
-
-  /**
-   * @brief Destructor
-   */
-  virtual ~Acceleration2D() = default;
-
-  /**
-   * @brief Shadowing extension to the AsyncSensorModel::initialize call
-   */
-  void initialize(
-    fuse_core::node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces,
-    const std::string & name,
-    fuse_core::TransactionCallback transaction_callback) override;
-
-  /**
-   * @brief Callback for acceleration messages
-   * @param[in] msg - The acceleration message to process
-   */
-  void process(const geometry_msgs::msg::AccelWithCovarianceStamped& msg);
-
-protected:
-  fuse_core::UUID device_id_;  //!< The UUID of this device
-
-  /**
-   * @brief Perform any required initialization for the sensor model
-   *
-   * This could include things like reading from the parameter server or subscribing to topics. The class's node
-   * handles will be properly initialized before SensorModel::onInit() is called. Spinning of the callback queue will
-   * not begin until after the call to SensorModel::onInit() completes.
-   */
-  void onInit() override;
-
-  /**
-   * @brief Subscribe to the input topic to start sending transactions to the optimizer
-   */
-  void onStart() override;
-
-  /**
-   * @brief Unsubscribe from the input topic to stop sending transactions to the optimizer
-   */
-  void onStop() override;
-
-  fuse_core::node_interfaces::NodeInterfaces<
-    fuse_core::node_interfaces::Base,
-    fuse_core::node_interfaces::Clock,
-    fuse_core::node_interfaces::Logging,
-    fuse_core::node_interfaces::Parameters,
-    fuse_core::node_interfaces::Topics,
-    fuse_core::node_interfaces::Waitables
-  > interfaces_;  //!< Shadows AsyncSensorModel interfaces_
-
-  rclcpp::Clock::SharedPtr clock_;  //!< The sensor model's clock, for timestamping and logging
-  rclcpp::Logger logger_;  //!< The sensor model's logger
-
-  ParameterType params_;
-
-  // NOTE(CH3): Unique ptr to defer till we have the node interfaces from initialize()
-  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
-  std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
-
-  rclcpp::Subscription<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr sub_;
-
-  using AccelerationThrottledCallback =
-    fuse_core::ThrottledMessageCallback<geometry_msgs::msg::AccelWithCovarianceStamped>;
-  AccelerationThrottledCallback throttled_callback_;
-};
-
-}  // namespace fuse_models
-
-#endif  // FUSE_MODELS_ACCELERATION_2D_H
+#endif  // FUSE_MODELS__ACCELERATION_2D_H_