From 84de71c24cbe4aa71d01cc1e6473aab78ae90ec8 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 29 Oct 2024 14:18:13 -0600 Subject: [PATCH] Add skeleton of mujoco motion models (#7) * clang tidy fixes on relevant files, update pre-commit * test quaternionToAngleAxis for very small rotations * add mujoco and skeleton of mujoco motion model * flesh out skeleton of mujoco motion model * remove erroneous package xml entry * ignore mujoco in dockerfile rosdep and add back to package xml * clang tidy fixes * more clang tidy fixes --------- Co-authored-by: giacomo --- .pre-commit-config.yaml | 2 +- Dockerfile | 2 +- fuse.repos | 4 + .../include/fuse_core/async_motion_model.hpp | 7 +- fuse_core/include/fuse_core/motion_model.hpp | 5 + fuse_core/include/fuse_core/util.hpp | 137 +++++++------- fuse_core/src/async_motion_model.cpp | 6 +- fuse_core/test/test_util.cpp | 170 +++++++++++------- fuse_models/CMakeLists.txt | 6 +- .../include/fuse_models/mujoco_model.hpp | 170 ++++++++++++++++++ .../fuse_models/omnidirectional_3d.hpp | 4 + fuse_models/package.xml | 1 + fuse_models/src/mujoco_model.cpp | 50 ++++++ fuse_models/src/omnidirectional_3d.cpp | 8 +- .../include/fuse_optimizers/optimizer.hpp | 10 +- fuse_optimizers/src/optimizer.cpp | 55 +++--- 16 files changed, 456 insertions(+), 181 deletions(-) create mode 100644 fuse_models/include/fuse_models/mujoco_model.hpp create mode 100644 fuse_models/src/mujoco_model.cpp diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1b5c22bcb..8e9d07d94 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -14,7 +14,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v3.4.0 + rev: v5.0.0 hooks: - id: check-ast - id: check-case-conflict diff --git a/Dockerfile b/Dockerfile index c815a1148..472ad5a29 100644 --- a/Dockerfile +++ b/Dockerfile @@ -24,7 +24,7 @@ RUN --mount=type=cache,target=/var/cache/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 && \ + rosdep install --from-paths src -y --ignore-src --skip-keys=mujoco && \ colcon build --mixin compile-commands coverage-gcc coverage-pytest # Set up final environment and entrypoint. diff --git a/fuse.repos b/fuse.repos index d569f4ec3..f00574a17 100644 --- a/fuse.repos +++ b/fuse.repos @@ -7,3 +7,7 @@ repositories: type: git url: https://github.com/giafranchini/covariance_geometry_ros.git version: iron + mujoco: + type: git + url: https://github.com/google-deepmind/mujoco.git + version: 3.2.4 diff --git a/fuse_core/include/fuse_core/async_motion_model.hpp b/fuse_core/include/fuse_core/async_motion_model.hpp index cdbb091a4..ac012f0ed 100644 --- a/fuse_core/include/fuse_core/async_motion_model.hpp +++ b/fuse_core/include/fuse_core/async_motion_model.hpp @@ -94,6 +94,10 @@ class AsyncMotionModel : public MotionModel * @brief Destructor */ virtual ~AsyncMotionModel(); + AsyncMotionModel(AsyncMotionModel const&) = delete; + AsyncMotionModel(AsyncMotionModel&&) = delete; + AsyncMotionModel& operator=(AsyncMotionModel&&) = delete; + AsyncMotionModel& operator=(AsyncMotionModel const&) = delete; /** * @brief Augment a transaction object such that all involved timestamps are connected by motion @@ -253,6 +257,7 @@ class AsyncMotionModel : public MotionModel * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed * whenever needed. */ + // NOLINTNEXTLINE virtual void onGraphUpdate(Graph::ConstSharedPtr /*graph*/) { } @@ -291,7 +296,7 @@ class AsyncMotionModel : public MotionModel private: //! Stop the internal executor thread (in order to use this class again it must be re-initialized) - void internal_stop(); + void internalStop(); }; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/motion_model.hpp b/fuse_core/include/fuse_core/motion_model.hpp index 7fdbc1746..20c60b160 100644 --- a/fuse_core/include/fuse_core/motion_model.hpp +++ b/fuse_core/include/fuse_core/motion_model.hpp @@ -58,6 +58,10 @@ class MotionModel * @brief Destructor */ virtual ~MotionModel() = default; + MotionModel(MotionModel const&) = default; + MotionModel(MotionModel&&) = default; + MotionModel& operator=(MotionModel&&) = default; + MotionModel& operator=(MotionModel const&) = default; /** * @brief Augment a transaction object such that all involved timestamps are connected by motion @@ -87,6 +91,7 @@ class MotionModel * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed * whenever needed. */ + // NOLINTNEXTLINE virtual void graphCallback(Graph::ConstSharedPtr /*graph*/) { } diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index d3f69ef07..0fe01c9df 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -65,10 +65,8 @@ static inline T getPitch(const T w, const T x, const T y, const T z) { return (sin_pitch >= T(0.0) ? T(1.0) : T(-1.0)) * T(M_PI / 2.0); } - else - { - return ceres::asin(sin_pitch); - } + + return ceres::asin(sin_pitch); } /** @@ -117,11 +115,11 @@ template void wrapAngle2D(T& angle) { // Define some necessary variations of PI with the correct type (double or Jet) - static const T PI = T(M_PI); - static const T TAU = T(2 * M_PI); + static const T pi = T(M_PI); + static const T tau = T(2 * M_PI); // Handle the 1*Tau roll-over (https://tauday.com/tau-manifesto) // Use ceres::floor because it is specialized for double and Jet types. - angle -= TAU * ceres::floor((angle + PI) / TAU); + angle -= tau * ceres::floor((angle + pi) / tau); } /** @@ -167,7 +165,7 @@ static inline void quaternion2rpy(const double* q, double* rpy, double* jacobian rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]); rpy[2] = fuse_core::getYaw(q[0], q[1], q[2], q[3]); - if (jacobian) + if (jacobian != nullptr) { Eigen::Map> jacobian_map(jacobian); const double qw = q[0]; @@ -185,7 +183,7 @@ static inline void quaternion2rpy(const double* q, double* rpy, double* jacobian jacobian_map(2, 1) = -2.0 / (qw * ((qx * qx / qw * qw) + 1.0)); return; } - else if (discr < -gl_limit) + if (discr < -gl_limit) { // pitch = -90 deg jacobian_map.setZero(); @@ -193,52 +191,46 @@ static inline void quaternion2rpy(const double* q, double* rpy, double* jacobian jacobian_map(2, 1) = 2.0 / (qw * ((qx * qx / qw * qw) + 1.0)); return; } - else - { - // Non-degenerate case: - jacobian_map(0, 0) = - -(2.0 * qx) / - ((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + - 1.0) * - (2.0 * qx * qx + 2.0 * qy * qy - 1.0)); - jacobian_map(0, 1) = - -((2.0 * qw) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) - - (4.0 * qx * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) / - (std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0); - jacobian_map(0, 2) = - -((2.0 * qz) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) - - (4.0 * qy * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) / - (std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0); - jacobian_map(0, 3) = - -(2.0 * qy) / - ((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + - 1.0) * - (2.0 * qx * qx + 2.0 * qy * qy - 1.0)); - jacobian_map(1, 0) = (2.0 * qy) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); - jacobian_map(1, 1) = -(2.0 * qz) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); - jacobian_map(1, 2) = (2.0 * qw) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); - jacobian_map(1, 3) = -(2.0 * qx) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + // Non-degenerate case: + jacobian_map(0, 0) = + -(2.0 * qx) / + ((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0) * + (2.0 * qx * qx + 2.0 * qy * qy - 1.0)); + jacobian_map(0, 1) = + -((2.0 * qw) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) - + (4.0 * qx * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) / + (std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0); + jacobian_map(0, 2) = + -((2.0 * qz) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) - + (4.0 * qy * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) / + (std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0); + jacobian_map(0, 3) = + -(2.0 * qy) / + ((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0) * + (2.0 * qx * qx + 2.0 * qy * qy - 1.0)); - jacobian_map(2, 0) = - -(2.0 * qz) / - ((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + - 1.0) * - (2.0 * qy * qy + 2.0 * qz * qz - 1.0)); - jacobian_map(2, 1) = - -(2.0 * qy) / - ((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + - 1.0) * - (2.0 * qy * qy + 2.0 * qz * qz - 1.0)); - jacobian_map(2, 2) = - -((2.0 * qx) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) - - (4.0 * qy * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) / - (std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0); - jacobian_map(2, 3) = - -((2.0 * qw) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) - - (4.0 * qz * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) / - (std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0); - } + jacobian_map(1, 0) = (2.0 * qy) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map(1, 1) = -(2.0 * qz) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map(1, 2) = (2.0 * qw) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map(1, 3) = -(2.0 * qx) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + + jacobian_map(2, 0) = + -(2.0 * qz) / + ((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0) * + (2.0 * qy * qy + 2.0 * qz * qz - 1.0)); + jacobian_map(2, 1) = + -(2.0 * qy) / + ((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0) * + (2.0 * qy * qy + 2.0 * qz * qz - 1.0)); + jacobian_map(2, 2) = + -((2.0 * qx) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) - + (4.0 * qy * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) / + (std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0); + jacobian_map(2, 3) = + -((2.0 * qw) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) - + (4.0 * qz * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) / + (std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0); } } @@ -256,7 +248,7 @@ static inline void quaternion2rpy(const double* q, double* rpy, double* jacobian static inline void quaternionProduct(const double* z, const double* w, double* zw, double* jacobian = nullptr) { ceres::QuaternionProduct(z, w, zw); - if (jacobian) + if (jacobian != nullptr) { Eigen::Map> jacobian_map(jacobian); jacobian_map << z[0], -z[1], -z[2], -z[3], z[1], z[0], -z[3], z[2], z[2], z[3], z[0], -z[1], z[3], -z[2], z[1], @@ -274,7 +266,7 @@ static inline void quaternionProduct(const double* z, const double* w, double* z static inline void quaternionToAngleAxis(const double* q, double* angle_axis, double* jacobian = nullptr) { ceres::QuaternionToAngleAxis(q, angle_axis); - if (jacobian) + if (jacobian != nullptr) { Eigen::Map> jacobian_map(jacobian); const double& q0 = q[0]; @@ -283,36 +275,31 @@ static inline void quaternionToAngleAxis(const double* q, double* angle_axis, do const double& q3 = q[3]; const double q_sum2 = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3; const double sin_theta2 = q1 * q1 + q2 * q2 + q3 * q3; - const double sin_theta = std::sqrt(sin_theta2); + const double sin_theta = std::hypot(q1, q2, q3); const double cos_theta = q0; - if (std::fpclassify(sin_theta) != FP_ZERO) + const double cond = std::pow(sin_theta2, 1.5); + if (std::fpclassify(cond) != FP_ZERO) { const double two_theta = 2.0 * (cos_theta < 0.0 ? std::atan2(-sin_theta, -cos_theta) : std::atan2(sin_theta, cos_theta)); + // const double a = two_theta / sin_theta; + const double b = sin_theta2 * q_sum2; + const double c = two_theta / cond; jacobian_map(0, 0) = -2.0 * q1 / q_sum2; - jacobian_map(0, 1) = two_theta / sin_theta + (2.0 * q0 * q1 * q1) / (sin_theta2 * q_sum2) - - (q1 * q1 * two_theta) / std::pow(sin_theta2, 1.5); - jacobian_map(0, 2) = - (2.0 * q0 * q1 * q2) / (sin_theta2 * q_sum2) - (q1 * q2 * two_theta) / std::pow(sin_theta2, 1.5); - jacobian_map(0, 3) = - (2.0 * q0 * q1 * q3) / (sin_theta2 * q_sum2) - (q1 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(0, 1) = two_theta / sin_theta + (2.0 * q0 * q1 * q1) / b - (q1 * q1 * c); + jacobian_map(0, 2) = (2.0 * q0 * q1 * q2) / b - (q1 * q2 * c); + jacobian_map(0, 3) = (2.0 * q0 * q1 * q3) / b - (q1 * q3 * c); jacobian_map(1, 0) = -2.0 * q2 / q_sum2; - jacobian_map(1, 1) = - (2.0 * q0 * q1 * q2) / (sin_theta2 * q_sum2) - (q1 * q2 * two_theta) / std::pow(sin_theta2, 1.5); - jacobian_map(1, 2) = two_theta / sin_theta + (2.0 * q0 * q2 * q2) / (sin_theta2 * q_sum2) - - (q2 * q2 * two_theta) / std::pow(sin_theta2, 1.5); - jacobian_map(1, 3) = - (2.0 * q0 * q2 * q3) / (sin_theta2 * q_sum2) - (q2 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(1, 1) = (2.0 * q0 * q1 * q2) / b - (q1 * q2 * c); + jacobian_map(1, 2) = two_theta / sin_theta + (2.0 * q0 * q2 * q2) / b - (q2 * q2 * c); + jacobian_map(1, 3) = (2.0 * q0 * q2 * q3) / b - (q2 * q3 * c); jacobian_map(2, 0) = -2.0 * q3 / q_sum2; - jacobian_map(2, 1) = - (2.0 * q0 * q1 * q3) / (sin_theta2 * q_sum2) - (q1 * q3 * two_theta) / std::pow(sin_theta2, 1.5); - jacobian_map(2, 2) = - (2.0 * q0 * q2 * q3) / (sin_theta2 * q_sum2) - (q2 * q3 * two_theta) / std::pow(sin_theta2, 1.5); - jacobian_map(2, 3) = two_theta / sin_theta + (2.0 * q0 * q3 * q3) / (sin_theta2 * q_sum2) - - (q3 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(2, 1) = (2.0 * q0 * q1 * q3) / b - (q1 * q3 * c); + jacobian_map(2, 2) = (2.0 * q0 * q2 * q3) / b - (q2 * q3 * c); + jacobian_map(2, 3) = two_theta / sin_theta + (2.0 * q0 * q3 * q3) / b - (q3 * q3 * c); } else { diff --git a/fuse_core/src/async_motion_model.cpp b/fuse_core/src/async_motion_model.cpp index b3fd5da11..d6dd6fc47 100644 --- a/fuse_core/src/async_motion_model.cpp +++ b/fuse_core/src/async_motion_model.cpp @@ -52,7 +52,7 @@ AsyncMotionModel::AsyncMotionModel(size_t thread_count) : name_("uninitialized") AsyncMotionModel::~AsyncMotionModel() { - internal_stop(); + internalStop(); } bool AsyncMotionModel::apply(Transaction& transaction) @@ -156,12 +156,12 @@ void AsyncMotionModel::stop() // Can't run in executor's thread because the executor won't service more // callbacks after the context is shutdown. // Join executor's threads right away. - internal_stop(); + internalStop(); onStop(); } } -void AsyncMotionModel::internal_stop() +void AsyncMotionModel::internalStop() { if (spinner_.joinable()) { diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 954714a14..4aa9efd0d 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -32,8 +32,10 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ +#include #include +#include #include #include @@ -51,9 +53,9 @@ struct Quat2RPY return true; } - static ceres::CostFunction* Create() + static std::unique_ptr create() { - return (new ceres::AutoDiffCostFunction(new Quat2RPY())); + return std::unique_ptr(new ceres::AutoDiffCostFunction(new Quat2RPY())); } }; @@ -66,9 +68,9 @@ struct QuatProd return true; } - static ceres::CostFunction* Create() + static std::unique_ptr create() { - return (new ceres::AutoDiffCostFunction(new QuatProd())); + return std::unique_ptr(new ceres::AutoDiffCostFunction(new QuatProd())); } }; @@ -81,9 +83,10 @@ struct Quat2AngleAxis return true; } - static ceres::CostFunction* Create() + static std::unique_ptr create() { - return (new ceres::AutoDiffCostFunction(new Quat2AngleAxis())); + return std::unique_ptr( + new ceres::AutoDiffCostFunction(new Quat2AngleAxis())); } }; @@ -134,9 +137,9 @@ TEST(Util, wrapAngle2D) TEST(Util, quaternion2rpy) { // Test correct conversion from quaternion to roll-pitch-yaw - double q[4] = { 1.0, 0.0, 0.0, 0.0 }; - double rpy[3]; - fuse_core::quaternion2rpy(q, rpy); + std::array q = { 1.0, 0.0, 0.0, 0.0 }; + std::array rpy{}; + fuse_core::quaternion2rpy(q.data(), rpy.data()); EXPECT_EQ(0.0, rpy[0]); EXPECT_EQ(0.0, rpy[1]); EXPECT_EQ(0.0, rpy[2]); @@ -146,32 +149,33 @@ TEST(Util, quaternion2rpy) q[2] = 0.0911575; q[3] = -0.1534393; - fuse_core::quaternion2rpy(q, rpy); + fuse_core::quaternion2rpy(q.data(), rpy.data()); EXPECT_NEAR(0.1, rpy[0], 1e-6); EXPECT_NEAR(0.2, rpy[1], 1e-6); EXPECT_NEAR(-0.3, rpy[2], 1e-6); // Test correct quaternion to roll-pitch-yaw function jacobian const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); - double J_analytic[12], J_autodiff[12]; + std::array j_analytic{}; + std::array j_autodiff{}; q[0] = q_eigen.w(); q[1] = q_eigen.x(); q[2] = q_eigen.y(); q[3] = q_eigen.z(); - fuse_core::quaternion2rpy(q, rpy, J_analytic); + fuse_core::quaternion2rpy(q.data(), rpy.data(), j_analytic.data()); - double* jacobians[1] = { J_autodiff }; - const double* parameters[1] = { q }; + std::array jacobians = { j_autodiff.data() }; + std::array const parameters = { q.data() }; - ceres::CostFunction* quat2rpy_cf = Quat2RPY::Create(); - double rpy_autodiff[3]; - quat2rpy_cf->Evaluate(parameters, rpy_autodiff, jacobians); + auto quat2rpy_cf = Quat2RPY::create(); + std::array rpy_autodiff{}; + quat2rpy_cf->Evaluate(parameters.data(), rpy_autodiff.data(), jacobians.data()); - Eigen::Map> J_autodiff_map(jacobians[0]); - Eigen::Map> J_analytic_map(J_analytic); + Eigen::Map> const j_autodiff_map(jacobians[0]); + Eigen::Map> const j_analytic_map(j_analytic.data()); - EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); + EXPECT_TRUE(j_analytic_map.isApprox(j_autodiff_map)); } TEST(Util, quaternionProduct) @@ -179,86 +183,120 @@ TEST(Util, quaternionProduct) // Test correct quaternion product function jacobian const Eigen::Quaterniond q1_eigen = Eigen::Quaterniond::UnitRandom(); const Eigen::Quaterniond q2_eigen = Eigen::Quaterniond::UnitRandom(); - double q_out[4]; - double q1[4]{ q1_eigen.w(), q1_eigen.x(), q1_eigen.y(), q1_eigen.z() }; + std::array q_out{}; + std::array q1{ q1_eigen.w(), q1_eigen.x(), q1_eigen.y(), q1_eigen.z() }; - double q2[4]{ q2_eigen.w(), q2_eigen.x(), q2_eigen.y(), q2_eigen.z() }; + std::array q2{ q2_eigen.w(), q2_eigen.x(), q2_eigen.y(), q2_eigen.z() }; // Atm only the jacobian wrt the second quaternion is implemented. If the computation will be // extended in future, we just have to compare J_analytic_q1 with the other automatic J_autodiff_q1. - // double J_analytic_q1[16]; // Analytical Jacobians wrt first quaternion - double J_analytic_q2[16]; // Analytical Jacobian wrt second quaternion - double J_autodiff_q1[16], J_autodiff_q2[16]; // Autodiff Jacobians wrt first and second quaternion + // std::array j_analytic_q1{}; + std::array j_analytic_q2{}; // Analytical Jacobians wrt first and second quaternion + std::array j_autodiff_q1{}; + std::array j_autodiff_q2{}; // Autodiff Jacobians wrt first and second quaternion - fuse_core::quaternionProduct(q1, q2, q_out, J_analytic_q2); + fuse_core::quaternionProduct(q1.data(), q2.data(), q_out.data(), j_analytic_q2.data()); - double* jacobians[2]; - jacobians[0] = J_autodiff_q1; - jacobians[1] = J_autodiff_q2; + fuse_core::quaternionProduct(q1.data(), q2.data(), q_out.data(), j_analytic_q2.data()); - const double* parameters[2]; - parameters[0] = q1; - parameters[1] = q2; + std::array jacobians{}; + jacobians[0] = j_autodiff_q1.data(); + jacobians[1] = j_autodiff_q2.data(); - ceres::CostFunction* quat_prod_cf = QuatProd::Create(); - double q_out_autodiff[4]; - quat_prod_cf->Evaluate(parameters, q_out_autodiff, jacobians); + std::array parameters{}; + parameters[0] = q1.data(); + parameters[1] = q2.data(); - Eigen::Map> J_autodiff_q1_map(jacobians[0]); - Eigen::Map> J_autodiff_q2_map(jacobians[1]); + auto quat_prod_cf = QuatProd::create(); + std::array q_out_autodiff{}; + quat_prod_cf->Evaluate(parameters.data(), q_out_autodiff.data(), jacobians.data()); + + Eigen::Map> const j_autodiff_q1_map(jacobians[0]); + Eigen::Map> const j_autodiff_q2_map(jacobians[1]); // Eigen::Map> J_analytic_q1_map(J_analytic_q1); - Eigen::Map> J_analytic_q2_map(J_analytic_q2); + Eigen::Map> const j_analytic_q2_map(j_analytic_q2.data()); - EXPECT_TRUE(J_analytic_q2_map.isApprox(J_autodiff_q2_map)); + EXPECT_TRUE(j_analytic_q2_map.isApprox(j_autodiff_q2_map)); } TEST(Util, quaternionToAngleAxis) { // Test correct quaternion to angle-axis function jacobian, for quaternions representing non-zero rotation + // The implementation of quaternionToAngleAxis changes slightly between ceres 2.1.0 and 2.2.0. We checked for + // both the versions for the test to pass. { const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); - double angle_axis[3]; - double q[4]{ q_eigen.w(), q_eigen.x(), q_eigen.y(), q_eigen.z() }; + std::array angle_axis{}; + std::array q{ q_eigen.w(), q_eigen.x(), q_eigen.y(), q_eigen.z() }; - double J_analytic[12]; - double J_autodiff[12]; + std::array j_analytic{}; + std::array j_autodiff{}; - fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + fuse_core::quaternionToAngleAxis(q.data(), angle_axis.data(), j_analytic.data()); - double* jacobians[1] = { J_autodiff }; - const double* parameters[1] = { q }; + std::array jacobians = { j_autodiff.data() }; + std::array parameters = { q.data() }; - ceres::CostFunction* quat2angle_axis_cf = Quat2AngleAxis::Create(); - double angle_axis_autodiff[3]; - quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); + auto quat2angle_axis_cf = Quat2AngleAxis::create(); + std::array angle_axis_autodiff{}; + quat2angle_axis_cf->Evaluate(parameters.data(), angle_axis_autodiff.data(), jacobians.data()); - Eigen::Map> J_autodiff_map(jacobians[0]); - Eigen::Map> J_analytic_map(J_analytic); + Eigen::Map> const j_autodiff_map(jacobians[0]); + Eigen::Map> const j_analytic_map(j_analytic.data()); - EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); + EXPECT_TRUE(j_analytic_map.isApprox(j_autodiff_map)); } // Test correct quaternion to angle-axis function jacobian, for quaternions representing zero rotation { - double angle_axis[3]; - double q[4]{ 1.0, 0.0, 0.0, 0.0 }; + std::array angle_axis{}; + std::array q{ 1.0, 0.0, 0.0, 0.0 }; + + std::array j_analytic{}; + std::array j_autodiff{}; + + fuse_core::quaternionToAngleAxis(q.data(), angle_axis.data(), j_analytic.data()); + + std::array jacobians = { j_autodiff.data() }; + std::array parameters = { q.data() }; + + auto quat2angle_axis_cf = Quat2AngleAxis::create(); + std::array angle_axis_autodiff{}; + quat2angle_axis_cf->Evaluate(parameters.data(), angle_axis_autodiff.data(), jacobians.data()); + + Eigen::Map> const j_autodiff_map(jacobians[0]); + Eigen::Map> const j_analytic_map(j_analytic.data()); + + EXPECT_TRUE(j_analytic_map.isApprox(j_autodiff_map)); + } + + { + // Test that approximate conversion and jacobian computation work for very small angles that + // could potentially cause underflow. - double J_analytic[12]; - double J_autodiff[12]; + double const theta = std::pow(std::numeric_limits::min(), 0.75); + std::array q = { cos(theta / 2.0), sin(theta / 2.0), 0, 0 }; + std::array angle_axis{}; + std::array expected = { theta, 0, 0 }; + std::array j_analytic{}; + std::array j_autodiff{}; - fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + fuse_core::quaternionToAngleAxis(q.data(), angle_axis.data(), j_analytic.data()); + EXPECT_DOUBLE_EQ(angle_axis[0], expected[0]); + EXPECT_DOUBLE_EQ(angle_axis[1], expected[1]); + EXPECT_DOUBLE_EQ(angle_axis[2], expected[2]); - double* jacobians[1] = { J_autodiff }; - const double* parameters[1] = { q }; + std::array jacobians = { j_autodiff.data() }; + std::array parameters = { q.data() }; - ceres::CostFunction* quat2angle_axis_cf = Quat2AngleAxis::Create(); - double angle_axis_autodiff[3]; - quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); + auto quat2angle_axis_cf = Quat2AngleAxis::create(); + std::array angle_axis_autodiff{}; + quat2angle_axis_cf->Evaluate(parameters.data(), angle_axis_autodiff.data(), jacobians.data()); - Eigen::Map> J_autodiff_map(jacobians[0]); - Eigen::Map> J_analytic_map(J_analytic); + Eigen::Map> const j_autodiff_map(jacobians[0]); + Eigen::Map> const j_analytic_map(j_analytic.data()); - EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); + EXPECT_TRUE(j_analytic_map.isApprox(j_autodiff_map)); } } diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index faf8ec3ef..abc639c8b 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -31,6 +31,7 @@ find_package(tf2 REQUIRED) find_package(tf2_2d REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(mujoco REQUIRED) find_package(Ceres REQUIRED) find_package(Eigen3 REQUIRED) @@ -46,6 +47,7 @@ add_library( src/graph_ignition.cpp src/imu_2d.cpp src/imu_3d.cpp + src/mujoco_model.cpp src/odometry_2d.cpp src/odometry_2d_publisher.cpp src/odometry_3d.cpp @@ -74,6 +76,7 @@ target_link_libraries( fuse_publishers::fuse_publishers fuse_variables::fuse_variables ${geometry_msgs_TARGETS} + mujoco::mujoco ${nav_msgs_TARGETS} pluginlib::pluginlib rclcpp_components::component @@ -132,6 +135,7 @@ ament_export_dependencies( tf2_ros Ceres Eigen3 - Boost) + Boost + mujoco) ament_package() diff --git a/fuse_models/include/fuse_models/mujoco_model.hpp b/fuse_models/include/fuse_models/mujoco_model.hpp new file mode 100644 index 000000000..967d3d810 --- /dev/null +++ b/fuse_models/include/fuse_models/mujoco_model.hpp @@ -0,0 +1,170 @@ +#include +#include +#include + +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, PickNik Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include +#include + +namespace fuse_models +{ + +/** + * @brief A motion model base class that provides an internal callback queue and executor. + * + * A model model plugin is responsible for generating constraints that link together timestamps + * introduced by other sensors in the system. The MujocoMotionModel class is designed similar to a + * nodelet, attempting to be as generic and easy to use as a standard ROS node. + * + * There are a few notable differences between the MujocoMotionModel class and a standard ROS + * node. First and most obvious, the MujocoMotionModel class is designed as a plugin, with all of + * the stipulations and requirements that come with all ROS plugins (must be derived from a known + * base class, will be default constructed). Second, the MujocoMotionModel class provides an internal + * node that is hooked to a local callback queue and local executor on init. This makes + * it act like a full ROS node -- subscriptions trigger message callbacks, callbacks will fire + * sequentially, etc. However, authors of derived classes should be aware of this fact and avoid + * creating additional sub-nodes, or at least take care when creating new sub-nodes and + * additional callback queues. Finally, the interaction of motion model nodes is best compared to + * a service call -- an external actor will provide a set of timestamps and wait for the motion + * model to respond with the required set of constraints to link the timestamps together (along + * with any previously existing timestamps). In lieu of a ROS service callback function, the + * MujocoMotionModel class requires the applyCallback() function to be implemented. This callback + * will be executed from the same callback queue as any other subscriptions or service callbacks. + * + * Derived classes: + * - _probably_ need to implement the onInit() method. This method is used to configure the motion + * model for operation. This includes things like accessing the parameter server and subscribing + * to sensor topics. + * - _must_ implement the applyCallback() method. This is the communication mechanism between the + * parent/optimizer and the derived motion model. This is how the optimizer tells the motion + * model what timestamps have been added, and how the motion model sends motion model constraints + * to the optimizer. + * - may _optionally_ implement the onGraphUpdate() method. This should only be done if the derived + * motion model needs access to the latest values of the state variables. In many cases, motion + * models will simply not need that information. If the motion model does need access the to + * graph, the most common implementation will simply be to move the provided pointer into a class + * member variable, for use in other callbacks. + */ +class MujocoMotionModel : public fuse_core::AsyncMotionModel +{ +public: + FUSE_SMART_PTR_ALIASES_ONLY(MujocoMotionModel) + + /** + * @brief Destructor + */ + ~MujocoMotionModel() = default; + MujocoMotionModel(MujocoMotionModel const&) = delete; + MujocoMotionModel(MujocoMotionModel&&) = delete; + MujocoMotionModel& operator=(MujocoMotionModel&&) = delete; + MujocoMotionModel& operator=(MujocoMotionModel const&) = delete; + +protected: + /** + * @brief Constructor + * + * Construct a new motion model and create a local callback queue and internal executor. + * + * @param[in] model The mujoco model + */ + explicit MujocoMotionModel(); + + /** + * @brief Augment a transaction object such that all involved timestamps are connected by motion + * model constraints. + * + * This is not as straightforward as it would seem. Depending on the history of previously + * generated constraints, fulfilling the request may require removing previously generated + * constraints and creating several new constraints, such that all involved timestamps are + * linked together in a sequential chain. This function is called by the MotionModel::apply() + * function, but it is done in such a way that *this* function will run inside the derived + * MujocoMotionModel's local callback queue. This function is roughly analogous to providing a + * service callback, where the caller makes a request and blocks until the request is + * completed. + * + * @param[in,out] transaction The transaction object that should be augmented with motion model + * constraints + * @return True if the motion models were generated successfully, false + * otherwise + */ + bool applyCallback(fuse_core::Transaction& transaction) override; + + void generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables); + + /** + * @brief Callback fired in the local callback queue thread(s) whenever a new Graph is received + * from the optimizer + * + * Receiving a new Graph object generally means that new variables have been inserted into + * the Graph, and new optimized values are available. To simplify synchronization between the + * sensor models and other consumers of Graph data, the provided Graph object will never be + * updated be updated by anyone. Thus, only read access to the Graph is provided. Information + * may be accessed or computed, but it cannot be changed. The optimizer provides the sensors + * with Graph updates by sending a new Graph object, not by modifying the Graph object. + * + * If the derived sensor model does not need access to the Graph object, there is not reason + * to overload this empty implementation. + * + * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed + * whenever needed. + */ + void onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) override; + + /** + * @brief Perform any required initialization for the motion model + * + * This could include things like reading from the parameter server or subscribing to topics. + * The class's node will be properly initialized before onInit() is called. Spinning + * of the callback queue will not begin until after the call to onInit() completes. + */ + void onInit() override; + + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The sensor model's logger + fuse_core::UUID device_id_; //!< The UUID of the device to be published + fuse_core::TimestampManager timestamp_manager_; //!< Tracks timestamps and previously created + //!< motion model segments + std::shared_ptr graph_; + std::unique_ptr model_; + std::unique_ptr data_; +}; + +} // namespace fuse_models diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index e6be1b84b..1532422b3 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -85,6 +85,10 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * @brief Destructor */ ~Omnidirectional3D() = default; + Omnidirectional3D(Omnidirectional3D const&) = delete; + Omnidirectional3D(Omnidirectional3D&&) = delete; + Omnidirectional3D& operator=(Omnidirectional3D const&) = delete; + Omnidirectional3D& operator=(Omnidirectional3D&&) = delete; /** * @brief Shadowing extension to the AsyncMotionModel::initialize call diff --git a/fuse_models/package.xml b/fuse_models/package.xml index 1eefe87c9..7fe1e09f1 100644 --- a/fuse_models/package.xml +++ b/fuse_models/package.xml @@ -16,6 +16,7 @@ boost libceres-dev eigen + mujoco covariance_geometry_ros fuse_constraints diff --git a/fuse_models/src/mujoco_model.cpp b/fuse_models/src/mujoco_model.cpp new file mode 100644 index 000000000..1a251e4fb --- /dev/null +++ b/fuse_models/src/mujoco_model.cpp @@ -0,0 +1,50 @@ +#include + +namespace fuse_models +{ +MujocoMotionModel::MujocoMotionModel() + : fuse_core::AsyncMotionModel(1) + , logger_(rclcpp::get_logger("mujoco_model")) + , device_id_(fuse_core::uuid::NIL) + , timestamp_manager_(&MujocoMotionModel::generateMotionModel, this, rclcpp::Duration::max()) +{ +} + +void MujocoMotionModel::generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables) +{ + // TODO(henrygerardmoore): implement this +} + +void MujocoMotionModel::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) +{ + // TODO(henrygerardmoore): see if we need to emulate unicycle_2d for this + this->graph_ = std::move(graph); +} + +void MujocoMotionModel::onInit() +{ + // TODO(henrygerardmoore): implement this + // model_ = fuse::getParam... + // data_ = ... +} + +bool MujocoMotionModel::applyCallback(fuse_core::Transaction& transaction) +{ + // Use the timestamp manager to generate just the required motion model segments. The timestamp + // manager, in turn, makes calls to the generateMotionModel() function. + try + { + // Now actually generate the motion model segments + timestamp_manager_.query(transaction, true); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "An error occurred while completing the motion model query. Error: " << e.what()); + return false; + } + return true; +} +} // namespace fuse_models diff --git a/fuse_models/src/omnidirectional_3d.cpp b/fuse_models/src/omnidirectional_3d.cpp index 5def0f0c3..bb3778cd2 100644 --- a/fuse_models/src/omnidirectional_3d.cpp +++ b/fuse_models/src/omnidirectional_3d.cpp @@ -67,7 +67,7 @@ namespace std inline bool isfinite(const fuse_core::Vector3d& vector) { - return std::isfinite(vector.x()) && std::isfinite(vector.y() && std::isfinite(vector.z())); + return std::isfinite(vector.x()) && std::isfinite(vector.y()) && std::isfinite(vector.z()); } inline bool isNormalized(const Eigen::Quaterniond& q) @@ -376,9 +376,6 @@ void Omnidirectional3D::generateMotionModel(const rclcpp::Time& beginning_stamp, state2.vel_angular_uuid = velocity_angular2->uuid(); state2.acc_linear_uuid = acceleration_linear2->uuid(); - state_history_.emplace(beginning_stamp, std::move(state1)); - state_history_.emplace(ending_stamp, std::move(state2)); - // Scale process noise covariance pose by the norm of the current state twist auto process_noise_covariance = process_noise_covariance_; if (scale_process_noise_) @@ -404,6 +401,9 @@ void Omnidirectional3D::generateMotionModel(const rclcpp::Time& beginning_stamp, } } + state_history_.emplace(beginning_stamp, std::move(state1)); + state_history_.emplace(ending_stamp, std::move(state2)); + // Create the constraints for this motion model segment auto constraint = fuse_models::Omnidirectional3DStateKinematicConstraint::make_shared( name(), *position1, *orientation1, *velocity_linear1, *velocity_angular1, *acceleration_linear1, *position2, diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp index 54ede9a92..1616c12a0 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp @@ -103,13 +103,17 @@ class Optimizer * @param[in] interfaces The node interfaces for the node driving the optimizer * @param[in] graph The graph used with the optimizer */ - Optimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph = nullptr); + explicit Optimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph = nullptr); /** * @brief Destructor */ virtual ~Optimizer(); + Optimizer(Optimizer const&) = delete; + Optimizer(Optimizer&&) = delete; + Optimizer& operator=(Optimizer&&) = delete; + Optimizer& operator=(Optimizer const&) = delete; protected: // The unique ptrs returned by pluginlib have a custom deleter. This makes specifying the type @@ -227,7 +231,7 @@ class Optimizer * removals * @param[in] graph A read-only pointer to the graph object */ - void notify(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph); + void notify(fuse_core::Transaction::ConstSharedPtr const& transaction, fuse_core::Graph::ConstSharedPtr const& graph); /** * @brief Inject a transaction callback function into the global callback queue diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index 5d9571eb3..404411ada 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -32,10 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include #include #include -#include #include #include #include @@ -70,7 +68,7 @@ Optimizer::Optimizer(fuse_core::node_interfaces::NodeInterfaces motion_model_config; - std::unordered_set motion_model_names = + std::unordered_set const motion_model_names = fuse_core::list_parameter_override_prefixes(interfaces_, "motion_models."); // declare config parameters for each model @@ -130,7 +132,7 @@ void Optimizer::loadMotionModels() } // get the type parameter for the motion model - rclcpp::Parameter motion_model_type_param = + rclcpp::Parameter const motion_model_type_param = interfaces_.get_node_parameters_interface()->get_parameter(config.param_name); // extract the type string from the parameter if (motion_model_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) @@ -139,7 +141,7 @@ void Optimizer::loadMotionModels() } // quickly check for common errors - if (config.type == "") + if (config.type.empty()) { RCLCPP_WARN_STREAM(logger_, "parameter '" << config.param_name << "' should be the string of a motion_model type " << "for the motion_model named '" << config.name << "'."); @@ -165,11 +167,11 @@ void Optimizer::loadMotionModels() void Optimizer::loadSensorModels() { // struct for readability - typedef struct + typedef struct ModelConfig { std::string name; std::string type; - bool ignition; + bool ignition = false; std::vector associated_motion_models; std::string type_param_name; std::string models_param_name; @@ -179,7 +181,7 @@ void Optimizer::loadSensorModels() // the configurations used to load models std::vector sensor_model_config; - std::unordered_set sensor_model_names = + std::unordered_set const sensor_model_names = fuse_core::list_parameter_override_prefixes(interfaces_, "sensor_models."); // declare config parameters for each model @@ -202,7 +204,7 @@ void Optimizer::loadSensorModels() } // get the type parameter for the sensor model - rclcpp::Parameter sensor_model_type_param = + rclcpp::Parameter const sensor_model_type_param = interfaces_.get_node_parameters_interface()->get_parameter(config.type_param_name); // extract the type string from the parameter if (sensor_model_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) @@ -220,7 +222,7 @@ void Optimizer::loadSensorModels() } // get the model_list parameter for the sensor model - rclcpp::Parameter sensor_model_model_list_param = + rclcpp::Parameter const sensor_model_model_list_param = interfaces_.get_node_parameters_interface()->get_parameter(config.models_param_name); // extract the model_list string from the parameter if (sensor_model_model_list_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) @@ -238,7 +240,7 @@ void Optimizer::loadSensorModels() } // get the model list parameter for the sensor model - rclcpp::Parameter sensor_model_ignition_param = + rclcpp::Parameter const sensor_model_ignition_param = interfaces_.get_node_parameters_interface()->get_parameter(config.ignition_param_name); // extract the ignition bool from the parameter if (sensor_model_ignition_param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) @@ -247,7 +249,7 @@ void Optimizer::loadSensorModels() } // quickly check for common errors - if (config.type == "") + if (config.type.empty()) { RCLCPP_WARN_STREAM(logger_, "parameter '" << config.type_param_name << "' should be the string of a sensor_model type " @@ -297,7 +299,7 @@ void Optimizer::loadPublishers() // the configurations used to load models std::vector publisher_config; - std::unordered_set publisher_names = + std::unordered_set const publisher_names = fuse_core::list_parameter_override_prefixes(interfaces_, "publishers."); // declare config parameters for each model @@ -317,7 +319,7 @@ void Optimizer::loadPublishers() } // get the type parameter for the publisher - rclcpp::Parameter publisher_type_param = + rclcpp::Parameter const publisher_type_param = interfaces_.get_node_parameters_interface()->get_parameter(config.param_name); // extract the type string from the parameter if (publisher_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) @@ -326,7 +328,7 @@ void Optimizer::loadPublishers() } // quickly check for common errors - if (config.type == "") + if (config.type.empty()) { RCLCPP_WARN_STREAM(logger_, "parameter '" << config.param_name << "' should be the string of a publisher type " << "for the publisher named '" << config.name << "'."); @@ -376,44 +378,45 @@ bool Optimizer::applyMotionModels(const std::string& sensor_name, fuse_core::Tra return success; } -void Optimizer::notify(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph) +void Optimizer::notify(fuse_core::Transaction::ConstSharedPtr const& transaction, + fuse_core::Graph::ConstSharedPtr const& graph) { - for (const auto& name__sensor_model : sensor_models_) + for (const auto& name_sensor_model : sensor_models_) { try { - name__sensor_model.second.model->graphCallback(graph); + name_sensor_model.second.model->graphCallback(graph); } catch (const std::exception& e) { - RCLCPP_ERROR_STREAM(logger_, "Failed calling graphCallback() on sensor '" << name__sensor_model.first + RCLCPP_ERROR_STREAM(logger_, "Failed calling graphCallback() on sensor '" << name_sensor_model.first << "'. Error: " << e.what()); continue; } } - for (const auto& name__motion_model : motion_models_) + for (const auto& name_motion_model : motion_models_) { try { - name__motion_model.second->graphCallback(graph); + name_motion_model.second->graphCallback(graph); } catch (const std::exception& e) { - RCLCPP_ERROR_STREAM(logger_, "Failed calling graphCallback() on motion model '" << name__motion_model.first + RCLCPP_ERROR_STREAM(logger_, "Failed calling graphCallback() on motion model '" << name_motion_model.first << ". Error: " << e.what()); continue; } } - for (const auto& name__publisher : publishers_) + for (const auto& name_publisher : publishers_) { try { - name__publisher.second->notify(transaction, graph); + name_publisher.second->notify(transaction, graph); } catch (const std::exception& e) { RCLCPP_ERROR_STREAM(logger_, - "Failed calling notify() on publisher '" << name__publisher.first << ". Error: " << e.what()); + "Failed calling notify() on publisher '" << name_publisher.first << ". Error: " << e.what()); continue; } }