From 5182ec0715db2a88072a9723fce972d02a81c318 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 11 Nov 2024 15:50:31 -0700 Subject: [PATCH] Apriltag measurement model (#9) * fix clang tidy errors in imu_3d * clang tidy fixes for various core files * more clang tidy fixes, allow c-style arrays * remove unused headers * add skeleton of april tag pose detector, clang tidy fix to imu * switch copyright * clang tidy fixes * undo unique ptr changes * april tag WIP * clang tidy fixes * build fixes * clang-tidy fixes * clang-tidy fixes * add skeleton of apriltag tutorial * remove now-unused compile options * finish april tag simulator * clang tidy fixes * remove unknown compile warning * fix memory leak * applied PR feedback * remove extraneous sleep * use more visually pleasing parameters * improve the tutorial config file * fix bit shift logic :woozy: * Update fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp Co-authored-by: Bilal Gill * apply PR feedback and remove extraneous comments * improve comments and parameter comments --------- Co-authored-by: Bilal Gill --- .clang-tidy | 8 +- fuse_constraints/CMakeLists.txt | 2 +- .../src/marginalize_variables.cpp | 102 +++-- .../test/test_marginalize_variables.cpp | 52 +-- .../include/fuse_core/async_publisher.hpp | 9 +- .../include/fuse_core/async_sensor_model.hpp | 8 +- fuse_core/include/fuse_core/graph.hpp | 2 +- fuse_core/include/fuse_core/sensor_model.hpp | 7 +- .../include/fuse_core/timestamp_manager.hpp | 5 + fuse_core/include/fuse_core/variable.hpp | 33 +- fuse_core/src/async_publisher.cpp | 6 +- fuse_core/src/async_sensor_model.cpp | 6 +- fuse_core/src/timestamp_manager.cpp | 14 +- fuse_core/src/transaction.cpp | 24 +- fuse_core/test/example_variable.hpp | 20 +- fuse_core/test/test_variable.cpp | 16 +- fuse_graphs/CMakeLists.txt | 52 --- fuse_graphs/src/hash_graph.cpp | 66 ++- fuse_models/CMakeLists.txt | 1 + fuse_models/fuse_plugins.xml | 5 + .../fuse_models/common/sensor_proc.hpp | 119 +++--- fuse_models/include/fuse_models/imu_3d.hpp | 7 +- .../include/fuse_models/odometry_3d.hpp | 6 +- .../fuse_models/parameters/imu_3d_params.hpp | 8 +- .../parameters/odometry_3d_params.hpp | 15 +- .../parameters/transform_sensor_params.hpp | 108 +++++ fuse_models/include/fuse_models/pose_2d.hpp | 6 +- .../include/fuse_models/transform_sensor.hpp | 155 +++++++ fuse_models/src/transform_sensor.cpp | 148 +++++++ .../fuse_optimizers/fixed_lag_smoother.hpp | 13 +- .../fixed_lag_smoother_params.hpp | 2 - fuse_optimizers/src/fixed_lag_smoother.cpp | 130 +++--- fuse_tutorials/CMakeLists.txt | 4 + .../config/fuse_apriltag_tutorial.yaml | 55 +++ .../launch/fuse_apriltag_tutorial.launch.py | 68 +++ fuse_tutorials/src/apriltag_simulator.cpp | 394 ++++++++++++++++++ .../src/three_dimensional_simulator.cpp | 68 +-- .../acceleration_angular_2d_stamped.hpp | 4 +- .../acceleration_angular_3d_stamped.hpp | 4 +- .../acceleration_linear_2d_stamped.hpp | 4 +- .../acceleration_linear_3d_stamped.hpp | 4 +- .../fuse_variables/fixed_size_variable.hpp | 10 +- .../fuse_variables/orientation_2d_stamped.hpp | 6 +- .../fuse_variables/orientation_3d_stamped.hpp | 27 +- .../include/fuse_variables/pinhole_camera.hpp | 4 +- .../fuse_variables/pinhole_camera_fixed.hpp | 2 +- .../point_2d_fixed_landmark.hpp | 4 +- .../fuse_variables/point_2d_landmark.hpp | 4 +- .../point_3d_fixed_landmark.hpp | 4 +- .../fuse_variables/point_3d_landmark.hpp | 4 +- .../fuse_variables/position_2d_stamped.hpp | 4 +- .../fuse_variables/position_3d_stamped.hpp | 4 +- .../include/fuse_variables/stamped.hpp | 6 +- .../velocity_angular_2d_stamped.hpp | 4 +- .../velocity_angular_3d_stamped.hpp | 4 +- .../velocity_linear_2d_stamped.hpp | 4 +- .../velocity_linear_3d_stamped.hpp | 4 +- fuse_variables/src/orientation_2d_stamped.cpp | 3 +- fuse_variables/src/orientation_3d_stamped.cpp | 2 +- .../test_acceleration_angular_2d_stamped.cpp | 30 +- .../test_acceleration_angular_3d_stamped.cpp | 31 +- .../test_acceleration_linear_2d_stamped.cpp | 30 +- .../test_acceleration_linear_3d_stamped.cpp | 30 +- .../test/test_fixed_size_variable.cpp | 12 +- .../test/test_orientation_2d_stamped.cpp | 98 +++-- .../test/test_orientation_3d_stamped.cpp | 127 +++--- .../test/test_point_2d_fixed_landmark.cpp | 18 +- .../test/test_point_2d_landmark.cpp | 18 +- .../test/test_point_3d_fixed_landmark.cpp | 18 +- .../test/test_point_3d_landmark.cpp | 18 +- .../test/test_position_2d_stamped.cpp | 30 +- .../test/test_position_3d_stamped.cpp | 40 +- .../test/test_velocity_angular_2d_stamped.cpp | 30 +- .../test/test_velocity_angular_3d_stamped.cpp | 30 +- .../test/test_velocity_linear_2d_stamped.cpp | 30 +- .../test/test_velocity_linear_3d_stamped.cpp | 30 +- 76 files changed, 1684 insertions(+), 796 deletions(-) create mode 100644 fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp create mode 100644 fuse_models/include/fuse_models/transform_sensor.hpp create mode 100644 fuse_models/src/transform_sensor.cpp create mode 100644 fuse_tutorials/config/fuse_apriltag_tutorial.yaml create mode 100644 fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py create mode 100644 fuse_tutorials/src/apriltag_simulator.cpp diff --git a/.clang-tidy b/.clang-tidy index 19ae154ea..b9ba1944d 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -11,21 +11,25 @@ Checks: -*, readability-*, -bugprone-easily-swappable-parameters, -bugprone-exception-escape, + -cppcoreguidelines-avoid-c-arrays, -cppcoreguidelines-avoid-magic-numbers, -cppcoreguidelines-non-private-member-variables-in-classes, + -cppcoreguidelines-owning-memory, + -cppcoreguidelines-pro-bounds-array-to-pointer-decay, -cppcoreguidelines-pro-bounds-constant-array-index, -cppcoreguidelines-pro-bounds-pointer-arithmetic, + -cppcoreguidelines-pro-type-vararg, -google-readability-casting, -google-default-arguments, -misc-include-cleaner, -misc-non-private-member-variables-in-classes, - -modernize-use-trailing-return-type, -modernize-avoid-bind, + -modernize-avoid-c-arrays, + -modernize-use-trailing-return-type, -readability-identifier-length, -readability-function-cognitive-complexity, -readability-magic-numbers, -readability-uppercase-literal-suffix, - -cppcoreguidelines-pro-type-vararg, HeaderFilterRegex: '' CheckOptions: - key: llvm-namespace-comment.ShortNamespaceLines diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 5f3a5d4cc..f88a54439 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -116,7 +116,7 @@ include(suitesparse-extras.cmake) # plain_matrix_type::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 -Wno-stringop-overread) + add_compile_options(-Wall -Werror -Wno-array-bounds) else() add_compile_options(-Wall -Werror) endif() diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index f3fa47a32..617f03c41 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -35,9 +35,7 @@ #include #include -#include #include -#include #include #include #include @@ -99,7 +97,7 @@ UuidOrdering computeEliminationOrder(const std::vector& margina // New constraint and variable indices are automatically generated for (const auto& constraint : constraints) { - unsigned int constraint_index = constraint_order[constraint.uuid()]; + unsigned int const constraint_index = constraint_order[constraint.uuid()]; for (const auto& constraint_variable_uuid : constraint.variables()) { variable_constraints.insert(constraint_index, variable_order[constraint_variable_uuid]); @@ -110,19 +108,20 @@ UuidOrdering computeEliminationOrder(const std::vector& margina // Construct the CCOLAMD input structures auto recommended_size = - ccolamd_recommended(variable_constraints.size(), constraint_order.size(), variable_order.size()); - auto A = std::vector(recommended_size); + ccolamd_recommended(static_cast(variable_constraints.size()), static_cast(constraint_order.size()), + static_cast(variable_order.size())); + auto a = std::vector(recommended_size); auto p = std::vector(variable_order.size() + 1); // Use the VariableConstraints table to construct the A and p structures - auto A_iter = A.begin(); + auto a_iter = a.begin(); auto p_iter = p.begin(); *p_iter = 0; ++p_iter; for (unsigned int variable_index = 0u; variable_index < variable_order.size(); ++variable_index) { - A_iter = variable_constraints.getConstraints(variable_index, A_iter); - *p_iter = std::distance(A.begin(), A_iter); + a_iter = variable_constraints.getConstraints(variable_index, a_iter); + *p_iter = static_cast(std::distance(a.begin(), a_iter)); ++p_iter; } @@ -141,9 +140,9 @@ UuidOrdering computeEliminationOrder(const std::vector& margina int stats[CCOLAMD_STATS]; // Finally call CCOLAMD - auto success = ccolamd(constraint_order.size(), variable_order.size(), recommended_size, A.data(), p.data(), knobs, - stats, variable_groups.data()); - if (!success) + auto success = ccolamd(static_cast(constraint_order.size()), static_cast(variable_order.size()), + static_cast(recommended_size), a.data(), p.data(), knobs, stats, variable_groups.data()); + if (success == 0) { throw std::runtime_error("Failed to call CCOLAMD to generate the elimination order."); } @@ -279,8 +278,8 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G LinearTerm result; // Generate the cost function from the input constraint - auto cost_function = constraint.costFunction(); - size_t row_count = cost_function->num_residuals(); + auto* cost_function = constraint.costFunction(); + size_t const row_count = cost_function->num_residuals(); // Loop over the constraint's variables and do several things: // * Generate a vector of variable value pointers. This is needed for the Ceres API. @@ -299,7 +298,7 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G const auto& variable = graph.getVariable(variable_uuid); variable_values.push_back(variable.data()); result.variables.push_back(elimination_order.at(variable_uuid)); - result.A.push_back(fuse_core::MatrixXd(row_count, variable.size())); + result.A.emplace_back(row_count, variable.size()); jacobians.push_back(result.A.back().data()); } result.b = fuse_core::VectorXd(row_count); @@ -308,9 +307,9 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G bool success = cost_function->Evaluate(variable_values.data(), result.b.data(), jacobians.data()); delete cost_function; success = success && result.b.array().isFinite().all(); - for (const auto& A : result.A) + for (const auto& a : result.A) { - success = success && A.array().isFinite().all(); + success = success && a.array().isFinite().all(); } if (!success) { @@ -352,46 +351,43 @@ LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::G delete local_parameterization; } #else - auto manifold = variable.manifold(); + auto* manifold = variable.manifold(); auto& jacobian = result.A[index]; if (variable.holdConstant()) { - if (manifold) + if (manifold != nullptr) { jacobian.resize(Eigen::NoChange, manifold->TangentSize()); } jacobian.setZero(); } - else if (manifold) - { - fuse_core::MatrixXd J(manifold->AmbientSize(), manifold->TangentSize()); - manifold->PlusJacobian(variable_values[index], J.data()); - jacobian *= J; - } - if (manifold) + else if (manifold != nullptr) { - delete manifold; + fuse_core::MatrixXd j(manifold->AmbientSize(), manifold->TangentSize()); + manifold->PlusJacobian(variable_values[index], j.data()); + jacobian *= j; } + delete manifold; #endif } // Correct A and b for the effects of the loss function - auto loss_function = constraint.lossFunction(); - if (loss_function) + auto* loss_function = constraint.lossFunction(); + if (loss_function != nullptr) { - double squared_norm = result.b.squaredNorm(); + double const squared_norm = result.b.squaredNorm(); double rho[3]; loss_function->Evaluate(squared_norm, rho); if (fuse_core::Loss::Ownership == ceres::Ownership::TAKE_OWNERSHIP) { delete loss_function; } - double sqrt_rho1 = std::sqrt(rho[1]); + double const sqrt_rho1 = std::sqrt(rho[1]); double alpha = 0.0; if ((squared_norm > 0.0) && (rho[2] > 0.0)) { - const double D = 1.0 + 2.0 * squared_norm * rho[2] / rho[1]; - alpha = 1.0 - std::sqrt(D); + const double d = 1.0 + 2.0 * squared_norm * rho[2] / rho[1]; + alpha = 1.0 - std::sqrt(d); } // Correct the Jacobians @@ -467,35 +463,35 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) auto column_offsets = std::vector(); column_offsets.reserve(dense_to_index.size() + 1ul); column_offsets.push_back(0u); - for (size_t dense = 0; dense < dense_to_index.size(); ++dense) + for (unsigned int const dense : dense_to_index) { - column_offsets.push_back(column_offsets.back() + index_to_cols[dense_to_index[dense]]); + column_offsets.push_back(column_offsets.back() + index_to_cols[dense]); } // Construct the Ab matrix - fuse_core::MatrixXd Ab = fuse_core::MatrixXd::Zero(row_offsets.back(), column_offsets.back() + 1u); + fuse_core::MatrixXd ab = fuse_core::MatrixXd::Zero(row_offsets.back(), column_offsets.back() + 1u); for (size_t term_index = 0ul; term_index < linear_terms.size(); ++term_index) { const auto& linear_term = linear_terms[term_index]; auto row_offset = row_offsets[term_index]; for (size_t i = 0ul; i < linear_term.variables.size(); ++i) { - const auto& A = linear_term.A[i]; + const auto& a = linear_term.A[i]; auto dense = index_to_dense[linear_term.variables[i]]; auto column_offset = column_offsets[dense]; - for (int row = 0; row < A.rows(); ++row) + for (int row = 0; row < a.rows(); ++row) { - for (int col = 0; col < A.cols(); ++col) + for (int col = 0; col < a.cols(); ++col) { - Ab(row_offset + row, column_offset + col) = A(row, col); + ab(row_offset + row, column_offset + col) = a(row, col); } } } const auto& b = linear_term.b; - int column_offset = column_offsets.back(); + int const column_offset = static_cast(column_offsets.back()); for (int row = 0; row < b.rows(); ++row) { - Ab(row_offset + row, column_offset) = b(row); + ab(row_offset + row, column_offset) = b(row); } } @@ -508,13 +504,13 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) using MatrixType = fuse_core::MatrixXd; using HCoeffsType = Eigen::internal::plain_diag_type::type; using RowVectorType = Eigen::internal::plain_row_type::type; - auto rows = Ab.rows(); - auto cols = Ab.cols(); + auto rows = ab.rows(); + auto cols = ab.cols(); auto size = std::min(rows, cols); - auto hCoeffs = HCoeffsType(size); + auto h_coeffs = HCoeffsType(size); auto temp = RowVectorType(cols); - Eigen::internal::householder_qr_inplace_blocked::run(Ab, hCoeffs, 48, temp.data()); - Ab.triangularView().setZero(); // Zero out the below-diagonal elements + Eigen::internal::householder_qr_inplace_blocked::run(ab, h_coeffs, 48, temp.data()); + ab.triangularView().setZero(); // Zero out the below-diagonal elements } // Extract the marginal term from R (now stored in Ab) @@ -522,7 +518,7 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) // The remaining rows are the marginal on the remaining variables: P(y, z, ...) auto min_row = column_offsets[1]; // However, depending on the input, not all rows may be usable. - auto max_row = std::min(Ab.rows(), Ab.cols() - 1); // -1 for the included b vector + auto max_row = std::min(ab.rows(), ab.cols() - 1); // -1 for the included b vector auto marginal_rows = max_row - min_row; auto marginal_term = LinearTerm(); if (marginal_rows > 0) @@ -535,22 +531,22 @@ LinearTerm marginalizeNext(const std::vector& linear_terms) { auto index = dense_to_index[dense]; marginal_term.variables.push_back(index); - fuse_core::MatrixXd A = fuse_core::MatrixXd::Zero(marginal_rows, index_to_cols[index]); + fuse_core::MatrixXd a = fuse_core::MatrixXd::Zero(marginal_rows, index_to_cols[index]); auto column_offset = column_offsets[dense]; - for (int row = 0; row < A.rows(); ++row) + for (int row = 0; row < a.rows(); ++row) { - for (int col = 0; col < A.cols(); ++col) + for (int col = 0; col < a.cols(); ++col) { - A(row, col) = Ab(min_row + row, column_offset + col); + a(row, col) = ab(min_row + row, column_offset + col); } } - marginal_term.A.push_back(std::move(A)); + marginal_term.A.push_back(std::move(a)); } marginal_term.b = fuse_core::VectorXd::Zero(marginal_rows); auto column_offset = column_offsets.back(); for (int row = 0; row < marginal_term.b.rows(); ++row) { - marginal_term.b(row) = Ab(min_row + row, column_offset); + marginal_term.b(row) = ab(min_row + row, column_offset); } } return marginal_term; diff --git a/fuse_constraints/test/test_marginalize_variables.cpp b/fuse_constraints/test/test_marginalize_variables.cpp index 2530a887b..369b906a1 100644 --- a/fuse_constraints/test/test_marginalize_variables.cpp +++ b/fuse_constraints/test/test_marginalize_variables.cpp @@ -71,12 +71,12 @@ class GenericVariable : public fuse_core::Variable { } - size_t size() const override + [[nodiscard]] size_t size() const override { return 1; } - const double* data() const override + [[nodiscard]] const double* data() const override { return &data_; } @@ -138,7 +138,7 @@ class GenericConstraint : public fuse_core::Constraint { } - ceres::CostFunction* costFunction() const override + [[nodiscard]] ceres::CostFunction* costFunction() const override { return nullptr; } @@ -173,7 +173,7 @@ class FixedOrientation3DStamped : public fuse_variables::Orientation3DStamped { } - bool holdConstant() const override + [[nodiscard]] bool holdConstant() const override { return true; } @@ -355,16 +355,16 @@ TEST(MarginalizeVariables, Linearize) auto actual = fuse_constraints::detail::linearize(*constraint, graph, elimination_order); // Define the expected values - fuse_core::MatrixXd expected_A0(3, 3); + fuse_core::MatrixXd expected_a0(3, 3); /* *INDENT-OFF* */ - expected_A0 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, 0.27712824947756498073, + expected_a0 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, 0.27712824947756498073, -0.65053818745824854020, -2.5352112979770691226e-07, 7.1505019336171038447e-08, 2.5546009342625186633e-07, -0.57735026918958520792; /* *INDENT-ON* */ - fuse_core::MatrixXd expected_A1(3, 3); + fuse_core::MatrixXd expected_a1(3, 3); /* *INDENT-OFF* */ - expected_A1 << 0.99999999999996114219, -1.8482708254510815671e-07, -2.873543621662033587e-07, + expected_a1 << 0.99999999999996114219, -1.8482708254510815671e-07, -2.873543621662033587e-07, 1.3069243487082160549e-07, 0.70710678118650927004, -2.5352115484711390536e-07, 1.6590414383954588118e-07, 2.0699913566568639567e-07, 0.57735026918958520792; /* *INDENT-ON* */ @@ -377,8 +377,8 @@ TEST(MarginalizeVariables, Linearize) EXPECT_EQ(3u, actual.variables[0]); EXPECT_EQ(1u, actual.variables[1]); - EXPECT_MATRIX_NEAR(expected_A0, actual.A[0], 1.0e-9); - EXPECT_MATRIX_NEAR(expected_A1, actual.A[1], 1.0e-9); + EXPECT_MATRIX_NEAR(expected_a0, actual.A[0], 1.0e-9); + EXPECT_MATRIX_NEAR(expected_a1, actual.A[1], 1.0e-9); EXPECT_MATRIX_NEAR(expected_b, actual.b, 1.0e-9); } @@ -387,13 +387,13 @@ TEST(MarginalizeVariables, MarginalizeNext) // Construct a couple of linear terms auto term1 = fuse_constraints::detail::LinearTerm(); term1.variables.push_back(1); - auto A1 = fuse_core::MatrixXd(3, 3); + auto a1 = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A1 << 0.99999999999999922284, 4.4999993911720714834e-08, -2.9999995598828377297e-08, -3.181980062078038074e-08, + a1 << 0.99999999999999922284, 4.4999993911720714834e-08, -2.9999995598828377297e-08, -3.181980062078038074e-08, 0.70710678118654701763, 1.0606600528428877794e-08, 1.7320505793505525105e-08, -8.6602525498080673572e-09, 0.57735026918962550901; /* *INDENT-ON* */ - term1.A.push_back(A1); + term1.A.push_back(a1); auto b1 = fuse_core::VectorXd(3); b1 << -2.9999995786018886696e-08, -4.2426400911723613558e-08, -5.1961516896187549911e-08; term1.b = b1; @@ -401,20 +401,20 @@ TEST(MarginalizeVariables, MarginalizeNext) auto term2 = fuse_constraints::detail::LinearTerm(); term2.variables.push_back(3); term2.variables.push_back(1); - auto A21 = fuse_core::MatrixXd(3, 3); + auto a21 = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A21 << 0.99999999999996114219, -1.8482708254510815671e-07, -2.873543621662033587e-07, 1.3069243487082160549e-07, + a21 << 0.99999999999996114219, -1.8482708254510815671e-07, -2.873543621662033587e-07, 1.3069243487082160549e-07, 0.70710678118650927004, -2.5352115484711390536e-07, 1.6590414383954588118e-07, 2.0699913566568639567e-07, 0.57735026918958520792; /* *INDENT-ON* */ - auto A22 = fuse_core::MatrixXd(3, 3); + auto a22 = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A22 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, 0.27712824947756498073, + a22 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, 0.27712824947756498073, -0.6505381874582485402, -2.5352112979770691226e-07, 7.1505019336171038447e-08, 2.5546009342625186633e-07, -0.57735026918958520792; /* *INDENT-ON* */ - term2.A.push_back(A21); - term2.A.push_back(A22); + term2.A.push_back(a21); + term2.A.push_back(a22); auto b2 = fuse_core::VectorXd(3); b2 << 7.1706607563166841896e-07, -4.0638046747479327072e-07, 2.1341989211309879704e-07; term2.b = b2; @@ -429,12 +429,12 @@ TEST(MarginalizeVariables, MarginalizeNext) // Define the expected marginal auto expected = fuse_constraints::detail::LinearTerm(); expected.variables.push_back(3); - auto A_expected = fuse_core::MatrixXd(3, 3); + auto a_expected = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A_expected << -0.686835139329528, 0.064384601986636, 0.000000153209328, 0.000000000000000, -0.509885650799691, + a_expected << -0.686835139329528, 0.064384601986636, 0.000000153209328, 0.000000000000000, -0.509885650799691, 0.000000079984512, 0.000000000000000, 0.000000000000000, 0.408248290463911; /* *INDENT-ON* */ - expected.A.push_back(A_expected); + expected.A.push_back(a_expected); auto b_expected = fuse_core::VectorXd(3); b_expected << -0.000000497197868, 0.000000315186479, 0.000000114168337; expected.b = b_expected; @@ -451,22 +451,22 @@ TEST(MarginalizeVariables, MarginalizeNext) TEST(MarginalizeVariables, MarginalizeVariables) { // Create variables - auto x1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(1.0)); + auto x1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(1, 0)); x1->w() = 0.927362; x1->x() = 0.1; x1->y() = 0.2; x1->z() = 0.3; - auto x2 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(2.0)); + auto x2 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(2, 0)); x2->w() = 0.848625; x2->x() = 0.13798; x2->y() = 0.175959; x2->z() = 0.479411; - auto x3 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3.0)); + auto x3 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3, 0)); x3->w() = 0.735597; x3->x() = 0.170384; x3->y() = 0.144808; x3->z() = 0.63945; - auto l1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3.5)); + auto l1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3, 500000000)); l1->w() = 0.803884; l1->x() = 0.304917; l1->y() = 0.268286; diff --git a/fuse_core/include/fuse_core/async_publisher.hpp b/fuse_core/include/fuse_core/async_publisher.hpp index af016b583..ff5c0bc05 100644 --- a/fuse_core/include/fuse_core/async_publisher.hpp +++ b/fuse_core/include/fuse_core/async_publisher.hpp @@ -34,10 +34,8 @@ #ifndef FUSE_CORE__ASYNC_PUBLISHER_HPP_ #define FUSE_CORE__ASYNC_PUBLISHER_HPP_ -#include #include #include -#include #include #include @@ -73,6 +71,10 @@ class AsyncPublisher : public Publisher * @brief Destructor */ virtual ~AsyncPublisher(); + AsyncPublisher(AsyncPublisher const&) = delete; + AsyncPublisher(AsyncPublisher&&) = delete; + AsyncPublisher& operator=(AsyncPublisher const&) = delete; + AsyncPublisher& operator=(AsyncPublisher&&) = delete; /** * @brief Initialize the AsyncPublisher object @@ -203,6 +205,7 @@ class AsyncPublisher : public Publisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ + // NOLINTNEXTLINE virtual void notifyCallback(Transaction::ConstSharedPtr /*transaction*/, Graph::ConstSharedPtr /*graph*/) { } @@ -230,7 +233,7 @@ class AsyncPublisher : public Publisher 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/async_sensor_model.hpp b/fuse_core/include/fuse_core/async_sensor_model.hpp index c550fe72e..c83b7c6d4 100644 --- a/fuse_core/include/fuse_core/async_sensor_model.hpp +++ b/fuse_core/include/fuse_core/async_sensor_model.hpp @@ -34,7 +34,6 @@ #ifndef FUSE_CORE__ASYNC_SENSOR_MODEL_HPP_ #define FUSE_CORE__ASYNC_SENSOR_MODEL_HPP_ -#include #include #include @@ -97,6 +96,10 @@ class AsyncSensorModel : public SensorModel * @brief Destructor */ virtual ~AsyncSensorModel(); + AsyncSensorModel(AsyncSensorModel const&) = delete; + AsyncSensorModel(AsyncSensorModel&&) = delete; + AsyncSensorModel& operator=(AsyncSensorModel const&) = delete; + AsyncSensorModel& operator=(AsyncSensorModel&&) = delete; /** * @brief Function to be executed whenever the optimizer has completed a Graph update @@ -236,6 +239,7 @@ class AsyncSensorModel : public SensorModel * @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*/) { } @@ -281,7 +285,7 @@ class AsyncSensorModel : public SensorModel 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/graph.hpp b/fuse_core/include/fuse_core/graph.hpp index 6e73ffc5d..d4ad20815 100644 --- a/fuse_core/include/fuse_core/graph.hpp +++ b/fuse_core/include/fuse_core/graph.hpp @@ -355,7 +355,7 @@ class Graph virtual void getCovariance(const std::vector>& covariance_requests, std::vector>& covariance_matrices, const ceres::Covariance::Options& options = ceres::Covariance::Options(), - const bool use_tangent_space = true) const = 0; + bool use_tangent_space = true) const = 0; /** * @brief Update the graph with the contents of a transaction diff --git a/fuse_core/include/fuse_core/sensor_model.hpp b/fuse_core/include/fuse_core/sensor_model.hpp index 935f0ea24..61b7bfbd5 100644 --- a/fuse_core/include/fuse_core/sensor_model.hpp +++ b/fuse_core/include/fuse_core/sensor_model.hpp @@ -76,6 +76,10 @@ class SensorModel * @brief Destructor */ virtual ~SensorModel() = default; + SensorModel(SensorModel const&) = default; + SensorModel(SensorModel&&) = default; + SensorModel& operator=(SensorModel const&) = default; + SensorModel& operator=(SensorModel&&) = default; /** * @brief Function to be executed whenever the optimizer has completed a Graph update @@ -91,6 +95,7 @@ class SensorModel * @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*/) { } @@ -113,7 +118,7 @@ class SensorModel /** * @brief Get the unique name of this sensor */ - virtual const std::string& name() const = 0; + [[nodiscard]] virtual const std::string& name() const = 0; /** * @brief Function to be executed whenever the optimizer is ready to receive transactions diff --git a/fuse_core/include/fuse_core/timestamp_manager.hpp b/fuse_core/include/fuse_core/timestamp_manager.hpp index 758395270..d084b8700 100644 --- a/fuse_core/include/fuse_core/timestamp_manager.hpp +++ b/fuse_core/include/fuse_core/timestamp_manager.hpp @@ -112,6 +112,11 @@ class TimestampManager explicit TimestampManager(MotionModelFunction generator, const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); + TimestampManager(TimestampManager const&) = default; + TimestampManager(TimestampManager&&) = default; + TimestampManager& operator=(TimestampManager const&) = default; + TimestampManager& operator=(TimestampManager&&) = default; + /** * @brief Constructor that accepts the motion model generator as a member function pointer and * object pointer diff --git a/fuse_core/include/fuse_core/variable.hpp b/fuse_core/include/fuse_core/variable.hpp index 550021977..9f05f76db 100644 --- a/fuse_core/include/fuse_core/variable.hpp +++ b/fuse_core/include/fuse_core/variable.hpp @@ -36,7 +36,6 @@ #include #include -#include #include #include @@ -224,11 +223,15 @@ class Variable * @brief Destructor */ virtual ~Variable() = default; + Variable(Variable const&) = default; + Variable(Variable&&) = default; + Variable& operator=(Variable const&) = default; + Variable& operator=(Variable&&) = default; /** * @brief Returns a UUID for this variable. */ - const UUID& uuid() const + [[nodiscard]] const UUID& uuid() const { return uuid_; } @@ -247,7 +250,7 @@ class Variable * To make this easy to implement in all derived classes, the FUSE_VARIABLE_TYPE_DEFINITION() * and FUSE_VARIABLE_DEFINITIONS() macro functions have been provided. */ - virtual std::string type() const = 0; + [[nodiscard]] virtual std::string type() const = 0; /** * @brief Returns the number of elements of this variable. @@ -257,7 +260,7 @@ class Variable * exception is a 3D rotation represented as a quaternion. It only has 3 degrees of freedom, * but it is represented as four elements, (w, x, y, z), so it's size will be 4. */ - virtual size_t size() const = 0; + [[nodiscard]] virtual size_t size() const = 0; /** * @brief Returns the number of elements of the local parameterization space. @@ -266,7 +269,7 @@ class Variable * override the \p localSize() method. By default, the \p size() method is used for \p * localSize() as well. */ - virtual size_t localSize() const + [[nodiscard]] virtual size_t localSize() const { return size(); } @@ -279,7 +282,7 @@ class Variable * Variable::size() elements will be accessed externally. This interface is provided for * integration with Ceres, which uses raw pointers. */ - virtual const double* data() const = 0; + [[nodiscard]] virtual const double* data() const = 0; /** * @brief Read-write access to the variable data @@ -314,7 +317,7 @@ class Variable * * @return A unique pointer to a new instance of the most-derived Variable */ - virtual Variable::UniquePtr clone() const = 0; + [[nodiscard]] virtual Variable::UniquePtr clone() const = 0; /** * @brief Create a new Ceres local parameterization object to apply to updates of this variable @@ -331,7 +334,7 @@ class Variable * * @return A base pointer to an instance of a derived LocalParameterization */ - virtual fuse_core::LocalParameterization* localParameterization() const + [[nodiscard]] virtual fuse_core::LocalParameterization* localParameterization() const { return nullptr; } @@ -354,21 +357,19 @@ class Variable * * @return A base pointer to an instance of a derived Manifold */ - virtual fuse_core::Manifold* manifold() const + [[nodiscard]] virtual fuse_core::Manifold* manifold() const { // To support legacy Variable classes that still implements the localParameterization() method, // construct a ManifoldAdapter object from the LocalParameterization pointer as the default // action. If the Variable has been updated to use the new Manifold classes, then the Variable // should override this method and return a pointer to the appropriate derived Manifold object. - auto local_parameterization = localParameterization(); - if (!local_parameterization) + auto* local_parameterization = localParameterization(); + if (local_parameterization == nullptr) { return nullptr; } - else - { - return new fuse_core::ManifoldAdapter(local_parameterization); - } + + return new fuse_core::ManifoldAdapter(local_parameterization); } #endif @@ -455,7 +456,7 @@ class Variable virtual void deserialize(fuse_core::TextInputArchive& /* archive */) = 0; private: - fuse_core::UUID uuid_; //!< The unique ID number for this variable + fuse_core::UUID uuid_{}; //!< The unique ID number for this variable // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_core/src/async_publisher.cpp b/fuse_core/src/async_publisher.cpp index cf174247a..df16c96f9 100644 --- a/fuse_core/src/async_publisher.cpp +++ b/fuse_core/src/async_publisher.cpp @@ -43,7 +43,7 @@ AsyncPublisher::AsyncPublisher(size_t thread_count) : name_("uninitialized"), ex AsyncPublisher::~AsyncPublisher() { - internal_stop(); + internalStop(); } void AsyncPublisher::initialize(node_interfaces::NodeInterfaces interfaces, @@ -131,12 +131,12 @@ void AsyncPublisher::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 AsyncPublisher::internal_stop() +void AsyncPublisher::internalStop() { if (spinner_.joinable()) { diff --git a/fuse_core/src/async_sensor_model.cpp b/fuse_core/src/async_sensor_model.cpp index 9f9903737..bf71909ae 100644 --- a/fuse_core/src/async_sensor_model.cpp +++ b/fuse_core/src/async_sensor_model.cpp @@ -51,7 +51,7 @@ AsyncSensorModel::AsyncSensorModel(size_t thread_count) : name_("uninitialized") AsyncSensorModel::~AsyncSensorModel() { - internal_stop(); + internalStop(); } void AsyncSensorModel::initialize(node_interfaces::NodeInterfaces interfaces, @@ -144,12 +144,12 @@ void AsyncSensorModel::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 AsyncSensorModel::internal_stop() +void AsyncSensorModel::internalStop() { if (spinner_.joinable()) { diff --git a/fuse_core/src/timestamp_manager.cpp b/fuse_core/src/timestamp_manager.cpp index 80031b584..93c9297bf 100644 --- a/fuse_core/src/timestamp_manager.cpp +++ b/fuse_core/src/timestamp_manager.cpp @@ -44,7 +44,7 @@ namespace fuse_core { TimestampManager::TimestampManager(MotionModelFunction generator, const rclcpp::Duration& buffer_length) - : generator_(generator), buffer_length_(buffer_length) + : generator_(std::move(generator)), buffer_length_(buffer_length) { } @@ -155,11 +155,11 @@ void TimestampManager::query(Transaction& transaction, bool update_variables) TimestampManager::const_stamp_range TimestampManager::stamps() const { - std::function extract_stamp = + std::function const extract_stamp = [](const MotionModelHistory::value_type& element) -> const rclcpp::Time& { return element.first; }; - return const_stamp_range(boost::make_transform_iterator(motion_model_history_.begin(), extract_stamp), - boost::make_transform_iterator(motion_model_history_.end(), extract_stamp)); + return { boost::make_transform_iterator(motion_model_history_.begin(), extract_stamp), + boost::make_transform_iterator(motion_model_history_.end(), extract_stamp) }; } void TimestampManager::addSegment(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, @@ -203,8 +203,8 @@ void TimestampManager::removeSegment(MotionModelHistory::iterator& iter, Transac void TimestampManager::splitSegment(MotionModelHistory::iterator& iter, const rclcpp::Time& stamp, Transaction& transaction) { - rclcpp::Time removed_beginning_stamp = iter->second.beginning_stamp; - rclcpp::Time removed_ending_stamp = iter->second.ending_stamp; + rclcpp::Time const removed_beginning_stamp = iter->second.beginning_stamp; + rclcpp::Time const removed_ending_stamp = iter->second.ending_stamp; // We need to remove the existing constraint. removeSegment(iter, transaction); // And add a new constraint from the beginning of the removed constraint to the provided stamp @@ -226,7 +226,7 @@ void TimestampManager::purgeHistory() // (a) are left with only one entry, OR // (b) the time delta between the beginning and end is within the buffer_length_ We compare with // the ending timestamp of each segment to be conservative - rclcpp::Time ending_stamp = motion_model_history_.rbegin()->first; + rclcpp::Time const ending_stamp = motion_model_history_.rbegin()->first; while ((motion_model_history_.size() > 1) && ((ending_stamp - motion_model_history_.begin()->second.ending_stamp) > buffer_length_)) { diff --git a/fuse_core/src/transaction.cpp b/fuse_core/src/transaction.cpp index dc1c088a6..39b6a3293 100644 --- a/fuse_core/src/transaction.cpp +++ b/fuse_core/src/transaction.cpp @@ -47,10 +47,8 @@ const rclcpp::Time& Transaction::minStamp() const { return stamp_; } - else - { - return std::min(*involved_stamps_.begin(), stamp_); - } + + return std::min(*involved_stamps_.begin(), stamp_); } const rclcpp::Time& Transaction::maxStamp() const @@ -59,10 +57,8 @@ const rclcpp::Time& Transaction::maxStamp() const { return stamp_; } - else - { - return std::max(*involved_stamps_.rbegin(), stamp_); - } + + return std::max(*involved_stamps_.rbegin(), stamp_); } void Transaction::addInvolvedStamp(const rclcpp::Time& stamp) @@ -72,11 +68,11 @@ void Transaction::addInvolvedStamp(const rclcpp::Time& stamp) Transaction::const_constraint_range Transaction::addedConstraints() const { - std::function to_constraint_ref = + std::function const to_constraint_ref = [](const Constraint::SharedPtr& constraint) -> const Constraint& { return *constraint; }; - return const_constraint_range(boost::make_transform_iterator(added_constraints_.cbegin(), to_constraint_ref), - boost::make_transform_iterator(added_constraints_.cend(), to_constraint_ref)); + return { boost::make_transform_iterator(added_constraints_.cbegin(), to_constraint_ref), + boost::make_transform_iterator(added_constraints_.cend(), to_constraint_ref) }; } void Transaction::addConstraint(Constraint::SharedPtr constraint, bool overwrite) @@ -130,11 +126,11 @@ void Transaction::removeConstraint(const UUID& constraint_uuid) Transaction::const_variable_range Transaction::addedVariables() const { - std::function to_variable_ref = + std::function const to_variable_ref = [](const Variable::SharedPtr& variable) -> const Variable& { return *variable; }; - return const_variable_range(boost::make_transform_iterator(added_variables_.cbegin(), to_variable_ref), - boost::make_transform_iterator(added_variables_.cend(), to_variable_ref)); + return { boost::make_transform_iterator(added_variables_.cbegin(), to_variable_ref), + boost::make_transform_iterator(added_variables_.cend(), to_variable_ref) }; } bool Transaction::empty() const diff --git a/fuse_core/test/example_variable.hpp b/fuse_core/test/example_variable.hpp index a0e6b80c0..4cf04aa8b 100644 --- a/fuse_core/test/example_variable.hpp +++ b/fuse_core/test/example_variable.hpp @@ -79,7 +79,7 @@ class ExampleVariable : public fuse_core::Variable * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -127,8 +127,8 @@ class LegacyParameterization : public fuse_core::LocalParameterization bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { double q_delta[4]; - ceres::AngleAxisToQuaternion(delta, q_delta); - ceres::QuaternionProduct(x, q_delta, x_plus_delta); + ceres::AngleAxisToQuaternion(delta, static_cast(q_delta)); + ceres::QuaternionProduct(x, static_cast(q_delta), x_plus_delta); return true; } @@ -164,8 +164,8 @@ class LegacyParameterization : public fuse_core::LocalParameterization x_inverse[3] = -x[3]; double q_delta[4]; - ceres::QuaternionProduct(x_inverse, y, q_delta); - ceres::QuaternionToAngleAxis(q_delta, y_minus_x); + ceres::QuaternionProduct(static_cast(x_inverse), y, static_cast(q_delta)); + ceres::QuaternionToAngleAxis(static_cast(q_delta), y_minus_x); return true; } @@ -219,17 +219,17 @@ class LegacyVariable : public fuse_core::Variable { } - size_t size() const override + [[nodiscard]] size_t size() const override { return 4; } - const double* data() const override + [[nodiscard]] const double* data() const override { - return data_; + return static_cast(data_); } double* data() override { - return data_; + return static_cast(data_); } void print(std::ostream& /*stream = std::cout*/) const override { @@ -251,7 +251,7 @@ class LegacyVariable : public fuse_core::Variable * * @return A pointer to a local parameterization object that indicates how to "add" increments to the quaternion */ - fuse_core::LocalParameterization* localParameterization() const override + [[nodiscard]] fuse_core::LocalParameterization* localParameterization() const override { return new LegacyParameterization(); } diff --git a/fuse_core/test/test_variable.cpp b/fuse_core/test/test_variable.cpp index 4198913a6..d2eb4cc34 100644 --- a/fuse_core/test/test_variable.cpp +++ b/fuse_core/test/test_variable.cpp @@ -45,7 +45,7 @@ TEST(Variable, Type) { - ExampleVariable variable; + ExampleVariable const variable; ASSERT_EQ("ExampleVariable", variable.type()); } @@ -82,7 +82,7 @@ TEST(LegacyVariable, Serialization) #if CERES_SUPPORTS_MANIFOLDS struct QuaternionCostFunction { - explicit QuaternionCostFunction(double* observation) + explicit QuaternionCostFunction(const double* observation) { observation_[0] = observation[0]; observation_[1] = observation[1]; @@ -109,7 +109,7 @@ struct QuaternionCostFunction return true; } - double observation_[4]; + double observation_[4]{}; }; TEST(LegacyVariable, ManifoldAdapter) @@ -123,19 +123,19 @@ TEST(LegacyVariable, ManifoldAdapter) // Create a simple a constraint with an identity quaternion double target_quat[4] = { 1.0, 0.0, 0.0, 0.0 }; - ceres::CostFunction* cost_function = - new ceres::AutoDiffCostFunction(new QuaternionCostFunction(target_quat)); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction( + new QuaternionCostFunction(static_cast(target_quat))); // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.manifold()); + problem.AddParameterBlock(orientation.data(), static_cast(orientation.size()), orientation.manifold()); std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver ceres::Solver::Summary summary; - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solve(options, &problem, &summary); // Check @@ -182,7 +182,7 @@ TEST(LegacyVariable, Deserialization) fuse_core::Manifold* actual_manifold = nullptr; ASSERT_NO_THROW(actual_manifold = actual.manifold()); ASSERT_NE(actual_manifold, nullptr); - auto actual_manifold_adapter = dynamic_cast(actual_manifold); + auto* actual_manifold_adapter = dynamic_cast(actual_manifold); ASSERT_NE(actual_manifold_adapter, nullptr); } #endif diff --git a/fuse_graphs/CMakeLists.txt b/fuse_graphs/CMakeLists.txt index 45838b883..9057891bc 100644 --- a/fuse_graphs/CMakeLists.txt +++ b/fuse_graphs/CMakeLists.txt @@ -20,58 +20,6 @@ find_package(Ceres REQUIRED) include(boost-extras.cmake) -# ############################################################################## -# Build ## -# ############################################################################## -# lint_cmake: -linelength Disable warnings about maybe uninitialized variables -# with -Wno-maybe-uninitialized until we fix the following error: -# -# In file included from include/c++/12.2.0/functional:59, from -# include/eigen3/Eigen/Core:85, from include/fuse_core/fuse_macros.h:63, from -# include/fuse_core/loss.h:37, from include/fuse_core/constraint.h:37, from -# src/fuse/fuse_graphs/include/fuse_graphs/hash_graph.h:38, from -# src/fuse/fuse_graphs/src/hash_graph.cpp:34: In copy constructor -# ‘std::function<_Res(_ArgTypes ...)>::function(const -# std::function<_Res(_ArgTypes ...)>&) [with _Res = const fuse_core::Variable&; -# _ArgTypes = {const std::pair >&}]’, inlined from -# ‘boost::iterators::transform_iterator::transform_iterator(const Iterator&, UnaryFunc) [with UnaryFunc = -# std::function >&)>; Iterator = -# std::__detail::_Node_const_iterator >, false, true>; Reference = -# boost::use_default; Value = boost::use_default]’ at -# include/boost/iterator/transform_iterator.hpp:96:21, inlined from -# ‘boost::iterators::transform_iterator -# boost::iterators::make_transform_iterator(Iterator, UnaryFunc) [with UnaryFunc -# = std::function >&)>; Iterator = -# std::__detail::_Node_const_iterator >, false, true>]’ at -# include/boost/iterator/transform_iterator.hpp:141:61, inlined from ‘virtual -# fuse_core::Graph::const_variable_range fuse_graphs::HashGraph::getVariables() -# const’ at fuse/fuse_graphs/src/hash_graph.cpp:284:35: -# /nix/store/b7hvml0m3qmqraz1022fwvyyg6fc1vdy-gcc-12.2.0/include/c++/12.2.0/bits/std_function.h:391:17: -# error: ‘’ may be used uninitialized [-Werror=maybe-uninitialized] -# 391 | __x._M_manager(_M_functor, __x._M_functor, __clone_functor); -# | ~~~~^~~~~~~~~~ -# /nix/store/b7hvml0m3qmqraz1022fwvyyg6fc1vdy-gcc-12.2.0/include/c++/12.2.0/bits/std_function.h: -# In member function ‘virtual fuse_core::Graph::const_variable_range -# fuse_graphs::HashGraph::getVariables() const’: -# /nix/store/b7hvml0m3qmqraz1022fwvyyg6fc1vdy-gcc-12.2.0/include/c++/12.2.0/bits/std_function.h:267:7: -# note: by argument 2 of type ‘const std::_Any_data&’ to ‘static bool -# std::_Function_handler<_Res(_ArgTypes ...), -# _Functor>::_M_manager(std::_Any_data&, const std::_Any_data&, -# std::_Manager_operation) [with _Res = const fuse_core::Variable&; _Functor = -# fuse_graphs::HashGraph::getVariables() const::, -# boost::hash >::value_type&)>; _ArgTypes = {const -# std::pair >&}]’ -# declared here 267 | _M_manager(_Any_data& __dest, const _Any_data& -# __source, | ^~~~~~~~~~ -add_compile_options(-Wall -Werror -Wno-maybe-uninitialized) - # fuse_graphs library add_library(${PROJECT_NAME} src/hash_graph.cpp) target_include_directories( diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index 14e385447..b99ae0aeb 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -63,13 +63,13 @@ HashGraph::HashGraph(const HashGraph& other) { // Make a deep copy of the constraints std::transform(other.constraints_.begin(), other.constraints_.end(), std::inserter(constraints_, constraints_.end()), - [](const Constraints::value_type& uuid__constraint) -> Constraints::value_type { - return { uuid__constraint.first, uuid__constraint.second->clone() }; + [](const Constraints::value_type& uuid_constraint) -> Constraints::value_type { + return { uuid_constraint.first, uuid_constraint.second->clone() }; }); // NOLINT(whitespace/braces) // Make a deep copy of the variables std::transform(other.variables_.begin(), other.variables_.end(), std::inserter(variables_, variables_.end()), - [](const Variables::value_type& uuid__variable) -> Variables::value_type { - return { uuid__variable.first, uuid__variable.second->clone() }; + [](const Variables::value_type& uuid_variable) -> Variables::value_type { + return { uuid_variable.first, uuid_variable.second->clone() }; }); // NOLINT(whitespace/braces) } @@ -165,14 +165,13 @@ const fuse_core::Constraint& HashGraph::getConstraint(const fuse_core::UUID& con fuse_core::Graph::const_constraint_range HashGraph::getConstraints() const noexcept { - std::function to_constraint_ref = - [](const Constraints::value_type& uuid__constraint) -> const fuse_core::Constraint& { - return *uuid__constraint.second; + std::function const to_constraint_ref = + [](const Constraints::value_type& uuid_constraint) -> const fuse_core::Constraint& { + return *uuid_constraint.second; }; - return fuse_core::Graph::const_constraint_range( - boost::make_transform_iterator(constraints_.cbegin(), to_constraint_ref), - boost::make_transform_iterator(constraints_.cend(), to_constraint_ref)); + return { boost::make_transform_iterator(constraints_.cbegin(), to_constraint_ref), + boost::make_transform_iterator(constraints_.cend(), to_constraint_ref) }; } fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(const fuse_core::UUID& variable_uuid) const @@ -180,29 +179,26 @@ fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(cons auto cross_reference_iter = constraints_by_variable_uuid_.find(variable_uuid); if (cross_reference_iter != constraints_by_variable_uuid_.end()) { - std::function uuid_to_constraint_ref = + std::function const uuid_to_constraint_ref = [this](const fuse_core::UUID& constraint_uuid) -> const fuse_core::Constraint& { return this->getConstraint(constraint_uuid); }; const auto& constraints = cross_reference_iter->second; - return fuse_core::Graph::const_constraint_range( - boost::make_transform_iterator(constraints.cbegin(), uuid_to_constraint_ref), - boost::make_transform_iterator(constraints.cend(), uuid_to_constraint_ref)); + return { boost::make_transform_iterator(constraints.cbegin(), uuid_to_constraint_ref), + boost::make_transform_iterator(constraints.cend(), uuid_to_constraint_ref) }; } - else if (variableExists(variable_uuid)) + if (variableExists(variable_uuid)) { // User requested a valid variable, but there are no attached constraints. Return an empty // range. - return fuse_core::Graph::const_constraint_range(); - } - else - { - // We only want to throw if the requested variable does not exist. - throw std::logic_error("Attempting to access constraints connected to variable (" + - fuse_core::uuid::to_string(variable_uuid) + - "), but that variable does not exist in this graph."); + return {}; } + + // We only want to throw if the requested variable does not exist. + throw std::logic_error("Attempting to access constraints connected to variable (" + + fuse_core::uuid::to_string(variable_uuid) + + "), but that variable does not exist in this graph."); } bool HashGraph::variableExists(const fuse_core::UUID& variable_uuid) const noexcept @@ -266,11 +262,11 @@ const fuse_core::Variable& HashGraph::getVariable(const fuse_core::UUID& variabl fuse_core::Graph::const_variable_range HashGraph::getVariables() const noexcept { - std::function to_variable_ref = - [](const Variables::value_type& uuid__variable) -> const fuse_core::Variable& { return *uuid__variable.second; }; + std::function const to_variable_ref = + [](const Variables::value_type& uuid_variable) -> const fuse_core::Variable& { return *uuid_variable.second; }; - return fuse_core::Graph::const_variable_range(boost::make_transform_iterator(variables_.cbegin(), to_variable_ref), - boost::make_transform_iterator(variables_.cend(), to_variable_ref)); + return { boost::make_transform_iterator(variables_.cbegin(), to_variable_ref), + boost::make_transform_iterator(variables_.cend(), to_variable_ref) }; } void HashGraph::holdVariable(const fuse_core::UUID& variable_uuid, bool hold_constant) @@ -418,7 +414,7 @@ ceres::Solver::Summary HashGraph::optimizeFor(const rclcpp::Duration& max_optimi auto created_problem = clock.now(); // Modify the options to enforce the maximum time - rclcpp::Duration remaining = max_optimization_time - (created_problem - start); + rclcpp::Duration const remaining = max_optimization_time - (created_problem - start); auto time_constrained_options = options; time_constrained_options.max_solver_time_in_seconds = std::max(0.0, remaining.seconds()); @@ -460,10 +456,10 @@ void HashGraph::print(std::ostream& stream) const void HashGraph::createProblem(ceres::Problem& problem) const { // Add all the variables to the problem - for (auto& uuid__variable : variables_) + for (auto const& uuid_variable : variables_) { - fuse_core::Variable& variable = *(uuid__variable.second); - problem.AddParameterBlock(variable.data(), variable.size(), + fuse_core::Variable& variable = *(uuid_variable.second); + problem.AddParameterBlock(variable.data(), static_cast(variable.size()), #if !CERES_SUPPORTS_MANIFOLDS variable.localParameterization()); #else @@ -475,12 +471,12 @@ void HashGraph::createProblem(ceres::Problem& problem) const auto lower_bound = variable.lowerBound(index); if (lower_bound > std::numeric_limits::lowest()) { - problem.SetParameterLowerBound(variable.data(), index, lower_bound); + problem.SetParameterLowerBound(variable.data(), static_cast(index), lower_bound); } auto upper_bound = variable.upperBound(index); if (upper_bound < std::numeric_limits::max()) { - problem.SetParameterUpperBound(variable.data(), index, upper_bound); + problem.SetParameterUpperBound(variable.data(), static_cast(index), upper_bound); } } // Handle variables that are held constant @@ -491,9 +487,9 @@ void HashGraph::createProblem(ceres::Problem& problem) const } // Add the constraints std::vector parameter_blocks; - for (auto& uuid__constraint : constraints_) + for (auto const& uuid_constraint : constraints_) { - fuse_core::Constraint& constraint = *(uuid__constraint.second); + fuse_core::Constraint const& constraint = *(uuid_constraint.second); // We need the memory address of each variable value referenced by this constraint parameter_blocks.clear(); parameter_blocks.reserve(constraint.variables().size()); diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index abc639c8b..355ffc8be 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -54,6 +54,7 @@ add_library( src/odometry_3d_publisher.cpp src/pose_2d.cpp src/transaction.cpp + src/transform_sensor.cpp src/twist_2d.cpp src/unicycle_2d.cpp src/unicycle_2d_ignition.cpp diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index 7830e9a81..dbffe9dca 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -79,6 +79,11 @@ published by another node + + + An adapter-type sensor that produces pose constraints from AprilTag detections published from another node + + An adapter-type sensor that produces absolute or relative pose constraints from information published by diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 2e0c6d71d..373b39b20 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -87,7 +87,7 @@ #include -static auto sensor_proc_clock = rclcpp::Clock(); +static auto const sensorProcClock = rclcpp::Clock(); namespace tf2 { @@ -99,23 +99,23 @@ namespace tf2 * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline void doTransform(const geometry_msgs::msg::TwistWithCovarianceStamped& t_in, - geometry_msgs::msg::TwistWithCovarianceStamped& t_out, +inline void doTransform(const geometry_msgs::msg::TwistWithCovarianceStamped& data_in, + geometry_msgs::msg::TwistWithCovarianceStamped& data_out, const geometry_msgs::msg::TransformStamped& transform) // NOLINT { tf2::Vector3 vl; - fromMsg(t_in.twist.twist.linear, vl); + fromMsg(data_in.twist.twist.linear, vl); tf2::Vector3 va; - fromMsg(t_in.twist.twist.angular, va); + fromMsg(data_in.twist.twist.angular, va); tf2::Transform t; fromMsg(transform.transform, t); - t_out.twist.twist.linear = tf2::toMsg(t.getBasis() * vl); - t_out.twist.twist.angular = tf2::toMsg(t.getBasis() * va); - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; + data_out.twist.twist.linear = tf2::toMsg(t.getBasis() * vl); + data_out.twist.twist.angular = tf2::toMsg(t.getBasis() * va); + data_out.header.stamp = transform.header.stamp; + data_out.header.frame_id = transform.header.frame_id; - t_out.twist.covariance = transformCovariance(t_in.twist.covariance, t); + data_out.twist.covariance = transformCovariance(data_in.twist.covariance, t); } /** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs AccelWithCovarianceStamped type. @@ -125,23 +125,23 @@ inline void doTransform(const geometry_msgs::msg::TwistWithCovarianceStamped& t_ * \param transform The timestamped transform to apply, as a TransformStamped message. */ template <> -inline void doTransform(const geometry_msgs::msg::AccelWithCovarianceStamped& t_in, - geometry_msgs::msg::AccelWithCovarianceStamped& t_out, - const geometry_msgs::msg::TransformStamped& transform) // NOLINT +inline void doTransform(const geometry_msgs::msg::AccelWithCovarianceStamped& data_in, + geometry_msgs::msg::AccelWithCovarianceStamped& data_out, + const geometry_msgs::msg::TransformStamped& transform) { tf2::Vector3 al; - fromMsg(t_in.accel.accel.linear, al); + fromMsg(data_in.accel.accel.linear, al); tf2::Vector3 aa; - fromMsg(t_in.accel.accel.angular, aa); + fromMsg(data_in.accel.accel.angular, aa); tf2::Transform t; fromMsg(transform.transform, t); - t_out.accel.accel.linear = tf2::toMsg(t.getBasis() * al); - t_out.accel.accel.angular = tf2::toMsg(t.getBasis() * aa); - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; + data_out.accel.accel.linear = tf2::toMsg(t.getBasis() * al); + data_out.accel.accel.angular = tf2::toMsg(t.getBasis() * aa); + data_out.header.stamp = transform.header.stamp; + data_out.header.frame_id = transform.header.frame_id; - t_out.accel.covariance = transformCovariance(t_in.accel.covariance, t); + data_out.accel.covariance = transformCovariance(data_in.accel.covariance, t); } } // namespace tf2 @@ -164,7 +164,7 @@ inline std::vector mergeIndices(const std::vector& lhs_indices, { auto merged_indices = boost::copy_range>(boost::range::join(lhs_indices, rhs_indices)); - const auto rhs_it = merged_indices.begin() + lhs_indices.size(); + const auto rhs_it = merged_indices.begin() + static_cast(lhs_indices.size()); std::transform(rhs_it, merged_indices.end(), rhs_it, std::bind(std::plus(), std::placeholders::_1, rhs_offset)); @@ -186,13 +186,13 @@ inline void populatePartialMeasurement(const fuse_core::VectorXd& mean_full, con const std::vector& indices, fuse_core::VectorXd& mean_partial, fuse_core::MatrixXd& covariance_partial) { - for (size_t r = 0; r < indices.size(); ++r) + for (int64_t r = 0; r < static_cast(indices.size()); ++r) { - mean_partial(r) = mean_full(indices[r]); + mean_partial(r) = mean_full(static_cast(indices[r])); - for (size_t c = 0; c < indices.size(); ++c) + for (int64_t c = 0; c < static_cast(indices.size()); ++c) { - covariance_partial(r, c) = covariance_full(indices[r], indices[c]); + covariance_partial(r, c) = covariance_full(static_cast(indices[r]), static_cast(indices[c])); } } } @@ -209,11 +209,11 @@ inline void populatePartialMeasurement(const fuse_core::VectorXd& mean_full, con inline void populatePartialMeasurement(const fuse_core::MatrixXd& covariance_full, const std::vector& indices, fuse_core::MatrixXd& covariance_partial) { - for (size_t r = 0; r < indices.size(); ++r) + for (int64_t r = 0; r < static_cast(indices.size()); ++r) { - for (size_t c = 0; c < indices.size(); ++c) + for (int64_t c = 0; c < static_cast(indices.size()); ++c) { - covariance_partial(r, c) = covariance_full(indices[r], indices[c]); + covariance_partial(r, c) = covariance_full(static_cast(indices[r]), static_cast(indices[c])); } } } @@ -298,7 +298,7 @@ bool transformMessage(const tf2_ros::Buffer& tf_buffer, const T& input, T& outpu } catch (const tf2::TransformException& ex) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 5.0 * 1000, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 5.0 * 1000, "Could not transform message from " << input.header.frame_id << " to " << output.header.frame_id << ". Error was " << ex.what()); @@ -353,7 +353,7 @@ inline bool processAbsolutePoseWithCovariance(const std::string& source, const f if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Failed to transform pose message with stamp " << rclcpp::Time(pose.header.stamp).nanoseconds() << ". Cannot create constraint."); @@ -400,7 +400,7 @@ inline bool processAbsolutePoseWithCovariance(const std::string& source, const f } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial absolute pose measurement from '" << source << "' source: " << ex.what()); return false; @@ -470,7 +470,7 @@ inline bool processAbsolutePose3DWithCovariance(const std::string& source, const if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Failed to transform pose message with stamp " << rclcpp::Time(pose.header.stamp).nanoseconds() << ". Cannot create constraint."); @@ -507,7 +507,7 @@ inline bool processAbsolutePose3DWithCovariance(const std::string& source, const } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial absolute pose measurement from '" << source << "' source: " << ex.what()); return false; @@ -556,7 +556,7 @@ inline bool processAbsolutePose3DWithCovariance(const std::string& source, const } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial absolute pose measurement from '" << source << "' source: " << ex.what()); return false; @@ -874,7 +874,7 @@ inline bool processDifferentialPoseWithCovariance(const std::string& source, con } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement from '" << source << "' source: " << ex.what()); return false; @@ -973,7 +973,9 @@ inline bool processDifferentialPose3DWithCovariance(const std::string& source, c // by José Luis Blanco Claraco (https://arxiv.org/abs/2103.15980) // Convert from ROS msg to covariance geometry types - covariance_geometry::PoseQuaternionCovarianceRPY p1, p2, p12; + covariance_geometry::PoseQuaternionCovarianceRPY p1; + covariance_geometry::PoseQuaternionCovarianceRPY p2; + covariance_geometry::PoseQuaternionCovarianceRPY p12; covariance_geometry::fromROS(pose1.pose, p1); covariance_geometry::fromROS(pose2.pose, p2); @@ -1011,14 +1013,18 @@ inline bool processDifferentialPose3DWithCovariance(const std::string& source, c // C12 = J_p12^-1 * (C2 - J_p1 * C1 * J_p1^T) * J_p12^-T // First we need to convert covariances from RPY (6x6) to quaternion (7x7) - covariance_geometry::PoseQuaternionCovariance p1_q, p2_q, p12_q; + covariance_geometry::PoseQuaternionCovariance p1_q; + covariance_geometry::PoseQuaternionCovariance p2_q; + covariance_geometry::PoseQuaternionCovariance p12_q; covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance(p1, p1_q); covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance(p2, p2_q); // Then we need to compute the delta pose covariance_geometry::ComposePose3DQuaternion(covariance_geometry::InversePose(p1_q.first), p2_q.first, p12_q.first); // Now we have to compute pose composition jacobians so we can rotate covariances - Eigen::Matrix7d j_p1, j_p12, j_p12_inv; + Eigen::Matrix7d j_p1; + Eigen::Matrix7d j_p12; + Eigen::Matrix7d j_p12_inv; Eigen::Matrix4d j_qn; covariance_geometry::jacobianQuaternionNormalization(p12_q.first.second, j_qn); @@ -1057,7 +1063,7 @@ inline bool processDifferentialPose3DWithCovariance(const std::string& source, c } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement from '" << source << "' source: " << ex.what()); return false; @@ -1112,7 +1118,7 @@ inline bool processDifferentialPose3DWithCovariance(const std::string& source, c } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement from '" << source << "' source: " << ex.what()); return false; @@ -1255,7 +1261,7 @@ inline bool processDifferentialPoseWithTwistCovariance(const std::string& source if (dt < 1e-6) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Very small time difference " << dt << "s from '" << source << "' source."); return false; } @@ -1285,7 +1291,7 @@ inline bool processDifferentialPoseWithTwistCovariance(const std::string& source } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement using the twist covariance from '" << source << "' source: " << ex.what()); return false; @@ -1359,7 +1365,8 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour } // Convert the poses into tf2 transforms - tf2::Transform pose1_tf2, pose2_tf2; + tf2::Transform pose1_tf2; + tf2::Transform pose2_tf2; tf2::fromMsg(pose1.pose.pose, pose1_tf2); tf2::fromMsg(pose2.pose.pose, pose2_tf2); @@ -1394,7 +1401,7 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour if (dt < 1e-6) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Very small time difference " << dt << "s from '" << source << "' source."); return false; } @@ -1421,7 +1428,7 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement using the twist covariance from '" << source << "' source: " << ex.what()); return false; @@ -1470,7 +1477,7 @@ inline bool processDifferentialPose3DWithTwistCovariance(const std::string& sour } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial differential pose measurement using the twist covariance from '" << source << "' source: " << ex.what()); return false; @@ -1547,7 +1554,7 @@ inline bool processTwistWithCovariance(const std::string& source, const fuse_cor if (!transformMessage(tf_buffer, twist, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Failed to transform twist message with stamp " << rclcpp::Time(twist.header.stamp).nanoseconds() << ". Cannot create constraint."); @@ -1590,7 +1597,7 @@ inline bool processTwistWithCovariance(const std::string& source, const fuse_cor } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial linear velocity measurement from '" << source << "' source: " << ex.what()); add_constraint = false; @@ -1632,7 +1639,7 @@ inline bool processTwistWithCovariance(const std::string& source, const fuse_cor } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0, "Invalid partial angular velocity measurement from '" << source << "' source: " << ex.what()); add_constraint = false; @@ -1714,7 +1721,7 @@ inline bool processTwist3DWithCovariance(const std::string& source, const fuse_c if (!transformMessage(tf_buffer, twist, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Failed to transform twist message with stamp " << rclcpp::Time(twist.header.stamp).nanoseconds() << ". Cannot create constraint."); @@ -1758,7 +1765,7 @@ inline bool processTwist3DWithCovariance(const std::string& source, const fuse_c } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial linear velocity measurement from '" << source << "' source: " << ex.what()); add_constraint = false; @@ -1810,7 +1817,7 @@ inline bool processTwist3DWithCovariance(const std::string& source, const fuse_c } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0, "Invalid partial angular velocity measurement from '" << source << "' source: " << ex.what()); add_constraint = false; @@ -1884,7 +1891,7 @@ inline bool processAccelWithCovariance(const std::string& source, const fuse_cor if (!transformMessage(tf_buffer, acceleration, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0, "Failed to transform acceleration message with stamp " << rclcpp::Time(acceleration.header.stamp).nanoseconds() << ". Cannot create constraint."); @@ -1920,7 +1927,7 @@ inline bool processAccelWithCovariance(const std::string& source, const fuse_cor } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial linear acceleration measurement from '" << source << "' source: " << ex.what()); return false; @@ -1987,7 +1994,7 @@ inline bool processAccel3DWithCovariance(const std::string& source, const fuse_c if (!transformMessage(tf_buffer, acceleration, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0, "Failed to transform acceleration message with stamp " << rclcpp::Time(acceleration.header.stamp).nanoseconds() << ". Cannot create constraint."); @@ -2017,7 +2024,7 @@ inline bool processAccel3DWithCovariance(const std::string& source, const fuse_c } catch (const std::runtime_error& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensorProcClock, 10.0 * 1000, "Invalid partial linear acceleration measurement from '" << source << "' source: " << ex.what()); return false; diff --git a/fuse_models/include/fuse_models/imu_3d.hpp b/fuse_models/include/fuse_models/imu_3d.hpp index 46c8979b9..cd6e5b392 100644 --- a/fuse_models/include/fuse_models/imu_3d.hpp +++ b/fuse_models/include/fuse_models/imu_3d.hpp @@ -112,6 +112,11 @@ class Imu3D : public fuse_core::AsyncSensorModel */ virtual ~Imu3D() = default; + Imu3D(Imu3D const&) = delete; + Imu3D(Imu3D&&) = delete; + Imu3D& operator=(Imu3D const&) = delete; + Imu3D& operator=(Imu3D&&) = delete; + /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ @@ -157,7 +162,7 @@ class Imu3D : public fuse_core::AsyncSensorModel * @param[out] transaction - The generated variables and constraints are added to this transaction */ void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, - const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, bool validate, fuse_core::Transaction& transaction); fuse_core::node_interfaces::NodeInterfaces angular_velocity_indices; std::vector linear_acceleration_indices; std::vector orientation_indices; diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp index 734e75c98..9fce065cd 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp @@ -46,10 +46,7 @@ #include #include -namespace fuse_models -{ - -namespace parameters +namespace fuse_models::parameters { /** @@ -129,9 +126,9 @@ struct Odometry3DParams : public ParameterBase //!< 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 pose_target_frame{}; - std::string twist_target_frame{}; + std::string topic; + std::string pose_target_frame; + std::string twist_target_frame; std::vector position_indices; std::vector orientation_indices; std::vector linear_velocity_indices; @@ -141,8 +138,6 @@ struct Odometry3DParams : public ParameterBase fuse_core::Loss::SharedPtr angular_velocity_loss; }; -} // namespace parameters - -} // namespace fuse_models +} // namespace fuse_models::parameters #endif // FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp new file mode 100644 index 000000000..e6506abe3 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/transform_sensor_params.hpp @@ -0,0 +1,108 @@ +/* + * 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. + */ +#ifndef FUSE_MODELS__PARAMETERS__APRIL_TAG_POSE_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__APRIL_TAG_POSE_PARAMS_HPP_ + +#include +#include + +#include + +#include +#include +#include +#include + +namespace fuse_models::parameters +{ + +/** + * @brief Defines the set of parameters required by the TransformSensor class + */ +struct TransformSensorParams : public ParameterBase +{ +public: + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) + { + position_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "position_dimensions")); + orientation_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "orientation_dimensions")); + + disable_checks = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_checks"), disable_checks); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); + + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "throttle_period"), throttle_period, false); + throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), + throttle_use_wall_time); + + fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + + target_frame = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame); + + pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + pose_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_covariance"), + std::vector{ 1, 1, 1, 1, 1, 1 }); + } + + bool disable_checks{ false }; + bool independent{ true }; + fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance matrix + 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::vector pose_covariance; //!< The diagonal elements of the tag pose covariance + int queue_size{ 10 }; + std::string topic; + std::string target_frame; + std::vector position_indices; + std::vector orientation_indices; + fuse_core::Loss::SharedPtr pose_loss; +}; + +} // namespace fuse_models::parameters + +#endif // FUSE_MODELS__PARAMETERS__APRIL_TAG_POSE_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/pose_2d.hpp b/fuse_models/include/fuse_models/pose_2d.hpp index 154102025..c5bc02ae7 100644 --- a/fuse_models/include/fuse_models/pose_2d.hpp +++ b/fuse_models/include/fuse_models/pose_2d.hpp @@ -91,6 +91,10 @@ class Pose2D : public fuse_core::AsyncSensorModel * @brief Destructor */ virtual ~Pose2D() = default; + Pose2D(Pose2D const&) = delete; + Pose2D(Pose2D&&) = delete; + Pose2D& operator=(Pose2D const&) = delete; + Pose2D& operator=(Pose2D&&) = delete; /** * @brief Shadowing extension to the AsyncSensorModel::initialize call @@ -134,7 +138,7 @@ class Pose2D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, const bool validate, + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, bool validate, fuse_core::Transaction& transaction); fuse_core::node_interfaces::NodeInterfaces +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "tf2_msgs/msg/tf_message.hpp" + +namespace fuse_models +{ + +/** + * @brief An adapter-type sensor that produces pose constraints from published transforms + * + * This sensor subscribes to a MessageType topic and creates orientation and pose variables and constraints. + * This sensor can be used for AprilTags or any pose for which the transform to the desired state estimation frame is + * known. For an example, try `ros2 launch fuse_tutorials fuse_apriltag_tutorial.launch.py` and see its relevant files. + * + * 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 transform messages + * - topic (string) The topic to which to subscribe for the transform messages + * - target_frame (string) the state estimation frame to transform tfs to + * + * Subscribes: + * - \p topic (MessageType) IMU data at a given timestep + */ +class TransformSensor : public fuse_core::AsyncSensorModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS(TransformSensor) + using ParameterType = parameters::TransformSensorParams; + using MessageType = tf2_msgs::msg::TFMessage; + + /** + * @brief Default constructor + */ + TransformSensor(); + + /** + * @brief Destructor + */ + virtual ~TransformSensor() = default; + + TransformSensor(TransformSensor const&) = delete; + TransformSensor(TransformSensor&&) = delete; + TransformSensor& operator=(TransformSensor const&) = delete; + TransformSensor& operator=(TransformSensor&&) = delete; + + /** + * @brief Shadowing extension to the AsyncSensorModel::initialize call + */ + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; + + /** + * @brief Callback for tf messages + * @param[in] msg - The IMU message to process + */ + void process(const MessageType& 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 + 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_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + rclcpp::Subscription::SharedPtr sub_; + + using AprilTagThrottledCallback = fuse_core::ThrottledMessageCallback; + AprilTagThrottledCallback throttled_callback_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__TRANSFORM_SENSOR_HPP_ diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp new file mode 100644 index 000000000..176b90ee8 --- /dev/null +++ b/fuse_models/src/transform_sensor.cpp @@ -0,0 +1,148 @@ +/* + * 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. + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Register this sensor model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::TransformSensor, fuse_core::SensorModel) + +namespace fuse_models +{ + +TransformSensor::TransformSensor() + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&TransformSensor::process, this, std::placeholders::_1)) +{ +} + +void TransformSensor::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + +void TransformSensor::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + // Read settings from the parameter server + device_id_ = fuse_variables::loadDeviceId(interfaces_); + + params_.loadFromROS(interfaces_, name_); + + throttled_callback_.setThrottlePeriod(params_.throttle_period); + + if (!params_.throttle_use_wall_time) + { + throttled_callback_.setClock(clock_); + } + + if (params_.position_indices.empty() && params_.orientation_indices.empty()) + { + throw std::runtime_error("No dimensions specified, so this sensor would not do anything (data from topic " + + params_.topic + " would be ignored)."); + } + + tf_buffer_ = std::make_unique(clock_); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); +} + +void TransformSensor::onStart() +{ + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription(interfaces_, params_.topic, params_.queue_size, + std::bind(&AprilTagThrottledCallback::callback, + &throttled_callback_, std::placeholders::_1), + sub_options); +} + +void TransformSensor::onStop() +{ + sub_.reset(); +} + +void TransformSensor::process(MessageType const& msg) +{ + for (auto const& transform : msg.transforms) + { + // Create a transaction object + auto transaction = fuse_core::Transaction::make_shared(); + transaction->stamp(transform.header.stamp); + + // Create the pose from the transform + auto pose = std::make_unique(); + pose->header = transform.header; + pose->header.frame_id = pose->header.frame_id; + pose->pose.pose.orientation = transform.transform.rotation; + pose->pose.pose.position.x = transform.transform.translation.x; + pose->pose.pose.position.y = transform.transform.translation.y; + pose->pose.pose.position.z = transform.transform.translation.z; + + // TODO(henrygerardmoore): figure out better method to set the covariance + for (std::size_t i = 0; i < params_.pose_covariance.size(); ++i) + { + pose->pose.covariance[i * 7] = params_.pose_covariance[i]; + } + + const bool validate = !params_.disable_checks; + common::processAbsolutePose3DWithCovariance(name(), device_id_, *pose, params_.pose_loss, params_.target_frame, + params_.position_indices, params_.orientation_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); + + // Send the transaction object to the plugin's parent + sendTransaction(transaction); + } +} + +} // namespace fuse_models diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp index 53a07f651..2d229b678 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp @@ -37,7 +37,6 @@ #include #include -#include #include #include #include @@ -124,13 +123,17 @@ class FixedLagSmoother : public Optimizer * @param[in] interfaces The node interfaces for the node driving the optimizer * @param[in] graph The graph used with the optimizer */ - FixedLagSmoother(fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph = nullptr); + explicit FixedLagSmoother(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph = nullptr); /** * @brief Destructor */ virtual ~FixedLagSmoother(); + FixedLagSmoother(FixedLagSmoother const&) = delete; + FixedLagSmoother(FixedLagSmoother&&) = delete; + FixedLagSmoother& operator=(FixedLagSmoother const&) = delete; + FixedLagSmoother& operator=(FixedLagSmoother&&) = delete; protected: /** @@ -301,8 +304,8 @@ class FixedLagSmoother : public Optimizer /** * @brief Service callback that resets the optimizer to its original state */ - bool resetServiceCallback(const std::shared_ptr, - std::shared_ptr); + bool resetServiceCallback(std::shared_ptr const&, + std::shared_ptr const&); /** * @brief Thread-safe read-only access to the timestamp of the first transaction diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp index ee967cc54..5033b2582 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp @@ -37,9 +37,7 @@ #include -#include #include -#include #include #include diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 3fcd33be1..f8816a299 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -187,7 +187,7 @@ void FixedLagSmoother::optimizationLoop() } // Optimize { - std::lock_guard lock(optimization_mutex_); + std::lock_guard const lock(optimization_mutex_); // Apply motion models auto new_transaction = fuse_core::Transaction::make_shared(); // DANGER: processQueue obtains a lock from the pending_transactions_mutex_ @@ -240,7 +240,7 @@ void FixedLagSmoother::optimizationLoop() RCLCPP_FATAL_STREAM(logger_, "Optimization failed after updating the graph with the transaction with timestamp " << new_transaction_stamp.nanoseconds() << ". Leaving optimization loop and requesting node shutdown..."); - RCLCPP_INFO(logger_, summary_.FullReport().c_str()); + RCLCPP_INFO(logger_, "%s", summary_.FullReport().c_str()); rclcpp::shutdown(); break; } @@ -276,13 +276,13 @@ void FixedLagSmoother::optimizerTimerCallback() // will not be waiting on the condition variable signal, so nothing will happen. auto optimization_request = false; { - std::lock_guard lock(pending_transactions_mutex_); + std::lock_guard const lock(pending_transactions_mutex_); optimization_request = !pending_transactions_.empty(); } if (optimization_request) { { - std::lock_guard lock(optimization_requested_mutex_); + std::lock_guard const lock(optimization_requested_mutex_); optimization_request_ = true; optimization_deadline_ = clock_->now() + optimize_timer_->time_until_trigger(); } @@ -293,7 +293,7 @@ void FixedLagSmoother::optimizerTimerCallback() void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const rclcpp::Time& lag_expiration) { // We need to get the pending transactions from the queue - std::lock_guard pending_transactions_lock(pending_transactions_mutex_); + std::lock_guard const pending_transactions_lock(pending_transactions_mutex_); if (pending_transactions_.empty()) { @@ -436,14 +436,16 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const r } } -bool FixedLagSmoother::resetServiceCallback(const std::shared_ptr, - std::shared_ptr) +// NOLINTBEGIN(performance-unnecessary-value-param) +bool FixedLagSmoother::resetServiceCallback(std::shared_ptr const& /*unused*/, + std::shared_ptr const& /*unused*/) { + // NOLINTEND(performance-unnecessary-value-param) // Tell all the plugins to stop stopPlugins(); // Reset the optimizer state { - std::lock_guard lock(optimization_requested_mutex_); + std::lock_guard const lock(optimization_requested_mutex_); optimization_request_ = false; } started_ = false; @@ -453,10 +455,10 @@ bool FixedLagSmoother::resetServiceCallback(const std::shared_ptr lock(optimization_mutex_); + std::lock_guard const lock(optimization_mutex_); // Clear all pending transactions { - std::lock_guard lock(pending_transactions_mutex_); + std::lock_guard const lock(pending_transactions_mutex_); pending_transactions_.clear(); } // Clear the graph and marginal tracking states @@ -486,65 +488,63 @@ void FixedLagSmoother::transactionCallback(const std::string& sensor_name, fuse_ << ", difference: " << (start_time - max_time).nanoseconds() << "ns"); return; } + // We need to add the new transaction to the pending_transactions_ queue + std::lock_guard const pending_transactions_lock(pending_transactions_mutex_); + + // Add the new transaction to the pending set + // The pending set is arranged "smallest stamp last" to making popping off the back more + // efficient + auto comparator = [](const rclcpp::Time& value, const TransactionQueueElement& element) { + return value >= element.stamp(); + }; + auto position = + std::upper_bound(pending_transactions_.begin(), pending_transactions_.end(), transaction->stamp(), comparator); + position = pending_transactions_.insert(position, { sensor_name, std::move(transaction) }); // NOLINT + + // If we haven't "started" yet.. + if (!started_) { - // We need to add the new transaction to the pending_transactions_ queue - std::lock_guard pending_transactions_lock(pending_transactions_mutex_); - - // Add the new transaction to the pending set - // The pending set is arranged "smallest stamp last" to making popping off the back more - // efficient - auto comparator = [](const rclcpp::Time& value, const TransactionQueueElement& element) { - return value >= element.stamp(); - }; - auto position = - std::upper_bound(pending_transactions_.begin(), pending_transactions_.end(), transaction->stamp(), comparator); - position = pending_transactions_.insert(position, { sensor_name, std::move(transaction) }); // NOLINT - - // If we haven't "started" yet.. - if (!started_) + // ...check if we should + if (sensor_models_.at(sensor_name).ignition) { - // ...check if we should - if (sensor_models_.at(sensor_name).ignition) + started_ = true; + ignited_ = true; + start_time = position->minStamp(); + setStartTime(start_time); + + // And purge out old transactions + // - Either before or exactly at the start time + // - Or with a minimum time before the minimum time of this ignition sensor transaction + // + // TODO(efernandez) Do '&min_time = std::as_const(start_ime)' when C++17 is supported and we + // can use std::as_const: + // https://en.cppreference.com/w/cpp/utility/as_const + pending_transactions_.erase( + std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), + [&sensor_name, max_time, + &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) + return transaction.sensor_name != sensor_name && + (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); + }), // NOLINT(whitespace/braces) + pending_transactions_.end()); + } + else + { + // And purge out old transactions to limit the pending size while waiting for an ignition + // sensor + auto purge_time = rclcpp::Time(0, 0, RCL_ROS_TIME); // NOTE(CH3): Uninitialized + auto last_pending_time = pending_transactions_.front().stamp(); + + // rclcpp::Time doesn't allow negatives + if (rclcpp::Time(params_.transaction_timeout.nanoseconds(), last_pending_time.get_clock_type()) < + last_pending_time) { - started_ = true; - ignited_ = true; - start_time = position->minStamp(); - setStartTime(start_time); - - // And purge out old transactions - // - Either before or exactly at the start time - // - Or with a minimum time before the minimum time of this ignition sensor transaction - // - // TODO(efernandez) Do '&min_time = std::as_const(start_ime)' when C++17 is supported and we - // can use std::as_const: - // https://en.cppreference.com/w/cpp/utility/as_const - pending_transactions_.erase( - std::remove_if(pending_transactions_.begin(), pending_transactions_.end(), - [&sensor_name, max_time, - &min_time = start_time](const auto& transaction) { // NOLINT(whitespace/braces) - return transaction.sensor_name != sensor_name && - (transaction.minStamp() < min_time || transaction.maxStamp() <= max_time); - }), // NOLINT(whitespace/braces) - pending_transactions_.end()); + purge_time = last_pending_time - params_.transaction_timeout; } - else - { - // And purge out old transactions to limit the pending size while waiting for an ignition - // sensor - auto purge_time = rclcpp::Time(0, 0, RCL_ROS_TIME); // NOTE(CH3): Uninitialized - auto last_pending_time = pending_transactions_.front().stamp(); - - // rclcpp::Time doesn't allow negatives - if (rclcpp::Time(params_.transaction_timeout.nanoseconds(), last_pending_time.get_clock_type()) < - last_pending_time) - { - purge_time = last_pending_time - params_.transaction_timeout; - } - while (!pending_transactions_.empty() && pending_transactions_.back().maxStamp() < purge_time) - { - pending_transactions_.pop_back(); - } + while (!pending_transactions_.empty() && pending_transactions_.back().maxStamp() < purge_time) + { + pending_transactions_.pop_back(); } } } @@ -602,7 +602,7 @@ void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrappe status.add("Started", started); { - std::lock_guard lock(pending_transactions_mutex_); + std::lock_guard const lock(pending_transactions_mutex_); status.add("Pending Transactions", pending_transactions_.size()); } diff --git a/fuse_tutorials/CMakeLists.txt b/fuse_tutorials/CMakeLists.txt index 83387db16..2bde24db1 100644 --- a/fuse_tutorials/CMakeLists.txt +++ b/fuse_tutorials/CMakeLists.txt @@ -48,6 +48,9 @@ target_link_libraries(range_sensor_simulator ${PROJECT_NAME}) add_executable(three_dimensional_simulator src/three_dimensional_simulator.cpp) target_link_libraries(three_dimensional_simulator ${PROJECT_NAME}) +add_executable(apriltag_simulator src/apriltag_simulator.cpp) +target_link_libraries(apriltag_simulator ${PROJECT_NAME}) + # ############################################################################## # Install ## # ############################################################################## @@ -63,6 +66,7 @@ install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) install(TARGETS range_sensor_simulator DESTINATION lib/${PROJECT_NAME}) install(TARGETS three_dimensional_simulator DESTINATION lib/${PROJECT_NAME}) +install(TARGETS apriltag_simulator DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch config data DESTINATION share/${PROJECT_NAME}) diff --git a/fuse_tutorials/config/fuse_apriltag_tutorial.yaml b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml new file mode 100644 index 000000000..51531cbe9 --- /dev/null +++ b/fuse_tutorials/config/fuse_apriltag_tutorial.yaml @@ -0,0 +1,55 @@ +# this yaml file is adapted from `fuse_simple_tutorial.yaml` +state_estimator: + ros__parameters: + # Fixed-lag smoother configuration + optimization_frequency: 100.0 + transaction_timeout: 0.01 + lag_duration: 0.01 + + # all our sensors will be using this motion model + motion_models: + 3d_motion_model: + type: fuse_models::Omnidirectional3D + + 3d_motion_model: + # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc + # use high values for the acceleration process noise because we aren't measuring the applied forces + process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10., 10., 10.] + + sensor_models: + initial_localization_sensor: + type: fuse_models::Omnidirectional3DIgnition + motion_models: [3d_motion_model] + ignition: true + transform_sensor: + type: fuse_models::TransformSensor + motion_models: [3d_motion_model] + + initial_localization_sensor: + publish_on_startup: true + # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc + initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + + transform_sensor: + topic: 'april_tf' + target_frame: 'base_link' + position_dimensions: ['x', 'y', 'z'] + orientation_dimensions: ['roll', 'pitch', 'yaw'] + # these are the true covariance values used by the simulator; what happens if we change these? + pose_covariance: [0.1, 0.1, 0.1, 0.25, 0.25, 0.25] + + # this publishes our estimated odometry + publishers: + filtered_publisher: + type: fuse_models::Odometry3DPublisher + + # the configuration of our output publisher + filtered_publisher: + topic: 'odom_filtered' + base_link_frame_id: 'base_link' + odom_frame_id: 'odom' + map_frame_id: 'map' + world_frame_id: 'odom' + publish_tf: true + publish_frequency: 100.0 diff --git a/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py b/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py new file mode 100644 index 000000000..6df7100cd --- /dev/null +++ b/fuse_tutorials/launch/fuse_apriltag_tutorial.launch.py @@ -0,0 +1,68 @@ +#! /usr/bin/env python3 + +# Copyright 2024 PickNik Robotics +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + pkg_dir = FindPackageShare("fuse_tutorials") + + return LaunchDescription( + [ + # create a fixed transform between map and odom for visualization purposes + Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0", "0", "0", "0", "0", "0", "map", "odom"], + ), + # run our simulator + Node( + package="fuse_tutorials", + executable="apriltag_simulator", + name="apriltag_simulator", + output="screen", + ), + # run our estimator + Node( + package="fuse_optimizers", + executable="fixed_lag_smoother_node", + name="state_estimator", + parameters=[ + PathJoinSubstitution( + [pkg_dir, "config", "fuse_apriltag_tutorial.yaml"] + ) + ], + ), + # run visualization + Node( + package="rviz2", + executable="rviz2", + name="rviz", + arguments=[ + "-d", + [ + PathJoinSubstitution( + [pkg_dir, "config", "fuse_3d_tutorial.rviz"] + ) + ], + ], + ), + ] + ) diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp new file mode 100644 index 000000000..c1bce41d1 --- /dev/null +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -0,0 +1,394 @@ +/* + * 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. + */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_msgs/msg/tf_message.hpp" + +namespace +{ +constexpr char baselinkFrame[] = "base_link"; //!< The base_link frame id used when + //!< publishing sensor data +constexpr char mapFrame[] = "map"; //!< The map frame id used when publishing ground truth + //!< data +constexpr double aprilTagPositionSigma = 0.1; //!< the april tag position std dev +constexpr double aprilTagOrientationSigma = 0.25; //!< the april tag orientation std dev +constexpr size_t numAprilTags = 8; //!< the number of april tags +constexpr double detectionProbability = + 0.5; //!< the probability that any given april tag is detectable on a given tick of the simulation +} // namespace + +/** + * @brief The true pose and velocity of the robot + */ +struct Robot +{ + rclcpp::Time stamp; + + double mass{}; + + double x = 0; + double y = 0; + double z = 0; + double roll = 0; + double pitch = 0; + double yaw = 0; + double vx = 0; + double vy = 0; + double vz = 0; + double vroll = 0; + double vpitch = 0; + double vyaw = 0; + double ax = 0; + double ay = 0; + double az = 0; +}; + +/** + * @brief Convert the robot state into a ground truth odometry message + */ +nav_msgs::msg::Odometry robotToOdometry(Robot const& state) +{ + nav_msgs::msg::Odometry msg; + msg.header.stamp = state.stamp; + msg.header.frame_id = mapFrame; + msg.child_frame_id = baselinkFrame; + msg.pose.pose.position.x = state.x; + msg.pose.pose.position.y = state.y; + msg.pose.pose.position.z = state.z; + + tf2::Quaternion q; + q.setEuler(state.yaw, state.pitch, state.roll); + msg.pose.pose.orientation.w = q.w(); + msg.pose.pose.orientation.x = q.x(); + msg.pose.pose.orientation.y = q.y(); + msg.pose.pose.orientation.z = q.z(); + msg.twist.twist.linear.x = state.vx; + msg.twist.twist.linear.y = state.vy; + msg.twist.twist.linear.z = state.vz; + msg.twist.twist.angular.x = state.vroll; + msg.twist.twist.angular.y = state.vpitch; + msg.twist.twist.angular.z = state.vyaw; + + // set covariances + msg.pose.covariance[0] = 0.1; + msg.pose.covariance[7] = 0.1; + msg.pose.covariance[14] = 0.1; + msg.pose.covariance[21] = 0.1; + msg.pose.covariance[28] = 0.1; + msg.pose.covariance[35] = 0.1; + msg.twist.covariance[0] = 0.1; + msg.twist.covariance[7] = 0.1; + msg.twist.covariance[14] = 0.1; + msg.twist.covariance[21] = 0.1; + msg.twist.covariance[28] = 0.1; + msg.twist.covariance[35] = 0.1; + return msg; +} + +/** + * @brief Compute the next robot state given the current robot state and a simulated step time + */ +Robot simulateRobotMotion(Robot const& previous_state, rclcpp::Time const& now, Eigen::Vector3d const& external_force) +{ + auto const dt = (now - previous_state.stamp).seconds(); + auto next_state = Robot(); + next_state.stamp = now; + next_state.mass = previous_state.mass; + + // just euler integrate to get the next position and velocity + next_state.x = previous_state.x + previous_state.vx * dt; + next_state.y = previous_state.y + previous_state.vy * dt; + next_state.z = previous_state.z + previous_state.vz * dt; + next_state.vx = previous_state.vx + previous_state.ax * dt; + next_state.vy = previous_state.vy + previous_state.ay * dt; + next_state.vz = previous_state.vz + previous_state.az * dt; + + // let's not deal with 3D orientation dynamics for this tutorial + next_state.roll = 0; + next_state.pitch = 0; + next_state.yaw = 0; + next_state.vroll = 0; + next_state.vpitch = 0; + next_state.vyaw = 0; + + // get the current acceleration from the current applied force + next_state.ax = external_force.x() / next_state.mass; + next_state.ay = external_force.y() / next_state.mass; + next_state.az = external_force.z() / next_state.mass; + + return next_state; +} + +tf2_msgs::msg::TFMessage aprilTagPoses(Robot const& robot) +{ + tf2_msgs::msg::TFMessage msg; + + // publish the april tag positions relative to the base + for (std::size_t i = 0; i < numAprilTags; ++i) + { + geometry_msgs::msg::TransformStamped april_to_base; + april_to_base.child_frame_id = "base_link"; + + // april tag names start at 1 + april_to_base.header.frame_id = "april_" + std::to_string(i + 1); + april_to_base.header.stamp = robot.stamp; + + april_to_base.transform.rotation.w = 1; + april_to_base.transform.rotation.x = 0; + april_to_base.transform.rotation.y = 0; + april_to_base.transform.rotation.z = 0; + + // calculate offset of each april tag + // we start with offset 1, 1, 1 and switch the z, y, then x as if they were binary digits based off of the april tag + // number see the launch file for a more readable offset for each april tag + bool const x_positive = ((i >> 2) & 1) == 0u; + bool const y_positive = ((i >> 1) & 1) == 0u; + bool const z_positive = ((i >> 0) & 1) == 0u; + + // robot position with offset and noise + april_to_base.transform.translation.x = x_positive ? 1. : -1.; + april_to_base.transform.translation.y = y_positive ? 1. : -1.; + april_to_base.transform.translation.z = z_positive ? 1. : -1.; + msg.transforms.push_back(april_to_base); + } + return msg; +} + +tf2_msgs::msg::TFMessage simulateAprilTag(const Robot& robot) +{ + static std::random_device rd{}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> position_noise{ 0.0, aprilTagPositionSigma }; + static std::normal_distribution<> orientation_noise{ 0.0, aprilTagOrientationSigma }; + static std::bernoulli_distribution april_tag_detectable(detectionProbability); + + tf2_msgs::msg::TFMessage msg; + + // publish the april tag positions + for (std::size_t i = 0; i < numAprilTags; ++i) + { + geometry_msgs::msg::TransformStamped april_to_world; + april_to_world.child_frame_id = "odom"; + // april tag names start at 1 + april_to_world.header.frame_id = "april_" + std::to_string(i + 1); + april_to_world.header.stamp = robot.stamp; + tf2::Quaternion q; + // robot orientation with noise + q.setRPY(robot.roll + orientation_noise(generator), robot.pitch + orientation_noise(generator), + robot.yaw + orientation_noise(generator)); + april_to_world.transform.rotation.w = q.w(); + april_to_world.transform.rotation.x = q.x(); + april_to_world.transform.rotation.y = q.y(); + april_to_world.transform.rotation.z = q.z(); + + // calculate offset of each april tag + // we start with offset 1, 1, 1 and switch the z, y, then x as if they were binary digits based off of the april tag + // number see the launch file for a more readable offset for each april tag + bool const x_positive = ((i >> 2) & 1) == 0u; + bool const y_positive = ((i >> 1) & 1) == 0u; + bool const z_positive = ((i >> 0) & 1) == 0u; + + double const x_offset = x_positive ? 1. : -1.; + double const y_offset = y_positive ? 1. : -1.; + double const z_offset = z_positive ? 1. : -1.; + + // robot position with offset and noise + april_to_world.transform.translation.x = robot.x + x_offset + position_noise(generator); + april_to_world.transform.translation.y = robot.y + y_offset + position_noise(generator); + april_to_world.transform.translation.z = robot.z + z_offset + position_noise(generator); + + if (april_tag_detectable(generator)) + { + msg.transforms.push_back(april_to_world); + } + } + return msg; +} + +void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces interfaces, + const Robot& state, const rclcpp::Clock::SharedPtr& clock, const rclcpp::Logger& logger) +{ + // Send the initial localization signal to the state estimator + auto srv = std::make_shared(); + srv->pose.header.frame_id = mapFrame; + srv->pose.pose.pose.position.x = state.x; + srv->pose.pose.pose.position.y = state.y; + srv->pose.pose.pose.position.z = state.z; + tf2::Quaternion q; + q.setEuler(state.yaw, state.pitch, state.roll); + srv->pose.pose.pose.orientation.w = q.w(); + srv->pose.pose.pose.orientation.x = q.x(); + srv->pose.pose.pose.orientation.y = q.y(); + srv->pose.pose.pose.orientation.z = q.z(); + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 1.0; + srv->pose.pose.covariance[14] = 1.0; + srv->pose.pose.covariance[21] = 1.0; + srv->pose.pose.covariance[28] = 1.0; + srv->pose.pose.covariance[35] = 1.0; + + auto const client = rclcpp::create_client( + interfaces.get_node_base_interface(), interfaces.get_node_graph_interface(), + interfaces.get_node_services_interface(), "/state_estimation/set_pose_service", rclcpp::ServicesQoS()); + + while (!client->wait_for_service(std::chrono::seconds(30)) && + interfaces.get_node_base_interface()->get_context()->is_valid()) + { + RCLCPP_WARN_STREAM(logger, "Waiting for '" << client->get_service_name() << "' service to become available."); + } + + auto success = false; + while (!success) + { + clock->sleep_for(std::chrono::milliseconds(100)); + srv->pose.header.stamp = clock->now(); + auto result_future = client->async_send_request(srv); + + if (rclcpp::spin_until_future_complete(interfaces.get_node_base_interface(), result_future, + std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(logger, "set pose service call failed"); + client->remove_pending_request(result_future); + return; + } + success = result_future.get()->success; + } +} + +int main(int argc, char** argv) +{ + // set up our ROS node + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("three_dimensional_simulator"); + + // create our sensor publishers + auto april_tf_publisher = node->create_publisher("april_tf", 1); + auto tf_publisher = node->create_publisher("tf", 1); + + // create the ground truth publisher + auto ground_truth_publisher = node->create_publisher("ground_truth", 1); + + // Initialize the robot state (state variables are zero-initialized) + auto state = Robot(); + state.stamp = node->now(); + state.mass = 10; // kg + + // you can modify the rate at which this loop runs to see the different performance of the estimator and the effect of + // integration inaccuracy on the ground truth + auto rate = rclcpp::Rate(1000.0); + + // normally we would have to initialize the state estimation, but we included an ignition 'sensor' in our config, + // which takes care of that. + + // parameters that control the motion pattern of the robot + double const motion_duration = 5; // length of time to oscillate on a given axis, in seconds + double const n_cycles = 2; // number of oscillations per `motion_duration` + + while (rclcpp::ok()) + { + // store the first time this runs (since it won't start running exactly at a multiple of `motion_duration`) + static auto const firstTime = node->now(); + auto const now = node->now(); + + // compensate for the original time offset + double const now_d = (now - firstTime).seconds(); + // store how long it has been (resetting at `motion_duration` seconds) + double const mod_time = std::fmod(now_d, motion_duration); + + // apply a harmonic force (oscillates `N_cycles` times per `motion_duration`) + double const force_magnitude = 100 * std::cos(2 * M_PI * n_cycles * mod_time / motion_duration); + Eigen::Vector3d external_force = { 0, 0, 0 }; + + // switch oscillation axes every `motion_duration` seconds (with one 'rest period') + if (std::fmod(now_d, 4 * motion_duration) < motion_duration) + { + external_force.x() = force_magnitude; + } + else if (std::fmod(now_d, 4 * motion_duration) < 2 * motion_duration) + { + external_force.y() = force_magnitude; + } + else if (std::fmod(now_d, 4 * motion_duration) < 3 * motion_duration) + { + external_force.z() = force_magnitude; + } + else + { + // reset the robot's position and velocity, leave the external force as 0 + // we do this so the ground truth doesn't drift (due to inaccuracy from euler integration) + state.x = 0; + state.y = 0; + state.z = 0; + state.vx = 0; + state.vy = 0; + state.vz = 0; + } + + // Simulate the robot motion + auto new_state = simulateRobotMotion(state, now, external_force); + + // Publish the new ground truth + ground_truth_publisher->publish(robotToOdometry(new_state)); + + // Generate and publish simulated measurements from the new robot state + tf_publisher->publish(aprilTagPoses(new_state)); + + // Wait for the next time step + state = new_state; + rclcpp::spin_some(node); + rate.sleep(); + + // publish simulated position after the static april tag poses since we need them to be in the tf buffer to run + april_tf_publisher->publish(simulateAprilTag(new_state)); + } + + rclcpp::shutdown(); + return 0; +} diff --git a/fuse_tutorials/src/three_dimensional_simulator.cpp b/fuse_tutorials/src/three_dimensional_simulator.cpp index 6bfb873fa..52dd53490 100644 --- a/fuse_tutorials/src/three_dimensional_simulator.cpp +++ b/fuse_tutorials/src/three_dimensional_simulator.cpp @@ -50,14 +50,14 @@ namespace { -static constexpr char BASELINK_FRAME[] = "base_link"; //!< The base_link frame id used when - //!< publishing sensor data -static constexpr char MAP_FRAME[] = "map"; //!< The map frame id used when publishing ground truth - //!< data -static constexpr double IMU_SIGMA = 0.1; //!< Std dev of simulated Imu measurement noise -static constexpr char ODOM_FRAME[] = "odom"; //!< The odom frame id used when publishing wheel -static constexpr double ODOM_POSITION_SIGMA = 0.5; //!< Std dev of simulated odom position measurement noise -static constexpr double ODOM_ORIENTATION_SIGMA = 0.1; //!< Std dev of simulated odom orientation measurement noise +constexpr char baselinkFrame[] = "base_link"; //!< The base_link frame id used when + //!< publishing sensor data +constexpr char mapFrame[] = "map"; //!< The map frame id used when publishing ground truth + //!< data +constexpr double imuSigma = 0.1; //!< Std dev of simulated Imu measurement noise +constexpr char odomFrame[] = "odom"; //!< The odom frame id used when publishing wheel +constexpr double odomPositionSigma = 0.5; //!< Std dev of simulated odom position measurement noise +constexpr double odomOrientationSigma = 0.1; //!< Std dev of simulated odom orientation measurement noise } // namespace /** @@ -67,7 +67,7 @@ struct Robot { rclcpp::Time stamp; - double mass; + double mass{}; double x = 0; double y = 0; @@ -93,8 +93,8 @@ nav_msgs::msg::Odometry robotToOdometry(Robot const& state) { nav_msgs::msg::Odometry msg; msg.header.stamp = state.stamp; - msg.header.frame_id = MAP_FRAME; - msg.child_frame_id = BASELINK_FRAME; + msg.header.frame_id = mapFrame; + msg.child_frame_id = baselinkFrame; msg.pose.pose.position.x = state.x; msg.pose.pose.position.y = state.y; msg.pose.pose.position.z = state.z; @@ -169,19 +169,19 @@ sensor_msgs::msg::Imu simulateImu(Robot const& robot) { static std::random_device rd{}; static std::mt19937 generator{ rd() }; - static std::normal_distribution<> noise{ 0.0, IMU_SIGMA }; + static std::normal_distribution<> noise{ 0.0, imuSigma }; sensor_msgs::msg::Imu msg; msg.header.stamp = robot.stamp; - msg.header.frame_id = BASELINK_FRAME; + msg.header.frame_id = baselinkFrame; // measure accel msg.linear_acceleration.x = robot.ax + noise(generator); msg.linear_acceleration.y = robot.ay + noise(generator); msg.linear_acceleration.z = robot.az + noise(generator); - msg.linear_acceleration_covariance[0] = IMU_SIGMA * IMU_SIGMA; - msg.linear_acceleration_covariance[4] = IMU_SIGMA * IMU_SIGMA; - msg.linear_acceleration_covariance[8] = IMU_SIGMA * IMU_SIGMA; + msg.linear_acceleration_covariance[0] = imuSigma * imuSigma; + msg.linear_acceleration_covariance[4] = imuSigma * imuSigma; + msg.linear_acceleration_covariance[8] = imuSigma * imuSigma; // Simulated IMU does not provide orientation (negative covariance indicates this) msg.orientation_covariance[0] = -1; @@ -191,9 +191,9 @@ sensor_msgs::msg::Imu simulateImu(Robot const& robot) msg.angular_velocity.x = robot.vroll + noise(generator); msg.angular_velocity.y = robot.vpitch + noise(generator); msg.angular_velocity.z = robot.vyaw + noise(generator); - msg.angular_velocity_covariance[0] = IMU_SIGMA * IMU_SIGMA; - msg.angular_velocity_covariance[4] = IMU_SIGMA * IMU_SIGMA; - msg.angular_velocity_covariance[8] = IMU_SIGMA * IMU_SIGMA; + msg.angular_velocity_covariance[0] = imuSigma * imuSigma; + msg.angular_velocity_covariance[4] = imuSigma * imuSigma; + msg.angular_velocity_covariance[8] = imuSigma * imuSigma; return msg; } @@ -201,20 +201,20 @@ nav_msgs::msg::Odometry simulateOdometry(const Robot& robot) { static std::random_device rd{}; static std::mt19937 generator{ rd() }; - static std::normal_distribution<> position_noise{ 0.0, ODOM_POSITION_SIGMA }; + static std::normal_distribution<> position_noise{ 0.0, odomPositionSigma }; nav_msgs::msg::Odometry msg; msg.header.stamp = robot.stamp; - msg.header.frame_id = ODOM_FRAME; - msg.child_frame_id = BASELINK_FRAME; + msg.header.frame_id = odomFrame; + msg.child_frame_id = baselinkFrame; // noisy position measurement msg.pose.pose.position.x = robot.x + position_noise(generator); msg.pose.pose.position.y = robot.y + position_noise(generator); msg.pose.pose.position.z = robot.z + position_noise(generator); - msg.pose.covariance[0] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; - msg.pose.covariance[7] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; - msg.pose.covariance[14] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + msg.pose.covariance[0] = odomPositionSigma * odomPositionSigma; + msg.pose.covariance[7] = odomPositionSigma * odomPositionSigma; + msg.pose.covariance[14] = odomPositionSigma * odomPositionSigma; // noisy orientation measurement tf2::Quaternion q; @@ -223,9 +223,9 @@ nav_msgs::msg::Odometry simulateOdometry(const Robot& robot) msg.pose.pose.orientation.x = q.x(); msg.pose.pose.orientation.y = q.y(); msg.pose.pose.orientation.z = q.z(); - msg.pose.covariance[21] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; - msg.pose.covariance[28] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; - msg.pose.covariance[35] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + msg.pose.covariance[21] = odomOrientationSigma * odomOrientationSigma; + msg.pose.covariance[28] = odomOrientationSigma * odomOrientationSigma; + msg.pose.covariance[35] = odomOrientationSigma * odomOrientationSigma; return msg; } @@ -234,7 +234,7 @@ void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces(); - srv->pose.header.frame_id = MAP_FRAME; + srv->pose.header.frame_id = mapFrame; srv->pose.pose.pose.position.x = state.x; srv->pose.pose.pose.position.y = state.y; srv->pose.pose.pose.position.z = state.z; @@ -306,21 +306,21 @@ int main(int argc, char** argv) // parameters that control the motion pattern of the robot double const motion_duration = 5; // length of time to oscillate on a given axis, in seconds - double const N_cycles = 2; // number of oscillations per `motion_duration` + double const n_cycles = 2; // number of oscillations per `motion_duration` while (rclcpp::ok()) { // store the first time this runs (since it won't start running exactly at a multiple of `motion_duration`) - static auto const first_time = node->now(); + static auto const firstTime = node->now(); auto const now = node->now(); // compensate for the original time offset - double now_d = (now - first_time).seconds(); + double const now_d = (now - firstTime).seconds(); // store how long it has been (resetting at `motion_duration` seconds) - double mod_time = std::fmod(now_d, motion_duration); + double const mod_time = std::fmod(now_d, motion_duration); // apply a harmonic force (oscillates `N_cycles` times per `motion_duration`) - double const force_magnitude = 100 * std::cos(2 * M_PI * N_cycles * mod_time / motion_duration); + double const force_magnitude = 100 * std::cos(2 * M_PI * n_cycles * mod_time / motion_duration); Eigen::Vector3d external_force = { 0, 0, 0 }; // switch oscillation axes every `motion_duration` seconds (with one 'rest period') diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp index efaeeb0f9..c1601dac4 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp @@ -118,7 +118,7 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -138,7 +138,7 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp index b91b1e1b5..39abe9994 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp @@ -152,7 +152,7 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -172,7 +172,7 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp index cbdf503cd..86b6a7659 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp @@ -135,7 +135,7 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -155,7 +155,7 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp index 11c71edaa..e5147accd 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp @@ -152,7 +152,7 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -172,7 +172,7 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/fixed_size_variable.hpp b/fuse_variables/include/fuse_variables/fixed_size_variable.hpp index 1d4e86f66..7c452e961 100644 --- a/fuse_variables/include/fuse_variables/fixed_size_variable.hpp +++ b/fuse_variables/include/fuse_variables/fixed_size_variable.hpp @@ -68,12 +68,16 @@ class FixedSizeVariable : public fuse_core::Variable /** * @brief A static version of the variable size */ - constexpr static size_t SIZE = N; + constexpr static size_t varSize = N; /** * @brief Default constructor */ FixedSizeVariable() = default; + FixedSizeVariable(FixedSizeVariable const&) = default; + FixedSizeVariable(FixedSizeVariable&&) = default; + FixedSizeVariable& operator=(FixedSizeVariable const&) = default; + FixedSizeVariable& operator=(FixedSizeVariable&&) = default; /** * @brief Constructor @@ -152,10 +156,6 @@ class FixedSizeVariable : public fuse_core::Variable archive& data_; } }; - -// Define the constant that was declared above -template -constexpr size_t FixedSizeVariable::SIZE; } // namespace fuse_variables #endif // FUSE_VARIABLES__FIXED_SIZE_VARIABLE_HPP_ diff --git a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp index 6393926f0..871f33ce7 100644 --- a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp @@ -266,7 +266,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * * @return A base pointer to an instance of a derived LocalParameterization */ - fuse_core::LocalParameterization* localParameterization() const override; + [[nodiscard]] fuse_core::LocalParameterization* localParameterization() const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -277,7 +277,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * * @return A base pointer to an instance of a derived manifold */ - fuse_core::Manifold* manifold() const override; + [[nodiscard]] fuse_core::Manifold* manifold() const override; #endif private: @@ -294,7 +294,7 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp index 1781597c1..ceae3a676 100644 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp @@ -36,6 +36,7 @@ #include +#include #include #include @@ -98,8 +99,8 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { double q_delta[4]; - ceres::AngleAxisToQuaternion(delta, q_delta); - ceres::QuaternionProduct(x, q_delta, x_plus_delta); + ceres::AngleAxisToQuaternion(delta, static_cast(q_delta)); + ceres::QuaternionProduct(x, static_cast(q_delta), x_plus_delta); return true; } @@ -129,10 +130,10 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati bool Minus(const double* x, const double* y, double* y_minus_x) const override { double x_inverse[4]; - QuaternionInverse(x, x_inverse); + QuaternionInverse(x, static_cast(x_inverse)); double q_delta[4]; - ceres::QuaternionProduct(x_inverse, y, q_delta); - ceres::QuaternionToAngleAxis(q_delta, y_minus_x); + ceres::QuaternionProduct(static_cast(x_inverse), y, static_cast(q_delta)); + ceres::QuaternionToAngleAxis(static_cast(q_delta), y_minus_x); return true; } @@ -203,8 +204,8 @@ class Orientation3DManifold : public fuse_core::Manifold bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { double q_delta[4]; - ceres::AngleAxisToQuaternion(delta, q_delta); - ceres::QuaternionProduct(x, q_delta, x_plus_delta); + ceres::AngleAxisToQuaternion(delta, static_cast(q_delta)); + ceres::QuaternionProduct(x, static_cast(q_delta), x_plus_delta); return true; } @@ -234,10 +235,10 @@ class Orientation3DManifold : public fuse_core::Manifold bool Minus(const double* y, const double* x, double* y_minus_x) const override { double x_inverse[4]; - QuaternionInverse(x, x_inverse); + QuaternionInverse(x, static_cast(x_inverse)); double q_delta[4]; - ceres::QuaternionProduct(x_inverse, y, q_delta); - ceres::QuaternionToAngleAxis(q_delta, y_minus_x); + ceres::QuaternionProduct(static_cast(x_inverse), y, static_cast(q_delta)); + ceres::QuaternionToAngleAxis(static_cast(q_delta), static_cast(y_minus_x)); return true; } @@ -445,7 +446,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * @return A pointer to a local parameterization object that indicates how to "add" increments to * the quaternion */ - fuse_core::LocalParameterization* localParameterization() const override; + [[nodiscard]] fuse_core::LocalParameterization* localParameterization() const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -453,7 +454,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * * @return A pointer to a manifold object that indicates how to "add" increments to the quaternion */ - fuse_core::Manifold* manifold() const override; + [[nodiscard]] fuse_core::Manifold* manifold() const override; #endif private: @@ -470,7 +471,7 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/pinhole_camera.hpp b/fuse_variables/include/fuse_variables/pinhole_camera.hpp index c74f32e3c..854460661 100644 --- a/fuse_variables/include/fuse_variables/pinhole_camera.hpp +++ b/fuse_variables/include/fuse_variables/pinhole_camera.hpp @@ -181,7 +181,7 @@ class PinholeCamera : public FixedSizeVariable<4> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -212,7 +212,7 @@ class PinholeCamera : public FixedSizeVariable<4> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp index bebfed26d..ee5a53d7a 100644 --- a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp +++ b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp @@ -91,7 +91,7 @@ class PinholeCameraFixed : public PinholeCamera * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } diff --git a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp index 623af71ac..c097e9e74 100644 --- a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp @@ -139,7 +139,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -160,7 +160,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp index 292d4b372..558fc8e74 100644 --- a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp @@ -134,7 +134,7 @@ class Point2DLandmark : public FixedSizeVariable<2> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -155,7 +155,7 @@ class Point2DLandmark : public FixedSizeVariable<2> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp index 699eb95fa..ca2eaa86f 100644 --- a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp @@ -156,7 +156,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -177,7 +177,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp index 4cd048e0e..630e51e4a 100644 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp @@ -154,7 +154,7 @@ class Point3DLandmark : public FixedSizeVariable<3> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -175,7 +175,7 @@ class Point3DLandmark : public FixedSizeVariable<3> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& id_; } }; diff --git a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp index af349ce34..55459d899 100644 --- a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp @@ -135,7 +135,7 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -155,7 +155,7 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp index 0e7bfd09a..faa614f9d 100644 --- a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp @@ -151,7 +151,7 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -171,7 +171,7 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/stamped.hpp b/fuse_variables/include/fuse_variables/stamped.hpp index 4f2da197d..98f186b72 100644 --- a/fuse_variables/include/fuse_variables/stamped.hpp +++ b/fuse_variables/include/fuse_variables/stamped.hpp @@ -76,6 +76,10 @@ class Stamped * @brief Destructor */ virtual ~Stamped() = default; + Stamped(Stamped const&) = default; + Stamped(Stamped&&) = default; + Stamped& operator=(Stamped const&) = default; + Stamped& operator=(Stamped&&) = default; /** * @brief Read-only access to the associated timestamp. @@ -94,7 +98,7 @@ class Stamped } private: - fuse_core::UUID device_id_; //!< The UUID associated with this specific device or hardware + fuse_core::UUID device_id_{}; //!< The UUID associated with this specific device or hardware rclcpp::Time stamp_{ 0, 0, RCL_ROS_TIME }; //!< The timestamp associated with this variable //!< instance diff --git a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp index 1050bb7c1..4b32ca169 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp @@ -117,7 +117,7 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -137,7 +137,7 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp index 000207d9f..48a79e6bd 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp @@ -151,7 +151,7 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -171,7 +171,7 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp index 9565bf33f..d70b9d05d 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp @@ -135,7 +135,7 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -155,7 +155,7 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp index 9c0c8b35b..23bf71587 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp @@ -152,7 +152,7 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold* manifold() const override + [[nodiscard]] fuse_core::Manifold* manifold() const override { return nullptr; } @@ -172,7 +172,7 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/src/orientation_2d_stamped.cpp b/fuse_variables/src/orientation_2d_stamped.cpp index 7b2f2d7a0..4f01dffb7 100644 --- a/fuse_variables/src/orientation_2d_stamped.cpp +++ b/fuse_variables/src/orientation_2d_stamped.cpp @@ -31,6 +31,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include @@ -68,7 +69,7 @@ fuse_core::LocalParameterization* Orientation2DStamped::localParameterization() } #if CERES_SUPPORTS_MANIFOLDS -fuse_core::Manifold* Orientation2DStamped::manifold() const +[[nodiscard]] fuse_core::Manifold* Orientation2DStamped::manifold() const { return new Orientation2DManifold(); } diff --git a/fuse_variables/src/orientation_3d_stamped.cpp b/fuse_variables/src/orientation_3d_stamped.cpp index b72f73a19..895ee1a4b 100644 --- a/fuse_variables/src/orientation_3d_stamped.cpp +++ b/fuse_variables/src/orientation_3d_stamped.cpp @@ -71,7 +71,7 @@ fuse_core::LocalParameterization* Orientation3DStamped::localParameterization() } #if CERES_SUPPORTS_MANIFOLDS -fuse_core::Manifold* Orientation3DStamped::manifold() const +[[nodiscard]] fuse_core::Manifold* Orientation3DStamped::manifold() const { return new Orientation3DManifold(); } diff --git a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp index f595a98f8..14b9e21d8 100644 --- a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::AccelerationAngular2DStamped; TEST(AccelerationAngular2DStamped, Type) { - AccelerationAngular2DStamped variable(rclcpp::Time(12345678, 910111213)); + AccelerationAngular2DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::AccelerationAngular2DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(AccelerationAngular2DStamped, UUID) { // Verify two accelerations at the same timestamp produce the same UUID { - AccelerationAngular2DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationAngular2DStamped variable2(rclcpp::Time(12345678, 910111213)); + AccelerationAngular2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationAngular2DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - AccelerationAngular2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - AccelerationAngular2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular2DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular2DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two accelerations at different timestamps produce different UUIDs { - AccelerationAngular2DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationAngular2DStamped variable2(rclcpp::Time(12345678, 910111214)); - AccelerationAngular2DStamped variable3(rclcpp::Time(12345679, 910111213)); + AccelerationAngular2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationAngular2DStamped const variable2(rclcpp::Time(12345678, 910111214)); + AccelerationAngular2DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(AccelerationAngular2DStamped, UUID) // Verify two accelerations with different hardware IDs produce different UUIDs { - AccelerationAngular2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - AccelerationAngular2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + AccelerationAngular2DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + AccelerationAngular2DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationAngular2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = AccelerationAngular2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(AccelerationAngular2DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -123,13 +121,13 @@ TEST(AccelerationAngular2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(acceleration.data(), acceleration.size()); + problem.AddParameterBlock(acceleration.data(), static_cast(acceleration.size())); std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp index 3f6a40061..ef59cad34 100644 --- a/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::AccelerationAngular3DStamped; TEST(AccelerationAngular3DStamped, Type) { - AccelerationAngular3DStamped variable(rclcpp::Time(12345678, 910111213)); + AccelerationAngular3DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::AccelerationAngular3DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(AccelerationAngular3DStamped, UUID) { // Verify two accelerations at the same timestamp produce the same UUID { - AccelerationAngular3DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationAngular3DStamped variable2(rclcpp::Time(12345678, 910111213)); + AccelerationAngular3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationAngular3DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - AccelerationAngular3DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - AccelerationAngular3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular3DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular3DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two accelerations at different timestamps produce different UUIDs { - AccelerationAngular3DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationAngular3DStamped variable2(rclcpp::Time(12345678, 910111214)); - AccelerationAngular3DStamped variable3(rclcpp::Time(12345679, 910111213)); + AccelerationAngular3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationAngular3DStamped const variable2(rclcpp::Time(12345678, 910111214)); + AccelerationAngular3DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,16 @@ TEST(AccelerationAngular3DStamped, UUID) // Verify two accelerations with different hardware IDs produce different UUIDs { - AccelerationAngular3DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); - AccelerationAngular3DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); + AccelerationAngular3DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + AccelerationAngular3DStamped const variable2(rclcpp::Time(12345678, 910111213), + fuse_core::uuid::generate("r4-p17")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationAngular3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = AccelerationAngular3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +101,7 @@ TEST(AccelerationAngular3DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -127,13 +126,13 @@ TEST(AccelerationAngular3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(acceleration.data(), acceleration.size()); + problem.AddParameterBlock(acceleration.data(), static_cast(acceleration.size())); std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp index 849a273fd..0513bc0c8 100644 --- a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::AccelerationLinear2DStamped; TEST(AccelerationLinear2DStamped, Type) { - AccelerationLinear2DStamped variable(rclcpp::Time(12345678, 910111213)); + AccelerationLinear2DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::AccelerationLinear2DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(AccelerationLinear2DStamped, UUID) { // Verify two accelerations at the same timestamp produce the same UUID { - AccelerationLinear2DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationLinear2DStamped variable2(rclcpp::Time(12345678, 910111213)); + AccelerationLinear2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationLinear2DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - AccelerationLinear2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - AccelerationLinear2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear2DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear2DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two accelerations at different timestamps produce different UUIDs { - AccelerationLinear2DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationLinear2DStamped variable2(rclcpp::Time(12345678, 910111214)); - AccelerationLinear2DStamped variable3(rclcpp::Time(12345679, 910111213)); + AccelerationLinear2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationLinear2DStamped const variable2(rclcpp::Time(12345678, 910111214)); + AccelerationLinear2DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(AccelerationLinear2DStamped, UUID) // Verify two accelerations with different hardware IDs produce different UUIDs { - AccelerationLinear2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - AccelerationLinear2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + AccelerationLinear2DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + AccelerationLinear2DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationLinear2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = AccelerationLinear2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(AccelerationLinear2DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -125,13 +123,13 @@ TEST(AccelerationLinear2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(acceleration.data(), acceleration.size()); + problem.AddParameterBlock(acceleration.data(), static_cast(acceleration.size())); std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp index a67b3e8b5..caf05a638 100644 --- a/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::AccelerationLinear3DStamped; TEST(AccelerationLinear3DStamped, Type) { - AccelerationLinear3DStamped variable(rclcpp::Time(12345678, 910111213)); + AccelerationLinear3DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::AccelerationLinear3DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(AccelerationLinear3DStamped, UUID) { // Verify two accelerations at the same timestamp produce the same UUID { - AccelerationLinear3DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationLinear3DStamped variable2(rclcpp::Time(12345678, 910111213)); + AccelerationLinear3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationLinear3DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - AccelerationLinear3DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - AccelerationLinear3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear3DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear3DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two accelerations at different timestamps produce different UUIDs { - AccelerationLinear3DStamped variable1(rclcpp::Time(12345678, 910111213)); - AccelerationLinear3DStamped variable2(rclcpp::Time(12345678, 910111214)); - AccelerationLinear3DStamped variable3(rclcpp::Time(12345679, 910111213)); + AccelerationLinear3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + AccelerationLinear3DStamped const variable2(rclcpp::Time(12345678, 910111214)); + AccelerationLinear3DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(AccelerationLinear3DStamped, UUID) // Verify two accelerations with different hardware IDs produce different UUIDs { - AccelerationLinear3DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); - AccelerationLinear3DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); + AccelerationLinear3DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + AccelerationLinear3DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationLinear3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = AccelerationLinear3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(AccelerationLinear3DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -127,13 +125,13 @@ TEST(AccelerationLinear3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(acceleration.data(), acceleration.size()); + problem.AddParameterBlock(acceleration.data(), static_cast(acceleration.size())); std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_fixed_size_variable.cpp b/fuse_variables/test/test_fixed_size_variable.cpp index 239f702d3..9c39e2a83 100644 --- a/fuse_variables/test/test_fixed_size_variable.cpp +++ b/fuse_variables/test/test_fixed_size_variable.cpp @@ -47,7 +47,11 @@ class TestVariable : public fuse_variables::FixedSizeVariable<2> TestVariable() : fuse_variables::FixedSizeVariable<2>(fuse_core::uuid::generate()) { } - virtual ~TestVariable() = default; + ~TestVariable() override = default; + TestVariable(TestVariable const&) = default; + TestVariable(TestVariable&&) = default; + TestVariable& operator=(TestVariable const&) = default; + TestVariable& operator=(TestVariable&&) = default; void print(std::ostream& /*stream = std::cout*/) const override { @@ -74,9 +78,9 @@ class TestVariable : public fuse_variables::FixedSizeVariable<2> TEST(FixedSizeVariable, Size) { // Verify the expected size is returned - TestVariable variable; - EXPECT_EQ(2u, variable.size()); // base class interface - EXPECT_EQ(2u, TestVariable::SIZE); // static member variable + TestVariable const variable; + EXPECT_EQ(2u, variable.size()); // base class interface + EXPECT_EQ(2u, TestVariable::varSize); // static member variable } TEST(FixedSizeVariable, Data) diff --git a/fuse_variables/test/test_orientation_2d_stamped.cpp b/fuse_variables/test/test_orientation_2d_stamped.cpp index a20ff82f0..910aaa6aa 100644 --- a/fuse_variables/test/test_orientation_2d_stamped.cpp +++ b/fuse_variables/test/test_orientation_2d_stamped.cpp @@ -52,7 +52,7 @@ using fuse_variables::Orientation2DStamped; TEST(Orientation2DStamped, Type) { - Orientation2DStamped variable(rclcpp::Time(12345678, 910111213)); + Orientation2DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::Orientation2DStamped", variable.type()); } @@ -60,20 +60,20 @@ TEST(Orientation2DStamped, UUID) { // Verify two velocities at the same timestamp produce the same UUID { - Orientation2DStamped variable1(rclcpp::Time(12345678, 910111213)); - Orientation2DStamped variable2(rclcpp::Time(12345678, 910111213)); + Orientation2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + Orientation2DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - Orientation2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - Orientation2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Orientation2DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Orientation2DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two velocities at different timestamps produce different UUIDs { - Orientation2DStamped variable1(rclcpp::Time(12345678, 910111213)); - Orientation2DStamped variable2(rclcpp::Time(12345678, 910111214)); - Orientation2DStamped variable3(rclcpp::Time(12345679, 910111213)); + Orientation2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + Orientation2DStamped const variable2(rclcpp::Time(12345678, 910111214)); + Orientation2DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -81,15 +81,15 @@ TEST(Orientation2DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - Orientation2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - Orientation2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + Orientation2DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + Orientation2DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(Orientation2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = Orientation2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -127,14 +127,15 @@ using Orientation2DLocalParameterization = TEST(Orientation2DStamped, Plus) { - auto parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); // Simple test { double x[1] = { 1.0 }; double delta[1] = { 0.5 }; double actual[1] = { 0.0 }; - bool success = parameterization->Plus(x, delta, actual); + bool const success = + parameterization->Plus(static_cast(x), static_cast(delta), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(1.5, actual[0], 1.0e-5); @@ -145,18 +146,17 @@ TEST(Orientation2DStamped, Plus) double x[1] = { 2.0 }; double delta[1] = { 3.0 }; double actual[1] = { 0.0 }; - bool success = parameterization->Plus(x, delta, actual); + bool const success = + parameterization->Plus(static_cast(x), static_cast(delta), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(5 - 2 * M_PI, actual[0], 1.0e-5); } - - delete parameterization; } TEST(Orientation2DStamped, PlusJacobian) { - auto parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation2DLocalParameterization(); auto test_values = std::vector{ -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; @@ -164,28 +164,27 @@ TEST(Orientation2DStamped, PlusJacobian) { double x[1] = { test_value }; double actual[1] = { 0.0 }; - bool success = parameterization->ComputeJacobian(x, actual); + bool const success = parameterization->ComputeJacobian(static_cast(x), static_cast(actual)); double expected[1] = { 0.0 }; - reference.ComputeJacobian(x, expected); + reference.ComputeJacobian(static_cast(x), static_cast(expected)); EXPECT_TRUE(success); EXPECT_NEAR(expected[0], actual[0], 1.0e-5); } - - delete parameterization; } TEST(Orientation2DStamped, Minus) { - auto parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); // Simple test { double x1[1] = { 1.0 }; double x2[1] = { 1.5 }; double actual[1] = { 0.0 }; - bool success = parameterization->Minus(x1, x2, actual); + bool const success = + parameterization->Minus(static_cast(x1), static_cast(x2), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(0.5, actual[0], 1.0e-5); @@ -196,7 +195,8 @@ TEST(Orientation2DStamped, Minus) double x1[1] = { 2.0 }; double x2[1] = { 5 - 2 * M_PI }; double actual[1] = { 0.0 }; - bool success = parameterization->Minus(x1, x2, actual); + bool const success = + parameterization->Minus(static_cast(x1), static_cast(x2), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(3.0, actual[0], 1.0e-5); @@ -205,7 +205,7 @@ TEST(Orientation2DStamped, Minus) TEST(Orientation2DStamped, MinusJacobian) { - auto parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation2DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation2DLocalParameterization(); auto test_values = std::vector{ -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; @@ -213,23 +213,19 @@ TEST(Orientation2DStamped, MinusJacobian) { double x[1] = { test_value }; double actual[1] = { 0.0 }; - bool success = parameterization->ComputeMinusJacobian(x, actual); + bool const success = parameterization->ComputeMinusJacobian(static_cast(x), static_cast(actual)); double expected[1] = { 0.0 }; - reference.ComputeMinusJacobian(x, expected); + reference.ComputeMinusJacobian(static_cast(x), static_cast(expected)); EXPECT_TRUE(success); EXPECT_NEAR(expected[0], actual[0], 1.0e-5); } - - delete parameterization; } struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -253,14 +249,14 @@ TEST(Orientation2DStamped, Optimization) #if !CERES_SUPPORTS_MANIFOLDS problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.localParameterization()); #else - problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.manifold()); + problem.AddParameterBlock(orientation.data(), static_cast(orientation.size()), orientation.manifold()); #endif std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); @@ -300,6 +296,7 @@ TEST(Orientation2DStamped, Serialization) struct Orientation2DFunctor { template + // NOLINTNEXTLINE bool Plus(const T* x, const T* delta, T* x_plus_delta) const { x_plus_delta[0] = fuse_core::wrapAngle2D(x[0] + delta[0]); @@ -307,6 +304,7 @@ struct Orientation2DFunctor } template + // NOLINTNEXTLINE bool Minus(const T* y, const T* x, T* y_minus_x) const { y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); @@ -318,14 +316,15 @@ using Orientation2DManifold = ceres::AutoDiffManifoldPlus(x, delta, actual); + bool const success = + manifold->Plus(static_cast(x), static_cast(delta), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(1.5, actual[0], 1.0e-5); @@ -336,18 +335,17 @@ TEST(Orientation2DStamped, ManifoldPlus) double x[1] = { 2.0 }; double delta[1] = { 3.0 }; double actual[1] = { 0.0 }; - bool success = manifold->Plus(x, delta, actual); + bool const success = + manifold->Plus(static_cast(x), static_cast(delta), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(5 - 2 * M_PI, actual[0], 1.0e-5); } - - delete manifold; } TEST(Orientation2DStamped, ManifoldPlusJacobian) { - auto manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); + auto* manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation2DManifold(); auto test_values = std::vector{ -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; @@ -355,28 +353,27 @@ TEST(Orientation2DStamped, ManifoldPlusJacobian) { double x[1] = { test_value }; double actual[1] = { 0.0 }; - bool success = manifold->PlusJacobian(x, actual); + bool const success = manifold->PlusJacobian(static_cast(x), static_cast(actual)); double expected[1] = { 0.0 }; - reference.PlusJacobian(x, expected); + reference.PlusJacobian(static_cast(x), static_cast(expected)); EXPECT_TRUE(success); EXPECT_NEAR(expected[0], actual[0], 1.0e-5); } - - delete manifold; } TEST(Orientation2DStamped, ManifoldMinus) { - auto manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); + auto* manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); // Simple test { double x1[1] = { 1.0 }; double x2[1] = { 1.5 }; double actual[1] = { 0.0 }; - bool success = manifold->Minus(x2, x1, actual); + bool const success = + manifold->Minus(static_cast(x2), static_cast(x1), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(0.5, actual[0], 1.0e-5); @@ -387,7 +384,8 @@ TEST(Orientation2DStamped, ManifoldMinus) double x1[1] = { 2.0 }; double x2[1] = { 5 - 2 * M_PI }; double actual[1] = { 0.0 }; - bool success = manifold->Minus(x2, x1, actual); + bool const success = + manifold->Minus(static_cast(x2), static_cast(x1), static_cast(actual)); EXPECT_TRUE(success); EXPECT_NEAR(3.0, actual[0], 1.0e-5); @@ -396,7 +394,7 @@ TEST(Orientation2DStamped, ManifoldMinus) TEST(Orientation2DStamped, ManifoldMinusJacobian) { - auto manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); + auto* manifold = Orientation2DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation2DManifold(); auto test_values = std::vector{ -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; @@ -404,7 +402,7 @@ TEST(Orientation2DStamped, ManifoldMinusJacobian) { double x[1] = { test_value }; double actual[1] = { 0.0 }; - bool success = manifold->MinusJacobian(x, actual); + bool const success = manifold->MinusJacobian(static_cast(x), static_cast(actual)); double expected[1] = { 0.0 }; reference.MinusJacobian(x, expected); @@ -412,7 +410,5 @@ TEST(Orientation2DStamped, ManifoldMinusJacobian) EXPECT_TRUE(success); EXPECT_NEAR(expected[0], actual[0], 1.0e-5); } - - delete manifold; } #endif diff --git a/fuse_variables/test/test_orientation_3d_stamped.cpp b/fuse_variables/test/test_orientation_3d_stamped.cpp index c50926b28..44acad27c 100644 --- a/fuse_variables/test/test_orientation_3d_stamped.cpp +++ b/fuse_variables/test/test_orientation_3d_stamped.cpp @@ -54,7 +54,7 @@ using fuse_variables::Orientation3DStamped; TEST(Orientation3DStamped, Type) { - Orientation3DStamped variable(rclcpp::Time(12345678, 910111213)); + Orientation3DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::Orientation3DStamped", variable.type()); } @@ -62,8 +62,8 @@ TEST(Orientation3DStamped, UUID) { // Verify two orientations at the same timestamp produce the same UUID { - Orientation3DStamped variable1(rclcpp::Time(12345678, 910111213)); - Orientation3DStamped variable2(rclcpp::Time(12345678, 910111213)); + Orientation3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + Orientation3DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } @@ -72,52 +72,55 @@ TEST(Orientation3DStamped, UUID) // Verify two orientations at the same timestamp and same hardware ID produce the same UUID { - Orientation3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Orientation3DStamped variable2(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable2(rclcpp::Time(12345678, 910111213), uuid_1); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two orientations with the same timestamp but different hardware IDs generate different // UUIDs { - Orientation3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Orientation3DStamped variable2(rclcpp::Time(12345678, 910111213), uuid_2); + Orientation3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable2(rclcpp::Time(12345678, 910111213), uuid_2); EXPECT_NE(variable1.uuid(), variable2.uuid()); } // Verify two orientations with the same hardware ID and different timestamps produce different // UUIDs { - Orientation3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Orientation3DStamped variable2(rclcpp::Time(12345678, 910111214), uuid_1); + Orientation3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable2(rclcpp::Time(12345678, 910111214), uuid_1); EXPECT_NE(variable1.uuid(), variable2.uuid()); - Orientation3DStamped variable3(rclcpp::Time(12345678, 910111213), uuid_1); - Orientation3DStamped variable4(rclcpp::Time(12345679, 910111213), uuid_1); + Orientation3DStamped const variable3(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable4(rclcpp::Time(12345679, 910111213), uuid_1); EXPECT_NE(variable3.uuid(), variable4.uuid()); } // Verify two orientations with different hardware IDs and different timestamps produce different // UUIDs { - Orientation3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Orientation3DStamped variable2(rclcpp::Time(12345678, 910111214), uuid_2); + Orientation3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable2(rclcpp::Time(12345678, 910111214), uuid_2); EXPECT_NE(variable1.uuid(), variable2.uuid()); - Orientation3DStamped variable3(rclcpp::Time(12345678, 910111213), uuid_1); - Orientation3DStamped variable4(rclcpp::Time(12345679, 910111213), uuid_2); + Orientation3DStamped const variable3(rclcpp::Time(12345678, 910111213), uuid_1); + Orientation3DStamped const variable4(rclcpp::Time(12345679, 910111213), uuid_2); EXPECT_NE(variable3.uuid(), variable4.uuid()); } } +namespace +{ template -inline static void QuaternionInverse(const T in[4], T out[4]) +inline void QuaternionInverse(const T in[4], T out[4]) { out[0] = in[0]; out[1] = -in[1]; out[2] = -in[2]; out[3] = -in[3]; } +} // namespace struct Orientation3DPlus { @@ -150,58 +153,56 @@ using Orientation3DLocalParameterization = TEST(Orientation3DStamped, Plus) { - auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); double x[4] = { 0.842614977, 0.2, 0.3, 0.4 }; double delta[3] = { 0.15, -0.2, 0.433012702 }; double result[4] = { 0.0, 0.0, 0.0, 0.0 }; - bool success = parameterization->Plus(x, delta, result); + bool const success = parameterization->Plus(x, delta, result); EXPECT_TRUE(success); EXPECT_NEAR(0.745561, result[0], 1.0e-5); EXPECT_NEAR(0.360184, result[1], 1.0e-5); EXPECT_NEAR(0.194124, result[2], 1.0e-5); EXPECT_NEAR(0.526043, result[3], 1.0e-5); - - delete parameterization; } TEST(Orientation3DStamped, Minus) { - auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); double x1[4] = { 0.842614977, 0.2, 0.3, 0.4 }; double x2[4] = { 0.745561, 0.360184, 0.194124, 0.526043 }; double result[3] = { 0.0, 0.0, 0.0 }; - bool success = parameterization->Minus(x1, x2, result); + bool const success = parameterization->Minus(x1, x2, result); EXPECT_TRUE(success); EXPECT_NEAR(0.15, result[0], 1.0e-5); EXPECT_NEAR(-0.2, result[1], 1.0e-5); EXPECT_NEAR(0.433012702, result[2], 1.0e-5); - - delete parameterization; } TEST(Orientation3DStamped, PlusJacobian) { - auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation3DLocalParameterization(); + // NOLINTBEGIN(clang-analyzer-security.FloatLoopCounter) for (double qx = -0.5; qx < 0.5; qx += 0.1) { for (double qy = -0.5; qy < 0.5; qy += 0.1) { for (double qz = -0.5; qz < 0.5; qz += 0.1) { - double qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); + // NOLINTEND(clang-analyzer-security.FloatLoopCounter) + double const qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); double x[4] = { qw, qx, qy, qz }; fuse_core::MatrixXd actual(4, 3); /* *INDENT-OFF* */ actual << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ - bool success = parameterization->ComputeJacobian(x, actual.data()); + bool const success = parameterization->ComputeJacobian(x, actual.data()); fuse_core::MatrixXd expected(4, 3); /* *INDENT-OFF* */ @@ -210,7 +211,7 @@ TEST(Orientation3DStamped, PlusJacobian) reference.ComputeJacobian(x, expected.data()); EXPECT_TRUE(success); - Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); + Eigen::IOFormat const clean(4, 0, ", ", "\n", "[", "]"); EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" << "Actual is:\n" @@ -220,29 +221,29 @@ TEST(Orientation3DStamped, PlusJacobian) } } } - - delete parameterization; } TEST(Orientation3DStamped, MinusJacobian) { - auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); + auto* parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation3DLocalParameterization(); + // NOLINTBEGIN(clang-analyzer-security.FloatLoopCounter) for (double qx = -0.5; qx < 0.5; qx += 0.1) { for (double qy = -0.5; qy < 0.5; qy += 0.1) { for (double qz = -0.5; qz < 0.5; qz += 0.1) { - double qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); + // NOLINTEND(clang-analyzer-security.FloatLoopCounter) + double const qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); double x[4] = { qw, qx, qy, qz }; fuse_core::MatrixXd actual(3, 4); /* *INDENT-OFF* */ actual << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ - bool success = parameterization->ComputeMinusJacobian(x, actual.data()); + bool const success = parameterization->ComputeMinusJacobian(x, actual.data()); fuse_core::MatrixXd expected(3, 4); /* *INDENT-OFF* */ @@ -251,7 +252,7 @@ TEST(Orientation3DStamped, MinusJacobian) reference.ComputeMinusJacobian(x, expected.data()); EXPECT_TRUE(success); - Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); + Eigen::IOFormat const clean(4, 0, ", ", "\n", "[", "]"); EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" << "Actual is:\n" @@ -261,13 +262,11 @@ TEST(Orientation3DStamped, MinusJacobian) } } } - - delete parameterization; } TEST(Orientation3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = Orientation3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -282,7 +281,7 @@ TEST(Orientation3DStamped, Stamped) struct QuaternionCostFunction { - explicit QuaternionCostFunction(double* observation) + explicit QuaternionCostFunction(double const* observation) { observation_[0] = observation[0]; observation_[1] = observation[1]; @@ -309,7 +308,7 @@ struct QuaternionCostFunction return true; } - double observation_[4]; + double observation_[4]{}; }; TEST(Orientation3DStamped, Optimization) @@ -331,14 +330,14 @@ TEST(Orientation3DStamped, Optimization) #if !CERES_SUPPORTS_MANIFOLDS problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.localParameterization()); #else - problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.manifold()); + problem.AddParameterBlock(orientation.data(), static_cast(orientation.size()), orientation.manifold()); #endif std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); @@ -351,7 +350,7 @@ TEST(Orientation3DStamped, Optimization) TEST(Orientation3DStamped, Euler) { - const double RAD_TO_DEG = 180.0 / M_PI; + const double rad_to_deg = 180.0 / M_PI; // Create an Orientation3DStamped with R, P, Y values of 10, -20, 30 degrees Orientation3DStamped orientation_r(rclcpp::Time(12345678, 910111213)); @@ -360,7 +359,7 @@ TEST(Orientation3DStamped, Euler) orientation_r.y() = 0.0; orientation_r.z() = 0.0; - EXPECT_NEAR(10.0, RAD_TO_DEG * orientation_r.roll(), 1e-5); + EXPECT_NEAR(10.0, rad_to_deg * orientation_r.roll(), 1e-5); Orientation3DStamped orientation_p(rclcpp::Time(12345678, 910111213)); orientation_p.w() = 0.9848078; @@ -368,7 +367,7 @@ TEST(Orientation3DStamped, Euler) orientation_p.y() = -0.1736482; orientation_p.z() = 0.0; - EXPECT_NEAR(-20.0, RAD_TO_DEG * orientation_p.pitch(), 1e-5); + EXPECT_NEAR(-20.0, rad_to_deg * orientation_p.pitch(), 1e-5); Orientation3DStamped orientation_y(rclcpp::Time(12345678, 910111213)); orientation_y.w() = 0.9659258; @@ -376,7 +375,7 @@ TEST(Orientation3DStamped, Euler) orientation_y.y() = 0.0; orientation_y.z() = 0.258819; - EXPECT_NEAR(30.0, RAD_TO_DEG * orientation_y.yaw(), 1e-5); + EXPECT_NEAR(30.0, rad_to_deg * orientation_y.yaw(), 1e-5); } TEST(Orientation3DStamped, Serialization) @@ -417,6 +416,7 @@ TEST(Orientation3DStamped, Serialization) struct Orientation3DFunctor { template + // NOLINTNEXTLINE bool Plus(const T* x, const T* delta, T* x_plus_delta) const { T q_delta[4]; @@ -425,6 +425,7 @@ struct Orientation3DFunctor return true; } template + // NOLINTNEXTLINE bool Minus(const T* y, const T* x, T* y_minus_x) const { T x_inverse[4]; @@ -440,41 +441,41 @@ using Orientation3DManifold = ceres::AutoDiffManifoldPlus(x, delta, result); + bool const success = manifold->Plus(x, delta, result); EXPECT_TRUE(success); EXPECT_NEAR(0.745561, result[0], 1.0e-5); EXPECT_NEAR(0.360184, result[1], 1.0e-5); EXPECT_NEAR(0.194124, result[2], 1.0e-5); EXPECT_NEAR(0.526043, result[3], 1.0e-5); - - delete manifold; } TEST(Orientation3DStamped, ManifoldPlusJacobian) { - auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); + auto* manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation3DManifold(); + // NOLINTBEGIN(clang-analyzer-security.FloatLoopCounter) for (double qx = -0.5; qx < 0.5; qx += 0.1) { for (double qy = -0.5; qy < 0.5; qy += 0.1) { for (double qz = -0.5; qz < 0.5; qz += 0.1) { - double qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); + // NOLINTEND(clang-analyzer-security.FloatLoopCounter) + double const qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); double x[4] = { qw, qx, qy, qz }; fuse_core::MatrixXd actual(4, 3); /* *INDENT-OFF* */ actual << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ - bool success = manifold->PlusJacobian(x, actual.data()); + bool const success = manifold->PlusJacobian(x, actual.data()); fuse_core::MatrixXd expected(4, 3); /* *INDENT-OFF* */ @@ -483,7 +484,7 @@ TEST(Orientation3DStamped, ManifoldPlusJacobian) reference.PlusJacobian(x, expected.data()); EXPECT_TRUE(success); - Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); + Eigen::IOFormat const clean(4, 0, ", ", "\n", "[", "]"); EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" << "Actual is:\n" @@ -493,8 +494,6 @@ TEST(Orientation3DStamped, ManifoldPlusJacobian) } } } - - delete manifold; } TEST(Orientation3DStamped, ManifoldMinus) @@ -503,36 +502,36 @@ TEST(Orientation3DStamped, ManifoldMinus) double x2[4] = { 0.745561, 0.360184, 0.194124, 0.526043 }; double result[3] = { 0.0, 0.0, 0.0 }; - auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); - bool success = manifold->Minus(x2, x1, result); + auto* manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); + bool const success = manifold->Minus(x2, x1, result); EXPECT_TRUE(success); EXPECT_NEAR(0.15, result[0], 1.0e-5); EXPECT_NEAR(-0.2, result[1], 1.0e-5); EXPECT_NEAR(0.433012702, result[2], 1.0e-5); - - delete manifold; } TEST(Orientation3DStamped, ManifoldMinusJacobian) { - auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); + auto* manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation3DManifold(); + // NOLINTBEGIN(clang-analyzer-security.FloatLoopCounter) for (double qx = -0.5; qx < 0.5; qx += 0.1) { for (double qy = -0.5; qy < 0.5; qy += 0.1) { for (double qz = -0.5; qz < 0.5; qz += 0.1) { - double qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); + // NOLINTEND(clang-analyzer-security.FloatLoopCounter) + double const qw = std::sqrt(1.0 - qx * qx - qy * qy - qz * qz); double x[4] = { qw, qx, qy, qz }; fuse_core::MatrixXd actual(3, 4); /* *INDENT-OFF* */ actual << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; /* *INDENT-ON* */ - bool success = manifold->MinusJacobian(x, actual.data()); + bool const success = manifold->MinusJacobian(x, actual.data()); fuse_core::MatrixXd expected(3, 4); /* *INDENT-OFF* */ @@ -541,7 +540,7 @@ TEST(Orientation3DStamped, ManifoldMinusJacobian) reference.MinusJacobian(x, expected.data()); EXPECT_TRUE(success); - Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); + Eigen::IOFormat const clean(4, 0, ", ", "\n", "[", "]"); EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" << "Actual is:\n" @@ -551,8 +550,6 @@ TEST(Orientation3DStamped, ManifoldMinusJacobian) } } } - - delete manifold; } TEST(Orientation3DStamped, ManifoldSerialization) diff --git a/fuse_variables/test/test_point_2d_fixed_landmark.cpp b/fuse_variables/test/test_point_2d_fixed_landmark.cpp index 45b13ffd7..084176cc8 100644 --- a/fuse_variables/test/test_point_2d_fixed_landmark.cpp +++ b/fuse_variables/test/test_point_2d_fixed_landmark.cpp @@ -48,7 +48,7 @@ using fuse_variables::Point2DFixedLandmark; TEST(Point2DFixedLandmark, Type) { - Point2DFixedLandmark variable(0); + Point2DFixedLandmark const variable(0); EXPECT_EQ("fuse_variables::Point2DFixedLandmark", variable.type()); } @@ -56,24 +56,22 @@ TEST(Point2DFixedLandmark, UUID) { // Verify two positions with the same landmark ids produce the same uuids { - Point2DFixedLandmark variable1(0); - Point2DFixedLandmark variable2(0); + Point2DFixedLandmark const variable1(0); + Point2DFixedLandmark const variable2(0); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two positions with the different landmark ids produce different uuids { - Point2DFixedLandmark variable1(0); - Point2DFixedLandmark variable2(1); + Point2DFixedLandmark const variable1(0); + Point2DFixedLandmark const variable2(1); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -96,7 +94,7 @@ TEST(Point2DFixedLandmark, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size()); + problem.AddParameterBlock(position.data(), static_cast(position.size())); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -106,7 +104,7 @@ TEST(Point2DFixedLandmark, Optimization) } // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_point_2d_landmark.cpp b/fuse_variables/test/test_point_2d_landmark.cpp index 0615470dd..bc4dfcb83 100644 --- a/fuse_variables/test/test_point_2d_landmark.cpp +++ b/fuse_variables/test/test_point_2d_landmark.cpp @@ -48,7 +48,7 @@ using fuse_variables::Point2DLandmark; TEST(Point2DLandmark, Type) { - Point2DLandmark variable(0); + Point2DLandmark const variable(0); EXPECT_EQ("fuse_variables::Point2DLandmark", variable.type()); } @@ -56,24 +56,22 @@ TEST(Point2DLandmark, UUID) { // Verify two positions with the same landmark ids produce the same uuids { - Point2DLandmark variable1(0); - Point2DLandmark variable2(0); + Point2DLandmark const variable1(0); + Point2DLandmark const variable2(0); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two positions with the different landmark ids produce different uuids { - Point2DLandmark variable1(0); - Point2DLandmark variable2(1); + Point2DLandmark const variable1(0); + Point2DLandmark const variable2(1); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -96,13 +94,13 @@ TEST(Point2DLandmark, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size()); + problem.AddParameterBlock(position.data(), static_cast(position.size())); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_point_3d_fixed_landmark.cpp b/fuse_variables/test/test_point_3d_fixed_landmark.cpp index 5bdf54a46..282b6e2c8 100644 --- a/fuse_variables/test/test_point_3d_fixed_landmark.cpp +++ b/fuse_variables/test/test_point_3d_fixed_landmark.cpp @@ -48,7 +48,7 @@ using fuse_variables::Point3DFixedLandmark; TEST(Point3DFixedLandmark, Type) { - Point3DFixedLandmark variable(0); + Point3DFixedLandmark const variable(0); EXPECT_EQ("fuse_variables::Point3DFixedLandmark", variable.type()); } @@ -56,24 +56,22 @@ TEST(Point3DFixedLandmark, UUID) { // Verify two positions with the same landmark ids produce the same uuids { - Point3DFixedLandmark variable1(0); - Point3DFixedLandmark variable2(0); + Point3DFixedLandmark const variable1(0); + Point3DFixedLandmark const variable2(0); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two positions with the different landmark ids produce different uuids { - Point3DFixedLandmark variable1(0); - Point3DFixedLandmark variable2(1); + Point3DFixedLandmark const variable1(0); + Point3DFixedLandmark const variable2(1); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -98,7 +96,7 @@ TEST(Point3DFixedLandmark, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size()); + problem.AddParameterBlock(position.data(), static_cast(position.size())); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -108,7 +106,7 @@ TEST(Point3DFixedLandmark, Optimization) } // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_point_3d_landmark.cpp b/fuse_variables/test/test_point_3d_landmark.cpp index 055b25331..3531074cb 100644 --- a/fuse_variables/test/test_point_3d_landmark.cpp +++ b/fuse_variables/test/test_point_3d_landmark.cpp @@ -48,7 +48,7 @@ using fuse_variables::Point3DLandmark; TEST(Point3DLandmark, Type) { - Point3DLandmark variable(0); + Point3DLandmark const variable(0); EXPECT_EQ("fuse_variables::Point3DLandmark", variable.type()); } @@ -56,24 +56,22 @@ TEST(Point3DLandmark, UUID) { // Verify two positions with the same landmark ids produce the same uuids { - Point3DLandmark variable1(0); - Point3DLandmark variable2(0); + Point3DLandmark const variable1(0); + Point3DLandmark const variable2(0); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two positions with the different landmark ids produce different uuids { - Point3DLandmark variable1(0); - Point3DLandmark variable2(1); + Point3DLandmark const variable1(0); + Point3DLandmark const variable2(1); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -98,13 +96,13 @@ TEST(Point3DLandmark, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size()); + problem.AddParameterBlock(position.data(), static_cast(position.size())); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_position_2d_stamped.cpp b/fuse_variables/test/test_position_2d_stamped.cpp index ce35789db..0feeafe16 100644 --- a/fuse_variables/test/test_position_2d_stamped.cpp +++ b/fuse_variables/test/test_position_2d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::Position2DStamped; TEST(Position2DStamped, Type) { - Position2DStamped variable(rclcpp::Time(12345678, 910111213)); + Position2DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::Position2DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(Position2DStamped, UUID) { // Verify two velocities at the same timestamp produce the same UUID { - Position2DStamped variable1(rclcpp::Time(12345678, 910111213)); - Position2DStamped variable2(rclcpp::Time(12345678, 910111213)); + Position2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + Position2DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - Position2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - Position2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Position2DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Position2DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two velocities at different timestamps produce different UUIDs { - Position2DStamped variable1(rclcpp::Time(12345678, 910111213)); - Position2DStamped variable2(rclcpp::Time(12345678, 910111214)); - Position2DStamped variable3(rclcpp::Time(12345679, 910111213)); + Position2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + Position2DStamped const variable2(rclcpp::Time(12345678, 910111214)); + Position2DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(Position2DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - Position2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - Position2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + Position2DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + Position2DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(Position2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = Position2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(Position2DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -125,13 +123,13 @@ TEST(Position2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size()); + problem.AddParameterBlock(position.data(), static_cast(position.size())); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_position_3d_stamped.cpp b/fuse_variables/test/test_position_3d_stamped.cpp index c70746b53..3e4052012 100644 --- a/fuse_variables/test/test_position_3d_stamped.cpp +++ b/fuse_variables/test/test_position_3d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::Position3DStamped; TEST(Position3DStamped, Type) { - Position3DStamped variable(rclcpp::Time(12345678, 910111213)); + Position3DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::Position3DStamped", variable.type()); } @@ -56,8 +56,8 @@ TEST(Position3DStamped, UUID) { // Verify two positions at the same timestamp produce the same UUID { - Position3DStamped variable1(rclcpp::Time(12345678, 910111213)); - Position3DStamped variable2(rclcpp::Time(12345678, 910111213)); + Position3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + Position3DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } @@ -66,46 +66,46 @@ TEST(Position3DStamped, UUID) // Verify two positions at the same timestamp and same hardware ID produce the same UUID { - Position3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Position3DStamped variable2(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable2(rclcpp::Time(12345678, 910111213), uuid_1); EXPECT_EQ(variable1.uuid(), variable2.uuid()); } // Verify two positions with the same timestamp but different hardware IDs generate different // UUIDs { - Position3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Position3DStamped variable2(rclcpp::Time(12345678, 910111213), uuid_2); + Position3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable2(rclcpp::Time(12345678, 910111213), uuid_2); EXPECT_NE(variable1.uuid(), variable2.uuid()); } // Verify two positions with the same hardware ID and different timestamps produce different UUIDs { - Position3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Position3DStamped variable2(rclcpp::Time(12345678, 910111214), uuid_1); + Position3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable2(rclcpp::Time(12345678, 910111214), uuid_1); EXPECT_NE(variable1.uuid(), variable2.uuid()); - Position3DStamped variable3(rclcpp::Time(12345678, 910111213), uuid_1); - Position3DStamped variable4(rclcpp::Time(12345679, 910111213), uuid_1); + Position3DStamped const variable3(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable4(rclcpp::Time(12345679, 910111213), uuid_1); EXPECT_NE(variable3.uuid(), variable4.uuid()); } // Verify two positions with different hardware IDs and different timestamps produce different // UUIDs { - Position3DStamped variable1(rclcpp::Time(12345678, 910111213), uuid_1); - Position3DStamped variable2(rclcpp::Time(12345678, 910111214), uuid_2); + Position3DStamped const variable1(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable2(rclcpp::Time(12345678, 910111214), uuid_2); EXPECT_NE(variable1.uuid(), variable2.uuid()); - Position3DStamped variable3(rclcpp::Time(12345678, 910111213), uuid_1); - Position3DStamped variable4(rclcpp::Time(12345679, 910111213), uuid_2); + Position3DStamped const variable3(rclcpp::Time(12345678, 910111213), uuid_1); + Position3DStamped const variable4(rclcpp::Time(12345679, 910111213), uuid_2); EXPECT_NE(variable3.uuid(), variable4.uuid()); } } TEST(Position3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = Position3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -120,9 +120,7 @@ TEST(Position3DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -147,13 +145,13 @@ TEST(Position3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(position.data(), position.size()); + problem.AddParameterBlock(position.data(), static_cast(position.size())); std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp index 029af9778..51d290c7d 100644 --- a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp +++ b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::VelocityAngular2DStamped; TEST(VelocityAngular2DStamped, Type) { - VelocityAngular2DStamped variable(rclcpp::Time(12345678, 910111213)); + VelocityAngular2DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::VelocityAngular2DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(VelocityAngular2DStamped, UUID) { // Verify two velocities at the same timestamp produce the same UUID { - VelocityAngular2DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityAngular2DStamped variable2(rclcpp::Time(12345678, 910111213)); + VelocityAngular2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityAngular2DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - VelocityAngular2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - VelocityAngular2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular2DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular2DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two velocities at different timestamps produce different UUIDs { - VelocityAngular2DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityAngular2DStamped variable2(rclcpp::Time(12345678, 910111214)); - VelocityAngular2DStamped variable3(rclcpp::Time(12345679, 910111213)); + VelocityAngular2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityAngular2DStamped const variable2(rclcpp::Time(12345678, 910111214)); + VelocityAngular2DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(VelocityAngular2DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - VelocityAngular2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - VelocityAngular2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + VelocityAngular2DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + VelocityAngular2DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityAngular2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = VelocityAngular2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(VelocityAngular2DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -123,13 +121,13 @@ TEST(VelocityAngular2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(velocity.data(), velocity.size()); + problem.AddParameterBlock(velocity.data(), static_cast(velocity.size())); std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_velocity_angular_3d_stamped.cpp b/fuse_variables/test/test_velocity_angular_3d_stamped.cpp index 5d269cb39..3dfb391c2 100644 --- a/fuse_variables/test/test_velocity_angular_3d_stamped.cpp +++ b/fuse_variables/test/test_velocity_angular_3d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::VelocityAngular3DStamped; TEST(VelocityAngular3DStamped, Type) { - VelocityAngular3DStamped variable(rclcpp::Time(12345678, 910111213)); + VelocityAngular3DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::VelocityAngular3DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(VelocityAngular3DStamped, UUID) { // Verify two velocities at the same timestamp produce the same UUID { - VelocityAngular3DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityAngular3DStamped variable2(rclcpp::Time(12345678, 910111213)); + VelocityAngular3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityAngular3DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - VelocityAngular3DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - VelocityAngular3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular3DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular3DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two velocities at different timestamps produce different UUIDs { - VelocityAngular3DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityAngular3DStamped variable2(rclcpp::Time(12345678, 910111214)); - VelocityAngular3DStamped variable3(rclcpp::Time(12345679, 910111213)); + VelocityAngular3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityAngular3DStamped const variable2(rclcpp::Time(12345678, 910111214)); + VelocityAngular3DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(VelocityAngular3DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - VelocityAngular3DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); - VelocityAngular3DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); + VelocityAngular3DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + VelocityAngular3DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityAngular3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = VelocityAngular3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(VelocityAngular3DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -127,13 +125,13 @@ TEST(VelocityAngular3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(velocity.data(), velocity.size()); + problem.AddParameterBlock(velocity.data(), static_cast(velocity.size())); std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp index ea8aef29e..a1f17855d 100644 --- a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp +++ b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::VelocityLinear2DStamped; TEST(VelocityLinear2DStamped, Type) { - VelocityLinear2DStamped variable(rclcpp::Time(12345678, 910111213)); + VelocityLinear2DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::VelocityLinear2DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(VelocityLinear2DStamped, UUID) { // Verify two velocities at the same timestamp produce the same UUID { - VelocityLinear2DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityLinear2DStamped variable2(rclcpp::Time(12345678, 910111213)); + VelocityLinear2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityLinear2DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - VelocityLinear2DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - VelocityLinear2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityLinear2DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityLinear2DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two velocities at different timestamps produce different UUIDs { - VelocityLinear2DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityLinear2DStamped variable2(rclcpp::Time(12345678, 910111214)); - VelocityLinear2DStamped variable3(rclcpp::Time(12345679, 910111213)); + VelocityLinear2DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityLinear2DStamped const variable2(rclcpp::Time(12345678, 910111214)); + VelocityLinear2DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(VelocityLinear2DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - VelocityLinear2DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); - VelocityLinear2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); + VelocityLinear2DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + VelocityLinear2DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityLinear2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = VelocityLinear2DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(VelocityLinear2DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -125,13 +123,13 @@ TEST(VelocityLinear2DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(velocity.data(), velocity.size()); + problem.AddParameterBlock(velocity.data(), static_cast(velocity.size())); std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/fuse_variables/test/test_velocity_linear_3d_stamped.cpp b/fuse_variables/test/test_velocity_linear_3d_stamped.cpp index 7bcca429d..f37a38b07 100644 --- a/fuse_variables/test/test_velocity_linear_3d_stamped.cpp +++ b/fuse_variables/test/test_velocity_linear_3d_stamped.cpp @@ -48,7 +48,7 @@ using fuse_variables::VelocityLinear3DStamped; TEST(VelocityLinear3DStamped, Type) { - VelocityLinear3DStamped variable(rclcpp::Time(12345678, 910111213)); + VelocityLinear3DStamped const variable(rclcpp::Time(12345678, 910111213)); EXPECT_EQ("fuse_variables::VelocityLinear3DStamped", variable.type()); } @@ -56,20 +56,20 @@ TEST(VelocityLinear3DStamped, UUID) { // Verify two velocities at the same timestamp produce the same UUID { - VelocityLinear3DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityLinear3DStamped variable2(rclcpp::Time(12345678, 910111213)); + VelocityLinear3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityLinear3DStamped const variable2(rclcpp::Time(12345678, 910111213)); EXPECT_EQ(variable1.uuid(), variable2.uuid()); - VelocityLinear3DStamped variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); - VelocityLinear3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityLinear3DStamped const variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityLinear3DStamped const variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } // Verify two velocities at different timestamps produce different UUIDs { - VelocityLinear3DStamped variable1(rclcpp::Time(12345678, 910111213)); - VelocityLinear3DStamped variable2(rclcpp::Time(12345678, 910111214)); - VelocityLinear3DStamped variable3(rclcpp::Time(12345679, 910111213)); + VelocityLinear3DStamped const variable1(rclcpp::Time(12345678, 910111213)); + VelocityLinear3DStamped const variable2(rclcpp::Time(12345678, 910111214)); + VelocityLinear3DStamped const variable3(rclcpp::Time(12345679, 910111213)); EXPECT_NE(variable1.uuid(), variable2.uuid()); EXPECT_NE(variable1.uuid(), variable3.uuid()); EXPECT_NE(variable2.uuid(), variable3.uuid()); @@ -77,15 +77,15 @@ TEST(VelocityLinear3DStamped, UUID) // Verify two velocities with different hardware IDs produce different UUIDs { - VelocityLinear3DStamped variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); - VelocityLinear3DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); + VelocityLinear3DStamped const variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + VelocityLinear3DStamped const variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r4-p17")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityLinear3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = + fuse_core::Variable::SharedPtr const base = VelocityLinear3DStamped::make_shared(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); auto derived = std::dynamic_pointer_cast(base); ASSERT_TRUE(static_cast(derived)); @@ -100,9 +100,7 @@ TEST(VelocityLinear3DStamped, Stamped) struct CostFunctor { - CostFunctor() - { - } + CostFunctor() = default; template bool operator()(const T* const x, T* residual) const @@ -127,13 +125,13 @@ TEST(VelocityLinear3DStamped, Optimization) // Build the problem. ceres::Problem problem; - problem.AddParameterBlock(velocity.data(), velocity.size()); + problem.AddParameterBlock(velocity.data(), static_cast(velocity.size())); std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver - ceres::Solver::Options options; + ceres::Solver::Options const options; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary);