Fix clang-tidy in CI, add further documentation

.clang-format

@@ -15,6 +15,7 @@ NamespaceIndentation: None
 ContinuationIndentWidth: 4
 IndentCaseLabels: true
 IndentFunctionDeclarationAfterType: false
+QualifierAlignment: Right
 
 AlignEscapedNewlinesLeft: false
 AlignTrailingComments: true

.github/workflows/ci.yaml

@@ -19,7 +19,7 @@ permissions:
 jobs:
   build-ws:
     name: Build colcon workspace
-    runs-on: ubuntu-24.04
+    runs-on: ubuntu-22.04
     steps:
       - name: Checkout source
         uses: actions/checkout@v4
@@ -54,7 +54,7 @@ jobs:
     needs:
       # Ensure the test job runs after the build job finishes instead of attempting to run in parallel
       - build-ws
-    runs-on: ubuntu-24.04
+    runs-on: ubuntu-22.04
     container:
       # Run on the Docker image we tagged and pushed to a private repo in the job above
       image: ghcr.io/picknikrobotics/fuse:${{ github.run_id }}
@@ -79,7 +79,7 @@ jobs:
       # Ensure the test job runs after the build job finishes instead of attempting to run in parallel
       - build-ws
     name: clang-tidy
-    runs-on: ubuntu-24.04
+    runs-on: ubuntu-22.04
     container:
       # Run on the Docker image we tagged and pushed to a private repo in the job above
       image: ghcr.io/picknikrobotics/fuse:${{ github.run_id }}
@@ -93,7 +93,8 @@ jobs:
             **.cpp
             **.hpp
       - if: ${{ steps.changed-cpp-files.outputs.all_changed_files != '' }}
-        run: run-clang-tidy -j $(nproc --all) -p build/ -export-fixes clang-tidy-fixes.yaml -config "$(cat src/fuse/.clang-tidy)" ${{ steps.changed-cpp-files.outputs.all_changed_files }}
+        # https://stackoverflow.com/questions/48404289/using-clang-tidy-to-check-c17-code
+        run: run-clang-tidy -j $(nproc --all) -p build/ -export-fixes clang-tidy-fixes.yaml -config "$(cat src/fuse/.clang-tidy)" -extra-arg=-std=c++17 ${{ steps.changed-cpp-files.outputs.all_changed_files }}
         working-directory: /colcon_ws
       - uses: asarium/[email protected]
         with:

.github/workflows/pre-commit.yaml

@@ -12,14 +12,12 @@ on:
 jobs:
   pre-commit:
     name: Format
-    runs-on: ubuntu-24.04
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v4
       - uses: actions/setup-python@v5
         with:
           python-version: '3.10'
-      - name: Install clang-format-18
-        run: sudo apt-get install clang-format-18
       - uses: pre-commit/[email protected]
         id: precommit
       - name: Upload pre-commit changes

README.md

@@ -2,6 +2,8 @@
 
 Welcome to PickNik Robotics's fork of fuse!
 
+This branch is for ROS Humble.
+
 ## Getting Started
 
 Using the Dockerfile is the easiest way to get started. Once inside, simply `rosdep update` and `colcon build` and you're ready to get started! Try `ros2 launch fuse_tutorials fuse_3d_tutorial.launch.py` and look at its pertinent code for a crash course in fuse and its capabilities.
@@ -40,7 +42,7 @@ optimizer will cache the constraints and process them in small batches on some s
 require considerable processing time, introducing a delay between the completion of the optimization cycle and the
 publishing of data to the ROS topic.
 
-![fuse sequence diagram](doc/fuse_sequence_diagram.png)
+![fuse sequence diagram](doc/images/fuse_sequence_diagram.png)
 
 ## Example
 
@@ -74,14 +76,14 @@ time. This is enough to construct our first `fuse` system. Below is the constrai
 system. The large circles represent state variables at a given time, while the small squares represent measurements.
 The graph connectivity indicates which variables are involved in what measurements.
 
-![fuse graph](doc/fuse_graph_1.png)
+![fuse graph](doc/images/fuse_graph_1.png)
 
 The two sensor models are configured as plugins to an optimizer implementation. The optimizer performs the required
 computation to generate the optimal state variable values based on the provided sensor constraints. We will never be
 able to exactly satisfy both the wheel encoder constraints and the laserscan constraints. Instead we minimize the error
 of all the constraints using nonlinear least squares optimization.
 
-![fuse optimizer](doc/fuse_optimizer_1.png)
+![fuse optimizer](doc/images/fuse_optimizer_1.png)
 
 While our `fuse` system is optimizing constraints from two different sensors, it is not yet publishing any data back
 out to ROS. In order to publish data to ROS, we derive a `fuse_core::Publisher` class and add it to the
@@ -90,7 +92,7 @@ implementation determines what type of messages are published and at what freque
 we would like visualize the current pose of the robot in RViz, so we create a `fuse` publisher that finds the most
 recent pose and converts it into a `geometry_msgs::msg::PoseStamped` message, then publishes the message to a topic.
 
-![fuse optimizer](doc/fuse_optimizer_2.png)
+![fuse optimizer](doc/images/fuse_optimizer_2.png)
 
 We finally have something that is starting to be useful.
 
@@ -100,38 +102,38 @@ Typically the laser measurements and the wheel encoder measurements are not sync
 sampled faster than the laser, and are sampled at different times using a different clock. If we do not do anything
 different in this situation, the constraint graph becomes disconnected.
 
-![fuse graph](doc/fuse_graph_2.png)
+![fuse graph](doc/images/fuse_graph_2.png)
 
 This is where motion models come into play. A motion model differs from a sensor model in that constraints can be
 generated between any two requested timestamps. Motion model constraints are generated upon request, not due to their
 own internal clock. We use the motion model to connect the states introduced by the other sensor measurements. We
 derive a class from the `fuse_core::MotionModel` base class and implement a differential drive kinematic
 constraint for our robot.
 
-![fuse optimizer](doc/fuse_optimizer_3.png)
+![fuse optimizer](doc/images/fuse_optimizer_3.png)
 
 The motion models are also configured as plugins to the optimizer. The optimizer requests motion models constraints
 from the configured plugins whenever new states are created by the sensor models.
 
-![fuse graph](doc/fuse_graph_3.png)
+![fuse graph](doc/images/fuse_graph_3.png)
 
 ### Adaptation #2: Full path publishing
 
 Nothing about the `fuse` framework limits you to having a single publisher. What if you want to visualize the entire
 robot trajectory, instead of just the most recent pose? Well, we can create a new derived `fuse_core::Publisher` class
 that publishes all of the robot poses using a `nav_msgs::msg::Path` message.
 
