Merge 3d models

.clang-tidy

@@ -12,8 +12,11 @@ Checks: -*,
   -bugprone-easily-swappable-parameters,
   -bugprone-exception-escape,
   -cppcoreguidelines-avoid-magic-numbers,
+  -cppcoreguidelines-non-private-member-variables-in-classes,
   -google-readability-casting,
+  -misc-include-cleaner,
   -modernize-use-trailing-return-type,
+  -modernize-avoid-bind,
   -readability-identifier-length,
   -readability-function-cognitive-complexity,
   -readability-magic-numbers,

.github/workflows/ci.yaml

@@ -61,7 +61,7 @@ jobs:
         run: |
           . /opt/ros/rolling/setup.sh
           . /colcon_ws/install/local_setup.sh
-          colcon test --event-handlers console_direct+ --retest-until-pass 3
+          colcon test --event-handlers console_direct+ --packages-select-regex fuse*
         working-directory: /colcon_ws
 
       # `colcon test` does not actually error on failure - run `colcon test-result` to generate a summary and an error code.
@@ -72,6 +72,7 @@ jobs:
         working-directory: /colcon_ws
 
   clang_tidy:
+    if: github.ref != 'refs/heads/main'
     needs:
       # Ensure the test job runs after the build job finishes instead of attempting to run in parallel
       - build-ws

Dockerfile

@@ -9,6 +9,7 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
     apt-get update && apt-get upgrade -y && \
     apt-get install -y --no-install-recommends \
       clang-tidy \
+      python3-vcstool \
       # use cyclonedds instead of fastdds
       ros-rolling-rmw-cyclonedds-cpp
 
@@ -22,6 +23,7 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
     --mount=type=cache,target=/var/lib/apt,sharing=locked \
     apt-get update && apt-get upgrade -y && \
     . /opt/ros/rolling/setup.sh && \
+    vcs import src --input src/fuse/fuse.repos && \
     rosdep install --from-paths src -y --ignore-src && \
     colcon build --mixin compile-commands coverage-gcc coverage-pytest
 

doc/Constraints.md

@@ -34,7 +34,7 @@ by accepting, at a minimum, the UUID of each involved Variable.
 The derived Constraints provided by the [fuse_constraints](../fuse_constraints) package go one step further, and
 require instances of full Variable types rather than just the Variable's UUID. This is done solely to enforce
 type-safety; e.g. a specific Constraint must involve two
-[Position2DStamped](../fuse_variables/include/fuse_variables/position_2d_stamped.h) variables, no other variable types
+[Position2DStamped](../fuse_variables/include/fuse_variables/position_2d_stamped.hpp) variables, no other variable types
 are acceptable.
 
 ### Cost Function

fuse.repos

@@ -0,0 +1,9 @@
+repositories:
+  locusrobotics/tf2_2d:
+    type: git
+    url: https://github.com/locusrobotics/tf2_2d.git
+    version: rolling
+  covariance_geometry_ros:
+    type: git
+    url: https://github.com/giafranchini/covariance_geometry_ros.git
+    version: iron

fuse_constraints/CMakeLists.txt

@@ -116,7 +116,7 @@ include(suitesparse-extras.cmake)
 # plain_matrix_type<Src>::type tmp(src); | ^~~
 if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION
                                             VERSION_GREATER_EQUAL 12.0)
-  add_compile_options(-Wall -Werror -Wno-array-bounds)
+  add_compile_options(-Wall -Werror -Wno-array-bounds -Wno-stringop-overread)
 else()
   add_compile_options(-Wall -Werror)
 endif()