-![fuse optimizer](doc/fuse_optimizer_4.png)
+![fuse optimizer](doc/images/fuse_optimizer_4.png)
 
 ### Adaptation #3: Changing kinematics
 
 In your spare time, you also build [autonomous power wheels racers](http://www.powerracingseries.org/). But race cars
 don't use differential drive; you need a different motion model. Easy enough. We simply derive a new
 `fuse_core::MotionModel` class that implements an Ackermann steering model. Everything else can be reused.
 
-![fuse optimizer](doc/fuse_optimizer_5.png)
+![fuse optimizer](doc/images/fuse_optimizer_5.png)
 
-![fuse graph](doc/fuse_graph_4.png)
+![fuse graph](doc/images/fuse_graph_4.png)
 
 ### Adaptation #4: Online calibration
 
@@ -146,9 +148,9 @@ how the wheel diameter is expected to change over time. Maybe some sort of expon
 derive a new publisher plugin from `fuse_core::Publisher` that publishes the current wheel diameter. This allows us to
 plot how the wheel diameter changes over the length of the race.
 
-![fuse optimizer](doc/fuse_optimizer_6.png)
+![fuse optimizer](doc/images/fuse_optimizer_6.png)
 
-![fuse graph](doc/fuse_graph_5.png)
+![fuse graph](doc/images/fuse_graph_5.png)
 
 Now our system estimates the wheel diameters at each time step as well as the robot's pose.
 
@@ -167,11 +169,27 @@ end users to concentrate on modeling the robot, sensor, system, etc. and spend l
 sensor models together into runable code. And since all of the models are implemented as plugins, separate plugin
 libraries can be shared or kept private at the discretion of their authors.
 
-## API Concepts
+## Documentation
+
+### API Concepts
 
-* [Variables](doc/Variables.md)
-* [Constraints](doc/Constraints.md)
+* Optimizers -- coming soon
+* [Variables](doc/concepts/Variables.md)
+* Graphs -- coming soon
 * Sensor Models -- coming soon
 * Motion Models -- coming soon
 * Publishers -- coming soon
-* Optimizers -- coming soon
+* [Constraints](doc/concepts/Constraints.md)
+
+### Implementation Documentation
+
+This is documentation of specific implementations of the above concepts.
+
+Documentation in progress, see [this issue](https://github.com/PickNikRobotics/fuse/issues/23).
+
+* [Optimizers](./fuse_optimizers/doc/optimizers.md)
+* [Variables](./fuse_variables/doc/variables.md)
+* [Graphs](./fuse_graphs/doc/graphs.md)
+* [Motion Models](./fuse_models/doc/motion_models/motion_models.md)
+* [Sensor Models](./fuse_models/doc/sensor_models/sensor_models.md)
+* [Constraints](./fuse_constraints/doc/constraints.md)

doc/Variables.md → doc/concepts/Variables.md

@@ -17,16 +17,16 @@ the system. There are several reasons multiple identities of a Variable will be
 
 * The Variable may represent a property common to multiple entity types. Both a robot and a visual landmark have a
   position in space. Different identities of the same Variable may be used to describe these different entities.
-  ![common property](variables-common_property.png)
+  ![common property](../images/variables-common_property.png)
 * Similarly, there may be multiple occurrences of the same entity type within the system. A multi-robot configuration
   will want to track the pose of each robot, and thus different identities of a Variable will be used for each robot
   in the system.
-  ![multiple occurrences](variables-multiple_occurrences.png)
+  ![multiple occurrences](../images/variables-multiple_occurrences.png)
 * Most commonly, a Variable will represent a time-varying process. A different identity will be required for each
   time instant for which the process value is to be estimated. For example, the pose of the robot will change over
   time, so we need a unique identity representing the robot pose at time `t1` **and** another unique identity
   representing the robot pose at time `t2`. Any time-varying process must be discretized within the fuse stack.
-  ![time series](variables-time_series.png)
+  ![time series](../images/variables-time_series.png)
 
 The identity takes the form of a UUID or hash, and is generally derived from a set of additional properties that
 describe what makes each occurrence unique from other occurrences. In the case of a time-varying process, this will

fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp

@@ -57,12 +57,12 @@ class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture
   static const fuse_core::Matrix3d sqrt_information;
 
   // Parameters
-  static const double* parameters[];
+  static double const* parameters[];
 
   // Residuals
   fuse_core::Vector3d residuals;
 
-  static const std::vector<int32_t>& block_sizes;
+  static std::vector<int32_t> const& block_sizes;
   static const size_t num_parameter_blocks;
 
   static const size_t num_residuals;
@@ -72,40 +72,40 @@ class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture
 
 private:
   // Cost function covariance
-  static const double covariance_diagonal[];
+  static double const covariance_diagonal[];
 
   static const fuse_core::Matrix3d covariance;
 
   // Parameter blocks
-  static const double position1[];
-  static const double orientation1[];
-  static const double position2[];
-  static const double orientation2[];
+  static double const position1[];
+  static double const orientation1[];
+  static double const position2[];
+  static double const orientation2[];
 
   // Jacobian matrices
   std::vector<fuse_core::MatrixXd> J;
 };
 
 // Cost function covariance
-const double NormalDeltaPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 };
+double const NormalDeltaPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 };
 
 const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::covariance =
     fuse_core::Vector3d(covariance_diagonal).asDiagonal();
 
 // Parameter blocks
-const double NormalDeltaPose2DBenchmarkFixture::position1[] = { 0.0, 1.0 };
-const double NormalDeltaPose2DBenchmarkFixture::orientation1[] = { 0.5 };
-const double NormalDeltaPose2DBenchmarkFixture::position2[] = { 2.0, 3.0 };
-const double NormalDeltaPose2DBenchmarkFixture::orientation2[] = { 1.5 };
+double const NormalDeltaPose2DBenchmarkFixture::position1[] = { 0.0, 1.0 };
+double const NormalDeltaPose2DBenchmarkFixture::orientation1[] = { 0.5 };
+double const NormalDeltaPose2DBenchmarkFixture::position2[] = { 2.0, 3.0 };
+double const NormalDeltaPose2DBenchmarkFixture::orientation2[] = { 1.5 };
 
 // Delta and sqrt information matrix
 const fuse_core::Vector3d NormalDeltaPose2DBenchmarkFixture::delta{ 1.0, 2.0, 3.0 };
 const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::sqrt_information(covariance.inverse().llt().matrixU());
 
 // Parameters
-const double* NormalDeltaPose2DBenchmarkFixture::parameters[] = { position1, orientation1, position2, orientation2 };
+double const* NormalDeltaPose2DBenchmarkFixture::parameters[] = { position1, orientation1, position2, orientation2 };
 