@@ -129,18 +129,23 @@ add_library(
   src/absolute_orientation_3d_stamped_euler_constraint.cpp
   src/absolute_pose_2d_stamped_constraint.cpp
   src/absolute_pose_3d_stamped_constraint.cpp
+  src/absolute_pose_3d_stamped_euler_constraint.cpp
   src/marginal_constraint.cpp
   src/marginal_cost_function.cpp
   src/marginalize_variables.cpp
   src/normal_delta.cpp
   src/normal_delta_orientation_2d.cpp
   src/normal_delta_pose_2d.cpp
   src/normal_prior_orientation_2d.cpp
+  src/normal_prior_orientation_3d.cpp
   src/normal_prior_pose_2d.cpp
+  src/normal_prior_pose_3d.cpp
+  src/normal_prior_pose_3d_euler.cpp
   src/relative_constraint.cpp
   src/relative_orientation_3d_stamped_constraint.cpp
   src/relative_pose_2d_stamped_constraint.cpp
   src/relative_pose_3d_stamped_constraint.cpp
+  src/relative_pose_3d_stamped_euler_constraint.cpp
   src/uuid_ordering.cpp
   src/variable_constraints.cpp)
 target_include_directories(

fuse_constraints/fuse_plugins.xml

@@ -5,12 +5,24 @@
     the 2D angular acceleration.
     </description>
   </class>
+  <class type="fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 3D angular acceleration, or a direct measurement of
+    the 3D angular acceleration.
+    </description>
+  </class>
   <class type="fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint" base_class_type="fuse_core::Constraint">
     <description>
     A constraint that represents either prior information about a 2D linear acceleration, or a direct measurement of
     the 2D linear acceleration.
     </description>
   </class>
+  <class type="fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 3D linear acceleration, or a direct measurement of
+    the 3D linear acceleration.
+    </description>
+  </class>
   <class type="fuse_constraints::AbsoluteOrientation2DStampedConstraint" base_class_type="fuse_core::Constraint">
     <description>
     A constraint that represents either prior information about a 2D orientation, or a direct measurement of the
@@ -35,12 +47,24 @@
     the 2D angular velocity.
     </description>
   </class>
+  <class type="fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 3D angular velocity, or a direct measurement of
+    the 3D angular velocity.
+    </description>
+  </class>
   <class type="fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint" base_class_type="fuse_core::Constraint">
     <description>
     A constraint that represents either prior information about a 2D linear velocity, or a direct measurement of
     the 2D linear velocity.
     </description>
   </class>
+  <class type="fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 3D linear velocity, or a direct measurement of
+    the 3D linear velocity.
+    </description>
+  </class>
   <class type="fuse_constraints::AbsoluteOrientation3DStampedConstraint" base_class_type="fuse_core::Constraint">
     <description>
     A constraint that represents either prior information about a 3D orientation, or a direct measurement of the
@@ -63,6 +87,12 @@
     A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose.
     </description>
   </class>
+  <class type="fuse_constraints::AbsolutePose3DStampedEulerConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose,
+    orientation parametrized in RPY.
+    </description>
+  </class>
   <class type="fuse_constraints::MarginalConstraint" base_class_type="fuse_core::Constraint">
     <description>
     A constraint that represents remaining marginal information on a set of variables.
@@ -118,4 +148,9 @@
     A constraint that represents a measurement on the difference between two 3D poses.
     </description>
   </class>
+  <class type="fuse_constraints::RelativePose3DStampedEulerConstraint" base_class_type="fuse_core::Constraint">
+    <description>
+    A constraint that represents a measurement on the difference between two 3D poses, orientation parametrized in RPY.
+    </description>
+  </class>
 </library>

fuse_constraints/include/fuse_constraints/absolute_constraint.hpp

@@ -46,11 +46,15 @@
 #include <fuse_core/serialization.hpp>
 #include <fuse_core/uuid.hpp>
 #include <fuse_variables/acceleration_angular_2d_stamped.hpp>
+#include <fuse_variables/acceleration_angular_3d_stamped.hpp>
 #include <fuse_variables/acceleration_linear_2d_stamped.hpp>
+#include <fuse_variables/acceleration_linear_3d_stamped.hpp>
 #include <fuse_variables/orientation_2d_stamped.hpp>
 #include <fuse_variables/position_2d_stamped.hpp>
 #include <fuse_variables/position_3d_stamped.hpp>
 #include <fuse_variables/velocity_angular_2d_stamped.hpp>
+#include <fuse_variables/velocity_angular_3d_stamped.hpp>
+#include <fuse_variables/velocity_linear_3d_stamped.hpp>
 #include <fuse_variables/velocity_linear_2d_stamped.hpp>
 
 #include <boost/serialization/access.hpp>
@@ -193,23 +197,31 @@ class AbsoluteConstraint : public fuse_core::Constraint
 
 // Define unique names for the different variations of the absolute constraint
 using AbsoluteAccelerationAngular2DStampedConstraint = AbsoluteConstraint<fuse_variables::AccelerationAngular2DStamped>;
+using AbsoluteAccelerationAngular3DStampedConstraint = AbsoluteConstraint<fuse_variables::AccelerationAngular3DStamped>;
 using AbsoluteAccelerationLinear2DStampedConstraint = AbsoluteConstraint<fuse_variables::AccelerationLinear2DStamped>;
+using AbsoluteAccelerationLinear3DStampedConstraint = AbsoluteConstraint<fuse_variables::AccelerationLinear3DStamped>;
 using AbsoluteOrientation2DStampedConstraint = AbsoluteConstraint<fuse_variables::Orientation2DStamped>;
 using AbsolutePosition2DStampedConstraint = AbsoluteConstraint<fuse_variables::Position2DStamped>;
 using AbsolutePosition3DStampedConstraint = AbsoluteConstraint<fuse_variables::Position3DStamped>;
 using AbsoluteVelocityAngular2DStampedConstraint = AbsoluteConstraint<fuse_variables::VelocityAngular2DStamped>;
+using AbsoluteVelocityAngular3DStampedConstraint = AbsoluteConstraint<fuse_variables::VelocityAngular3DStamped>;
 using AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint<fuse_variables::VelocityLinear2DStamped>;
+using AbsoluteVelocityLinear3DStampedConstraint = AbsoluteConstraint<fuse_variables::VelocityLinear3DStamped>;
 }  // namespace fuse_constraints
 
 // Include the template implementation
 #include <fuse_constraints/absolute_constraint_impl.hpp>
 
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint);
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint);
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteOrientation2DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePosition2DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePosition3DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint);
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint);
 BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint);
+BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint);
 
 #endif  // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_HPP_

fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp

@@ -149,6 +149,18 @@ inline std::string AbsoluteConstraint<fuse_variables::AccelerationLinear2DStampe
   return "fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint";
 }
 
+template <>
+inline std::string AbsoluteConstraint<fuse_variables::AccelerationAngular3DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint";
+}
+
+template <>
+inline std::string AbsoluteConstraint<fuse_variables::AccelerationLinear3DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint";
+}
+
 template <>
 inline std::string AbsoluteConstraint<fuse_variables::Orientation2DStamped>::type() const
 {
@@ -179,6 +191,18 @@ inline std::string AbsoluteConstraint<fuse_variables::VelocityLinear2DStamped>::
   return "fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint";
 }
 
+template <>
+inline std::string AbsoluteConstraint<fuse_variables::VelocityAngular3DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint";
+}
+
+template <>
+inline std::string AbsoluteConstraint<fuse_variables::VelocityLinear3DStamped>::type() const
+{
+  return "fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint";
+}
+
 }  // namespace fuse_constraints
 
 #endif  // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_IMPL_HPP_