-const std::vector<int32_t>& NormalDeltaPose2DBenchmarkFixture::block_sizes = { 2, 1, 2, 1 };
+std::vector<int32_t> const& NormalDeltaPose2DBenchmarkFixture::block_sizes = { 2, 1, 2, 1 };
 const size_t NormalDeltaPose2DBenchmarkFixture::num_parameter_blocks = block_sizes.size();
 
 const size_t NormalDeltaPose2DBenchmarkFixture::num_residuals = 3;
@@ -126,7 +126,7 @@ BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2
 BENCHMARK_DEFINE_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)(benchmark::State& state)
 {
   // Create cost function using automatic differentiation on the cost functor
-  const auto partial_sqrt_information = sqrt_information.topRows(state.range(0));
+  auto const partial_sqrt_information = sqrt_information.topRows(state.range(0));
   const ceres::AutoDiffCostFunction<fuse_constraints::NormalDeltaPose2DCostFunctor, ceres::DYNAMIC, 2, 1, 2, 1>
       cost_function_autodiff(new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, delta),
                              partial_sqrt_information.rows());

fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp

@@ -57,12 +57,12 @@ class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture
   static const fuse_core::Matrix3d sqrt_information;
 
   // Parameters
-  static const double* parameters[];
+  static double const* parameters[];
 
   // Residuals
   fuse_core::Vector3d residuals;
 
-  static const std::vector<int32_t>& block_sizes;
+  static std::vector<int32_t> const& block_sizes;
   static const size_t num_parameter_blocks;
 
   static const size_t num_residuals;
@@ -72,36 +72,36 @@ class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture
 
 private:
   // Cost function covariance
-  static const double covariance_diagonal[];
+  static double const covariance_diagonal[];
 
   static const fuse_core::Matrix3d covariance;
 
   // Parameter blocks
-  static const double position[];
-  static const double orientation[];
+  static double const position[];
+  static double const orientation[];
 
   // Jacobian matrices
   std::vector<fuse_core::MatrixXd> J;
 };
 
 // Cost function covariance
-const double NormalPriorPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 };
+double const NormalPriorPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 };
 
 const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::covariance =
     fuse_core::Vector3d(covariance_diagonal).asDiagonal();
 
 // Parameter blocks
-const double NormalPriorPose2DBenchmarkFixture::position[] = { 0.0, 0.0 };
-const double NormalPriorPose2DBenchmarkFixture::orientation[] = { 0.0 };
+double const NormalPriorPose2DBenchmarkFixture::position[] = { 0.0, 0.0 };
+double const NormalPriorPose2DBenchmarkFixture::orientation[] = { 0.0 };
 
 // Mean and sqrt information matrix
 const fuse_core::Vector3d NormalPriorPose2DBenchmarkFixture::mean{ 1.0, 2.0, 3.0 };
 const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::sqrt_information(covariance.inverse().llt().matrixU());
 
 // Parameters
-const double* NormalPriorPose2DBenchmarkFixture::parameters[] = { position, orientation };
+double const* NormalPriorPose2DBenchmarkFixture::parameters[] = { position, orientation };
 
-const std::vector<int32_t>& NormalPriorPose2DBenchmarkFixture::block_sizes = { 2, 1 };
+std::vector<int32_t> const& NormalPriorPose2DBenchmarkFixture::block_sizes = { 2, 1 };
 const size_t NormalPriorPose2DBenchmarkFixture::num_parameter_blocks = block_sizes.size();
 
 const size_t NormalPriorPose2DBenchmarkFixture::num_residuals = 3;
@@ -122,7 +122,7 @@ BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2
 BENCHMARK_DEFINE_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)(benchmark::State& state)
 {
   // Create cost function using automatic differentiation on the cost functor
-  const auto partial_sqrt_information = sqrt_information.topRows(state.range(0));
+  auto const partial_sqrt_information = sqrt_information.topRows(state.range(0));
   const ceres::AutoDiffCostFunction<fuse_constraints::NormalPriorPose2DCostFunctor, ceres::DYNAMIC, 2, 1>
       cost_function_autodiff(new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, mean),
                              partial_sqrt_information.rows());

fuse_constraints/doc/constraints.md

@@ -0,0 +1,3 @@
+# Constraints
+
+[Coming soon](https://github.com/PickNikRobotics/fuse/issues/23)