diff --git a/.clang-format b/.clang-format new file mode 100644 index 000000000..88156faef --- /dev/null +++ b/.clang-format @@ -0,0 +1,76 @@ +--- +# This was copied over from the moveit_studio repository. +BasedOnStyle: Google +ColumnLimit: 120 +MaxEmptyLinesToKeep: 1 +SortIncludes: false + +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +NamespaceIndentation: None +ContinuationIndentWidth: 4 +IndentCaseLabels: true +IndentFunctionDeclarationAfterType: false + +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true + +AllowAllParametersOfDeclarationOnNextLine: false +ExperimentalAutoDetectBinPacking: false +ObjCSpaceBeforeProtocolList: true +Cpp11BracedListStyle: false + +AllowShortBlocksOnASingleLine: true +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortCaseLabelsOnASingleLine: false + +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true + +BinPackParameters: true +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true + +PenaltyExcessCharacter: 50 +PenaltyBreakBeforeFirstCallParameter: 30 +PenaltyBreakComment: 1000 +PenaltyBreakFirstLessLess: 10 +PenaltyBreakString: 100 +PenaltyReturnTypeOnItsOwnLine: 50 + +SpacesBeforeTrailingComments: 2 +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterCStyleCast: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: + AfterCaseLabel: true + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false +... diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 000000000..693d15f05 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,57 @@ +--- +# This was copied over from the moveit_studio repository. +# +# TODO(henningkayser): Re-enable performance-unnecessary-value-param once #214 is resolved +Checks: '-*, + performance-*, + -performance-unnecessary-value-param, + llvm-namespace-comment, + modernize-redundant-void-arg, + modernize-use-nullptr, + modernize-use-default, + modernize-use-override, + modernize-loop-convert, + readability-braces-around-statements, + readability-named-parameter, + readability-redundant-smartptr-get, + readability-redundant-string-cstr, + readability-simplify-boolean-expr, + readability-container-size-empty, + readability-identifier-naming, + ' +HeaderFilterRegex: '' +AnalyzeTemporaryDtors: false +CheckOptions: + - key: llvm-namespace-comment.ShortNamespaceLines + value: '10' + - key: llvm-namespace-comment.SpacesBeforeComments + value: '2' + - key: readability-braces-around-statements.ShortStatementLines + value: '2' + # type names + - key: readability-identifier-naming.ClassCase + value: CamelCase + - key: readability-identifier-naming.EnumCase + value: CamelCase + - key: readability-identifier-naming.EnumConstantCase + value: CamelCase + - key: readability-identifier-naming.UnionCase + value: CamelCase + # method names + - key: readability-identifier-naming.MethodCase + value: camelBack + # variable names + - key: readability-identifier-naming.VariableCase + value: lower_case + - key: readability-identifier-naming.ClassMemberSuffix + value: '_' + # const static, constexpr or global variables are camelBack (we write them as kMyConstant) + - key: readability-identifier-naming.ConstexprVariableCase + value: camelBack + - key: readability-identifier-naming.StaticConstantCase + value: camelBack + - key: readability-identifier-naming.ClassConstantCase + value: camelBack + - key: readability-identifier-naming.GlobalVariableCase + value: camelBack +... diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml new file mode 100644 index 000000000..03bdea8c8 --- /dev/null +++ b/.github/workflows/ci.yaml @@ -0,0 +1,72 @@ +name: CI + +on: + # Run if someone manually presses the button in the GitHub Actions UI + workflow_dispatch: + # Run when a PR is opened or updated + pull_request: + # Run when a commit is pushed to main + push: + branches: + - main + +permissions: + # Allow reading the source code + contents: read + # Allow writing built containers to GitHub Package Registry + packages: write + +jobs: + build-ws: + name: Build colcon workspace + runs-on: ubuntu-24.04 + steps: + - name: Checkout source + uses: actions/checkout@v4 + + - name: Setup Docker Buildx + uses: docker/setup-buildx-action@v2 + + # Log into GitHub Container Registry so we can push an image + - name: Log in to GHCR + uses: docker/login-action@v2 + with: + registry: ghcr.io + username: ${{ github.repository_owner }} + password: ${{ secrets.GITHUB_TOKEN }} + + # Build the Dockerfile and push the image to the private GitHub Container Registry on this repo + - name: Build workspace + uses: docker/build-push-action@v4 + with: + context: . + # run_id is unique to a particular run of this workflow so shouldn't clobber + tags: ghcr.io/picknikrobotics/fuse:${{ github.run_id }} + push: true + # This project is small enough that caching to GitHub Actions should be fine (it has a 10GB cache limit) + cache-to: type=gha,mode=max + cache-from: type=gha + + test-ws: + name: Test colcon workspace + needs: + # Ensure the test job runs after the build job finishes instead of attempting to run in parallel + - build-ws + runs-on: ubuntu-24.04 + container: + # Run on the Docker image we tagged and pushed to a private repo in the job above + image: ghcr.io/picknikrobotics/fuse:${{ github.run_id }} + steps: + - name: Unit test workspace + run: | + . /opt/ros/rolling/setup.sh + . /colcon_ws/install/local_setup.sh + colcon test --event-handlers console_direct+ --retest-until-pass 3 + working-directory: /colcon_ws + + # `colcon test` does not actually error on failure - run `colcon test-result` to generate a summary and an error code. + - name: Display colcon test results + # Run this step even if a previous step failed + if: always() + run: colcon test-result --verbose + working-directory: /colcon_ws diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml new file mode 100644 index 000000000..416508d7a --- /dev/null +++ b/.github/workflows/pre-commit.yml @@ -0,0 +1,29 @@ +# see https://github.com/pre-commit/action + +name: pre-commit + +on: + workflow_dispatch: + pull_request: + push: + branches: + - main + +jobs: + pre-commit: + name: Format + runs-on: ubuntu-24.04 + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: '3.10' + - name: Install clang-format-18 + run: sudo apt-get install clang-format-18 + - uses: pre-commit/action@v3.0.1 + id: precommit + - name: Upload pre-commit changes + if: failure() && steps.precommit.outcome == 'failure' + uses: rhaschke/upload-git-patch-action@main + with: + name: pre-commit diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml new file mode 100644 index 000000000..02f231083 --- /dev/null +++ b/.github/workflows/ros2.yml @@ -0,0 +1,15 @@ +name: ros2 + +on: [push, pull_request] + +jobs: + industrial_ci: + strategy: + matrix: + env: + - {ROS_DISTRO: rolling, ROS_REPO: main} + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v1 + - uses: 'ros-industrial/industrial_ci@master' + env: ${{matrix.env}} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 000000000..1b5c22bcb --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,83 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v3.4.0 + hooks: + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-yaml + args: ["--unsafe"] # Fixes errors parsing custom YAML constructors like ur_description's !degrees + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: fix-byte-order-marker + + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.3.0 + hooks: + - id: ruff-format + + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.6 + hooks: + - id: clang-format + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|m|proto|vert)$ + # -i arg is included by default by the hook + args: ["-fallback-style=none"] + + - repo: https://github.com/adrienverge/yamllint + rev: v1.27.1 + hooks: + - id: yamllint + args: + [ + "--no-warnings", + "--config-data", + "{extends: default, rules: {line-length: disable, braces: {max-spaces-inside: 1}}}", + ] + types: [text] + files: \.(yml|yaml)$ + + - repo: https://github.com/tcort/markdown-link-check + rev: v3.12.2 + hooks: + - id: markdown-link-check + args: + - "-c" + - "markdown-link-check-config.json" + + - repo: https://github.com/cheshirekow/cmake-format-precommit + rev: v0.6.10 + hooks: + - id: cmake-format + - id: cmake-lint + args: + - "--disabled-codes=C0301" # Disable Line too long lint + - "--suppress-decorations" + + - repo: https://github.com/pre-commit/mirrors-prettier + rev: "v3.1.0" + hooks: + # Use Prettier to lint XML files because, well.. its rules are prettier than most linters, as the name implies. + # Also we use it in the UI, so it's familiar. + - id: prettier + additional_dependencies: + - "prettier@3.1.0" + - "@prettier/plugin-xml@3.3.1" + files: \.(xml|xacro)$ diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 000000000..d82059ae6 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,38 @@ +# Docker setup that's used for CI. + +FROM osrf/ros:rolling-desktop-full + +# Install external packages. +# hadolint ignore=DL3008 +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + apt-get update && apt-get upgrade -y && \ + apt-get install -y --no-install-recommends \ + clang-tidy \ + # use cyclonedds instead of fastdds + ros-rolling-rmw-cyclonedds-cpp + +# Create the colcon ws. For now, copy the source files into the workspace +# so that we don't have to deal with cloning this repo, which is private. +WORKDIR /colcon_ws/src/fuse +COPY . . +WORKDIR /colcon_ws +# hadolint ignore=SC1091 +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + apt-get update && apt-get upgrade -y && \ + . /opt/ros/rolling/setup.sh && \ + rosdep install --from-paths src -y --ignore-src && \ + colcon build --mixin compile-commands coverage-gcc coverage-pytest + +# Set up final environment and entrypoint. +ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp +ENV CYCLONEDDS_URI /root/.ros/cyclonedds.xml +COPY dds/cyclonedds_local.xml $CYCLONEDDS_URI +COPY .clang-tidy /colcon_ws +COPY entrypoint.sh / +ENTRYPOINT [ "/entrypoint.sh" ] +RUN echo "source /entrypoint.sh" >> ~/.bashrc + +ENV SHELL /bin/bash +CMD ["/bin/bash"] diff --git a/Jenkinsfile b/Jenkinsfile index 8080c5a0c..06c18cd4a 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -17,4 +17,4 @@ tailorTestPipeline( source_branch: 'rolling', // Docker registry where test image is stored docker_registry: 'https://084758475884.dkr.ecr.us-east-1.amazonaws.com/locus-tailor' -) \ No newline at end of file +) diff --git a/dds/cyclonedds_local.xml b/dds/cyclonedds_local.xml new file mode 100644 index 000000000..a46ad402c --- /dev/null +++ b/dds/cyclonedds_local.xml @@ -0,0 +1,22 @@ + + + + + + + + false + + + auto + + + + 120 + + + diff --git a/doc/Variables.md b/doc/Variables.md index 4e41fcbf1..d6c067b88 100644 --- a/doc/Variables.md +++ b/doc/Variables.md @@ -47,8 +47,6 @@ places where most of the dimensions are unused. However, including too few physi also leads to inefficient and cumbersome usage when even the simplest of observation models involve many variables. This is one of those "Goldilocks principle" situations. -![Goldilocks principle](http://home.netcom.com/~swansont_2/goldilocks.jpg) - Understanding how Variable interact with the rest of the system will help in the design of "good" Variable. * The fuse stack is designed to combine observations _of the same variable identity_ from multiple sources. As diff --git a/entrypoint.sh b/entrypoint.sh new file mode 100755 index 000000000..992ad8540 --- /dev/null +++ b/entrypoint.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +source /opt/ros/rolling/setup.bash + +if [ -f /colcon_ws/install/local_setup.bash ] +then + source /colcon_ws/install/local_setup.bash +fi + +exec "$@" diff --git a/fuse/README.md b/fuse/README.md index b3c5d812d..0f1c6263b 100644 --- a/fuse/README.md +++ b/fuse/README.md @@ -1,2 +1,2 @@ # fuse -This is the metapackage for the entire fuse stack. \ No newline at end of file +This is the metapackage for the entire fuse stack. diff --git a/fuse_constraints/CHANGELOG.rst b/fuse_constraints/CHANGELOG.rst index 849586087..2e20464de 100644 --- a/fuse_constraints/CHANGELOG.rst +++ b/fuse_constraints/CHANGELOG.rst @@ -7,7 +7,7 @@ Changelog for package fuse_constraints 1.1.1 (2024-05-02) ------------------ -* Required formatting changes for the lastest version of ROS 2 Rolling (`#368 `_) +* Required formatting changes for the latest version of ROS 2 Rolling (`#368 `_) * Contributors: Stephen Williams 1.1.0 (2024-04-20) diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 8640fe075..06091a383 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -26,53 +26,104 @@ find_package(glog REQUIRED) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake) include(suitesparse-extras.cmake) -########### -## Build ## -########### -# lint_cmake: -linelength -# Disable warnings about array bounds with -Wno-array-bounds until gcc 12 fixes this bug: +# ############################################################################## +# Build ## +# ############################################################################## +# lint_cmake: -linelength Disable warnings about array bounds with +# -Wno-array-bounds until gcc 12 fixes this bug: # https://gcc.gnu.org/bugzilla/show_bug.cgi?id=106247 # # Also reported in Eigen, and confirmed to be due to gcc 12: # https://gitlab.com/libeigen/eigen/-/issues/2506 # -# In file included from include/immintrin.h:43, -# from include/eigen3/Eigen/src/Core/util/ConfigureVectorization.h:361, -# from include/eigen3/Eigen/Core:22, -# 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_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h:37, -# from src/fuse/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp:34: -# In function ‘__m256d _mm256_loadu_pd(const double*)’, -# inlined from ‘Packet Eigen::internal::ploadu(const typename unpacket_traits::type*) [with Packet = __vector(4) double]’ at include/eigen3/Eigen/src/Core/arch/AVX/PacketMath.h:582:129, -# inlined from ‘Packet Eigen::internal::ploadt(const typename unpacket_traits::type*) [with Packet = __vector(4) double; int Alignment = 0]’ at include/eigen3/Eigen/src/Core/GenericPacketMath.h:969:26, -# inlined from ‘PacketType Eigen::internal::evaluator >::packet(Eigen::Index) const [with int LoadMode = 0; PacketType = __vector(4) double; Derived = Eigen::Matrix]’ at include/eigen3/Eigen/src/Core/CoreEvaluators.h:245:40, -# inlined from ‘void Eigen::internal::generic_dense_assignment_kernel::assignPacket(Eigen::Index) [with int StoreMode = 32; int LoadMode = 0; PacketType = __vector(4) double; DstEvaluatorTypeT = Eigen::internal::evaluator > >; SrcEvaluatorTypeT = Eigen::internal::evaluator >; Functor = Eigen::internal::assign_op; int Version = 0]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:681:114, -# inlined from ‘static void Eigen::internal::dense_assignment_loop::run(Kernel&) [with Kernel = Eigen::internal::generic_dense_assignment_kernel > >, Eigen::internal::evaluator >, Eigen::internal::assign_op, 0>]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:437:75, -# inlined from ‘void Eigen::internal::call_dense_assignment_loop(DstXprType&, const SrcXprType&, const Functor&) [with DstXprType = Eigen::Map >; SrcXprType = Eigen::Matrix; Functor = assign_op]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:785:37, -# inlined from ‘static void Eigen::internal::Assignment::run(DstXprType&, const SrcXprType&, const Functor&) [with DstXprType = Eigen::Map >; SrcXprType = Eigen::Matrix; Functor = Eigen::internal::assign_op; Weak = void]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:954:31, -# inlined from ‘void Eigen::internal::call_assignment_no_alias(Dst&, const Src&, const Func&) [with Dst = Eigen::Map >; Src = Eigen::Matrix; Func = assign_op]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:890:49, -# inlined from ‘void Eigen::internal::call_assignment(Dst&, const Src&, const Func&, typename enable_if::value, void*>::type) [with Dst = Eigen::Map >; Src = Eigen::Product, Eigen::Map >, 0>; Func = assign_op]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:851:27, -# inlined from ‘void Eigen::internal::call_assignment(Dst&, const Src&) [with Dst = Eigen::Map >; Src = Eigen::Product, Eigen::Map >, 0>]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:836:18, -# inlined from ‘Derived& Eigen::MatrixBase::operator=(const Eigen::DenseBase&) [with OtherDerived = Eigen::Product, Eigen::Map >, 0>; Derived = Eigen::Map >]’ at include/eigen3/Eigen/src/Core/Assign.h:66:28, -# inlined from ‘void Eigen::EigenBase::applyThisOnTheLeft(Dest&) const [with Dest = Eigen::Map >; Derived = Eigen::Matrix]’ at include/eigen3/Eigen/src/Core/EigenBase.h:114:9: -# include/avxintrin.h:893:24: error: array subscript ‘__m256d_u[0]’ is partly outside array bounds of ‘Eigen::internal::plain_matrix_type, Eigen::Map >, 0>, Eigen::Dense>::type [1]’ {aka ‘Eigen::Matrix [1]’} [-Werror=array-bounds] -# 893 | return *(__m256d_u *)__P; -# | ^~~ -# In file included from include/eigen3/Eigen/Core:278: -# include/eigen3/Eigen/src/Core/AssignEvaluator.h: In member function ‘void Eigen::EigenBase::applyThisOnTheLeft(Dest&) const [with Dest = Eigen::Map >; Derived = Eigen::Matrix]’: -# include/eigen3/Eigen/src/Core/AssignEvaluator.h:850:41: note: at offset [0, 16] into object ‘tmp’ of size 24 -# 850 | typename plain_matrix_type::type tmp(src); -# | ^~~ -if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 12.0) +# In file included from include/immintrin.h:43, from +# include/eigen3/Eigen/src/Core/util/ConfigureVectorization.h:361, from +# include/eigen3/Eigen/Core:22, 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_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h:37, +# from +# src/fuse/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp:34: +# In function ‘__m256d _mm256_loadu_pd(const double*)’, inlined from ‘Packet +# Eigen::internal::ploadu(const typename unpacket_traits::type*) [with Packet +# = __vector(4) double]’ at +# include/eigen3/Eigen/src/Core/arch/AVX/PacketMath.h:582:129, inlined from +# ‘Packet Eigen::internal::ploadt(const typename unpacket_traits::type*) +# [with Packet = __vector(4) double; int Alignment = 0]’ at +# include/eigen3/Eigen/src/Core/GenericPacketMath.h:969:26, inlined from +# ‘PacketType Eigen::internal::evaluator +# >::packet(Eigen::Index) const [with int LoadMode = 0; PacketType = __vector(4) +# double; Derived = Eigen::Matrix]’ at +# include/eigen3/Eigen/src/Core/CoreEvaluators.h:245:40, inlined from ‘void +# Eigen::internal::generic_dense_assignment_kernel::assignPacket(Eigen::Index) [with int +# StoreMode = 32; int LoadMode = 0; PacketType = __vector(4) double; +# DstEvaluatorTypeT = +# Eigen::internal::evaluator > >; +# SrcEvaluatorTypeT = Eigen::internal::evaluator >; +# Functor = Eigen::internal::assign_op; int Version = 0]’ at +# include/eigen3/Eigen/src/Core/AssignEvaluator.h:681:114, inlined from ‘static +# void Eigen::internal::dense_assignment_loop::run(Kernel&) [with +# Kernel = +# Eigen::internal::generic_dense_assignment_kernel > >, Eigen::internal::evaluator >, +# Eigen::internal::assign_op, 0>]’ at +# include/eigen3/Eigen/src/Core/AssignEvaluator.h:437:75, inlined from ‘void +# Eigen::internal::call_dense_assignment_loop(DstXprType&, const SrcXprType&, +# const Functor&) [with DstXprType = Eigen::Map >; +# SrcXprType = Eigen::Matrix; Functor = assign_op]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:785:37, inlined +# from ‘static void Eigen::internal::Assignment::run(DstXprType&, const SrcXprType&, const +# Functor&) [with DstXprType = Eigen::Map >; +# SrcXprType = Eigen::Matrix; Functor = +# Eigen::internal::assign_op; Weak = void]’ at +# include/eigen3/Eigen/src/Core/AssignEvaluator.h:954:31, inlined from ‘void +# Eigen::internal::call_assignment_no_alias(Dst&, const Src&, const Func&) [with +# Dst = Eigen::Map >; Src = Eigen::Matrix; Func = assign_op]’ at +# include/eigen3/Eigen/src/Core/AssignEvaluator.h:890:49, inlined from ‘void +# Eigen::internal::call_assignment(Dst&, const Src&, const Func&, typename +# enable_if::value, void*>::type) [with Dst = +# Eigen::Map >; Src = +# Eigen::Product, +# Eigen::Map >, 0>; Func = assign_op]’ at include/eigen3/Eigen/src/Core/AssignEvaluator.h:851:27, inlined +# from ‘void Eigen::internal::call_assignment(Dst&, const Src&) [with Dst = +# Eigen::Map >; Src = +# Eigen::Product, +# Eigen::Map >, 0>]’ at +# include/eigen3/Eigen/src/Core/AssignEvaluator.h:836:18, inlined from ‘Derived& +# Eigen::MatrixBase::operator=(const Eigen::DenseBase&) +# [with OtherDerived = Eigen::Product, +# Eigen::Map >, 0>; Derived = +# Eigen::Map >]’ at +# include/eigen3/Eigen/src/Core/Assign.h:66:28, inlined from ‘void +# Eigen::EigenBase::applyThisOnTheLeft(Dest&) const [with Dest = +# Eigen::Map >; Derived = Eigen::Matrix]’ at include/eigen3/Eigen/src/Core/EigenBase.h:114:9: +# include/avxintrin.h:893:24: error: array subscript ‘__m256d_u[0]’ is partly +# outside array bounds of +# ‘Eigen::internal::plain_matrix_type, Eigen::Map >, 0>, Eigen::Dense>::type [1]’ +# {aka ‘Eigen::Matrix [1]’} [-Werror=array-bounds] 893 | return +# *(__m256d_u *)__P; | ^~~ In file included from +# include/eigen3/Eigen/Core:278: +# include/eigen3/Eigen/src/Core/AssignEvaluator.h: In member function ‘void +# Eigen::EigenBase::applyThisOnTheLeft(Dest&) const [with Dest = +# Eigen::Map >; Derived = Eigen::Matrix]’: include/eigen3/Eigen/src/Core/AssignEvaluator.h:850:41: note: at +# offset [0, 16] into object ‘tmp’ of size 24 850 | typename +# 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) else() add_compile_options(-Wall -Werror) endif() # fuse_constraints library -add_library(${PROJECT_NAME} +add_library( + ${PROJECT_NAME} src/absolute_constraint.cpp src/absolute_orientation_3d_stamped_constraint.cpp src/absolute_orientation_3d_stamped_euler_constraint.cpp @@ -91,60 +142,48 @@ add_library(${PROJECT_NAME} src/relative_pose_2d_stamped_constraint.cpp src/relative_pose_3d_stamped_constraint.cpp src/uuid_ordering.cpp - src/variable_constraints.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$" -) -target_link_libraries(${PROJECT_NAME} PUBLIC - Boost::serialization - Ceres::ceres - Eigen3::Eigen - fuse_core::fuse_core - fuse_graphs::fuse_graphs - fuse_variables::fuse_variables - ${geometry_msgs_TARGETS} - pluginlib::pluginlib - rclcpp::rclcpp -) -target_link_libraries(${PROJECT_NAME} PRIVATE - ${SUITESPARSE_LIBRARIES} -) -target_include_directories(${PROJECT_NAME} PRIVATE - ${SUITESPARSE_INCLUDE_DIRS} -) - -############# -## Testing ## -############# + src/variable_constraints.cpp) +target_include_directories( + ${PROJECT_NAME} + PUBLIC "$" + "$") +target_link_libraries( + ${PROJECT_NAME} + PUBLIC Boost::serialization + Ceres::ceres + Eigen3::Eigen + fuse_core::fuse_core + fuse_graphs::fuse_graphs + fuse_variables::fuse_variables + ${geometry_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp) +target_link_libraries(${PROJECT_NAME} PRIVATE ${SUITESPARSE_LIBRARIES}) +target_include_directories(${PROJECT_NAME} PRIVATE ${SUITESPARSE_INCLUDE_DIRS}) + +# ############################################################################## +# Testing ## +# ############################################################################## if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - add_subdirectory(test) add_subdirectory(benchmark) endif() -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## -install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-export ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) -install( - DIRECTORY cmake - DESTINATION share/${PROJECT_NAME} -) +install(DIRECTORY cmake DESTINATION share/${PROJECT_NAME}) pluginlib_export_plugin_description_file(fuse_core fuse_plugins.xml) @@ -159,7 +198,6 @@ ament_export_dependencies( rclcpp Ceres Eigen3 - glog -) + glog) ament_package(CONFIG_EXTRAS suitesparse-extras.cmake) diff --git a/fuse_constraints/benchmark/CMakeLists.txt b/fuse_constraints/benchmark/CMakeLists.txt index f7aa03cdd..9a4cd8661 100644 --- a/fuse_constraints/benchmark/CMakeLists.txt +++ b/fuse_constraints/benchmark/CMakeLists.txt @@ -2,20 +2,18 @@ find_package(benchmark QUIET) if(benchmark_FOUND) # Normal Delta Pose 2D benchmark - add_executable(benchmark_normal_delta_pose_2d benchmark_normal_delta_pose_2d.cpp) + add_executable(benchmark_normal_delta_pose_2d + benchmark_normal_delta_pose_2d.cpp) if(TARGET benchmark_normal_delta_pose_2d) - target_link_libraries(benchmark_normal_delta_pose_2d - benchmark::benchmark - ${PROJECT_NAME} - ) + target_link_libraries(benchmark_normal_delta_pose_2d benchmark::benchmark + ${PROJECT_NAME}) endif() # Normal Prior Pose 2D benchmark - add_executable(benchmark_normal_prior_pose_2d benchmark_normal_prior_pose_2d.cpp) + add_executable(benchmark_normal_prior_pose_2d + benchmark_normal_prior_pose_2d.cpp) if(TARGET benchmark_normal_prior_pose_2d) - target_link_libraries(benchmark_normal_prior_pose_2d - benchmark::benchmark - ${PROJECT_NAME} - ) + target_link_libraries(benchmark_normal_prior_pose_2d benchmark::benchmark + ${PROJECT_NAME}) endif() endif() diff --git a/fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp b/fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp index 14c1b7d2d..7e489ef1b 100644 --- a/fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp +++ b/fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp @@ -43,11 +43,10 @@ class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture { public: - NormalDeltaPose2DBenchmarkFixture() - : jacobians(num_parameter_blocks) - , J(num_parameter_blocks) + NormalDeltaPose2DBenchmarkFixture() : jacobians(num_parameter_blocks), J(num_parameter_blocks) { - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } @@ -58,18 +57,18 @@ class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture static const fuse_core::Matrix3d sqrt_information; // Parameters - static const double * parameters[]; + static const double* parameters[]; // Residuals fuse_core::Vector3d residuals; - static const std::vector & block_sizes; + static const std::vector& block_sizes; static const size_t num_parameter_blocks; static const size_t num_residuals; // Jacobians - std::vector jacobians; + std::vector jacobians; private: // Cost function covariance @@ -88,69 +87,56 @@ class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture }; // Cost function covariance -const double NormalDeltaPose2DBenchmarkFixture::covariance_diagonal[] = {2e-3, 1e-3, 1e-2}; +const double NormalDeltaPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 }; const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::covariance = - fuse_core::Vector3d(covariance_diagonal).asDiagonal(); + fuse_core::Vector3d(covariance_diagonal).asDiagonal(); // Parameter blocks -const double NormalDeltaPose2DBenchmarkFixture::position1[] = {0.0, 1.0}; -const double NormalDeltaPose2DBenchmarkFixture::orientation1[] = {0.5}; -const double NormalDeltaPose2DBenchmarkFixture::position2[] = {2.0, 3.0}; -const double NormalDeltaPose2DBenchmarkFixture::orientation2[] = {1.5}; +const double NormalDeltaPose2DBenchmarkFixture::position1[] = { 0.0, 1.0 }; +const double NormalDeltaPose2DBenchmarkFixture::orientation1[] = { 0.5 }; +const double NormalDeltaPose2DBenchmarkFixture::position2[] = { 2.0, 3.0 }; +const double NormalDeltaPose2DBenchmarkFixture::orientation2[] = { 1.5 }; // Delta and sqrt information matrix -const fuse_core::Vector3d NormalDeltaPose2DBenchmarkFixture::delta{1.0, 2.0, 3.0}; -const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::sqrt_information(covariance.inverse(). - llt().matrixU()); +const fuse_core::Vector3d NormalDeltaPose2DBenchmarkFixture::delta{ 1.0, 2.0, 3.0 }; +const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::sqrt_information(covariance.inverse().llt().matrixU()); // Parameters -const double * NormalDeltaPose2DBenchmarkFixture::parameters[] = -{position1, orientation1, position2, orientation2}; +const double* NormalDeltaPose2DBenchmarkFixture::parameters[] = { position1, orientation1, position2, orientation2 }; -const std::vector & NormalDeltaPose2DBenchmarkFixture::block_sizes = {2, 1, 2, 1}; +const std::vector& NormalDeltaPose2DBenchmarkFixture::block_sizes = { 2, 1, 2, 1 }; const size_t NormalDeltaPose2DBenchmarkFixture::num_parameter_blocks = block_sizes.size(); const size_t NormalDeltaPose2DBenchmarkFixture::num_residuals = 3; -BENCHMARK_DEFINE_F( - NormalDeltaPose2DBenchmarkFixture, - AnalyticNormalDeltaPose2D)(benchmark::State & state) +BENCHMARK_DEFINE_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2D)(benchmark::State& state) { // Create analytic cost function - const fuse_constraints::NormalDeltaPose2D cost_function{sqrt_information.topRows(state.range(0)), - delta}; + const fuse_constraints::NormalDeltaPose2D cost_function{ sqrt_information.topRows(state.range(0)), delta }; - for (auto _ : state) { + for (auto _ : state) + { cost_function.Evaluate(parameters, residuals.data(), jacobians.data()); } } -BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2D)->DenseRange( - 1, 3); +BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2D)->DenseRange(1, 3); -BENCHMARK_DEFINE_F( - NormalDeltaPose2DBenchmarkFixture, - AutoDiffNormalDeltaPose2D)(benchmark::State & state) +BENCHMARK_DEFINE_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)(benchmark::State& state) { // Create cost function using automatic differentiation on the cost functor const auto partial_sqrt_information = sqrt_information.topRows(state.range(0)); - const ceres::AutoDiffCostFunction< - fuse_constraints::NormalDeltaPose2DCostFunctor, - ceres::DYNAMIC, - 2, 1, 2, 1 - > - cost_function_autodiff(new fuse_constraints::NormalDeltaPose2DCostFunctor( - partial_sqrt_information, - delta), - partial_sqrt_information.rows()); - - for (auto _ : state) { + const ceres::AutoDiffCostFunction + cost_function_autodiff(new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, delta), + partial_sqrt_information.rows()); + + for (auto _ : state) + { cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians.data()); } } -BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)->DenseRange( - 1, 3); +BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)->DenseRange(1, 3); BENCHMARK_MAIN(); diff --git a/fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp b/fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp index f1808f942..c550a82cf 100644 --- a/fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp +++ b/fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp @@ -43,11 +43,10 @@ class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture { public: - NormalPriorPose2DBenchmarkFixture() - : jacobians(num_parameter_blocks) - , J(num_parameter_blocks) + NormalPriorPose2DBenchmarkFixture() : jacobians(num_parameter_blocks), J(num_parameter_blocks) { - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } @@ -58,18 +57,18 @@ class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture static const fuse_core::Matrix3d sqrt_information; // Parameters - static const double * parameters[]; + static const double* parameters[]; // Residuals fuse_core::Vector3d residuals; - static const std::vector & block_sizes; + static const std::vector& block_sizes; static const size_t num_parameter_blocks; static const size_t num_residuals; // Jacobians - std::vector jacobians; + std::vector jacobians; private: // Cost function covariance @@ -86,66 +85,54 @@ class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture }; // Cost function covariance -const double NormalPriorPose2DBenchmarkFixture::covariance_diagonal[] = {2e-3, 1e-3, 1e-2}; +const double NormalPriorPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 }; const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::covariance = - fuse_core::Vector3d(covariance_diagonal).asDiagonal(); + fuse_core::Vector3d(covariance_diagonal).asDiagonal(); // Parameter blocks -const double NormalPriorPose2DBenchmarkFixture::position[] = {0.0, 0.0}; -const double NormalPriorPose2DBenchmarkFixture::orientation[] = {0.0}; +const double NormalPriorPose2DBenchmarkFixture::position[] = { 0.0, 0.0 }; +const double NormalPriorPose2DBenchmarkFixture::orientation[] = { 0.0 }; // Mean and sqrt information matrix -const fuse_core::Vector3d NormalPriorPose2DBenchmarkFixture::mean{1.0, 2.0, 3.0}; -const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::sqrt_information(covariance.inverse(). - llt().matrixU()); +const fuse_core::Vector3d NormalPriorPose2DBenchmarkFixture::mean{ 1.0, 2.0, 3.0 }; +const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::sqrt_information(covariance.inverse().llt().matrixU()); // Parameters -const double * NormalPriorPose2DBenchmarkFixture::parameters[] = {position, orientation}; +const double* NormalPriorPose2DBenchmarkFixture::parameters[] = { position, orientation }; -const std::vector & NormalPriorPose2DBenchmarkFixture::block_sizes = {2, 1}; +const std::vector& NormalPriorPose2DBenchmarkFixture::block_sizes = { 2, 1 }; const size_t NormalPriorPose2DBenchmarkFixture::num_parameter_blocks = block_sizes.size(); const size_t NormalPriorPose2DBenchmarkFixture::num_residuals = 3; -BENCHMARK_DEFINE_F( - NormalPriorPose2DBenchmarkFixture, - AnalyticNormalPriorPose2D)(benchmark::State & state) +BENCHMARK_DEFINE_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2D)(benchmark::State& state) { // Create analytic cost function - const fuse_constraints::NormalPriorPose2D cost_function{sqrt_information.topRows(state.range(0)), - mean}; + const fuse_constraints::NormalPriorPose2D cost_function{ sqrt_information.topRows(state.range(0)), mean }; - for (auto _ : state) { + for (auto _ : state) + { cost_function.Evaluate(parameters, residuals.data(), jacobians.data()); } } -BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2D)->DenseRange( - 1, 3); +BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2D)->DenseRange(1, 3); -BENCHMARK_DEFINE_F( - NormalPriorPose2DBenchmarkFixture, - AutoDiffNormalPriorPose2D)(benchmark::State & state) +BENCHMARK_DEFINE_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)(benchmark::State& state) { // Create cost function using automatic differentiation on the cost functor const auto partial_sqrt_information = sqrt_information.topRows(state.range(0)); - const ceres::AutoDiffCostFunction< - fuse_constraints::NormalPriorPose2DCostFunctor, - ceres::DYNAMIC, - 2, 1 - > - cost_function_autodiff(new fuse_constraints::NormalPriorPose2DCostFunctor( - partial_sqrt_information, - mean), - partial_sqrt_information.rows()); - - for (auto _ : state) { + const ceres::AutoDiffCostFunction + cost_function_autodiff(new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, mean), + partial_sqrt_information.rows()); + + for (auto _ : state) + { cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians.data()); } } -BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)->DenseRange( - 1, 3); +BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)->DenseRange(1, 3); BENCHMARK_MAIN(); diff --git a/fuse_constraints/cmake/FindSUITESPARSE.cmake b/fuse_constraints/cmake/FindSUITESPARSE.cmake index d6d8ae456..db71d1431 100644 --- a/fuse_constraints/cmake/FindSUITESPARSE.cmake +++ b/fuse_constraints/cmake/FindSUITESPARSE.cmake @@ -1,30 +1,28 @@ -# Ceres Solver - A fast non-linear least squares minimizer -# Copyright 2015 Google Inc. All rights reserved. -# http://ceres-solver.org/ +# Ceres Solver - A fast non-linear least squares minimizer Copyright 2015 Google +# Inc. All rights reserved. http://ceres-solver.org/ # # 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. +# * 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. +# 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. # # Author: alexs.mac@gmail.com (Alex Stewart) # @@ -36,78 +34,60 @@ # SUITESPARSE_FOUND: TRUE iff SuiteSparse and all dependencies have been found. # SUITESPARSE_INCLUDE_DIRS: Include directories for all SuiteSparse components. # SUITESPARSE_LIBRARIES: Libraries for all SuiteSparse component libraries and -# dependencies. -# SUITESPARSE_VERSION: Extracted from UFconfig.h (<= v3) or -# SuiteSparse_config.h (>= v4). -# SUITESPARSE_MAIN_VERSION: Equal to 4 if SUITESPARSE_VERSION = 4.2.1 -# SUITESPARSE_SUB_VERSION: Equal to 2 if SUITESPARSE_VERSION = 4.2.1 -# SUITESPARSE_SUBSUB_VERSION: Equal to 1 if SUITESPARSE_VERSION = 4.2.1 +# dependencies. SUITESPARSE_VERSION: Extracted from UFconfig.h (<= v3) or +# SuiteSparse_config.h (>= v4). SUITESPARSE_MAIN_VERSION: Equal to 4 if +# SUITESPARSE_VERSION = 4.2.1 SUITESPARSE_SUB_VERSION: Equal to 2 if +# SUITESPARSE_VERSION = 4.2.1 SUITESPARSE_SUBSUB_VERSION: Equal to 1 if +# SUITESPARSE_VERSION = 4.2.1 # # SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION: TRUE iff running -# on Ubuntu, SUITESPARSE_VERSION is 3.4.0 and found SuiteSparse is a system -# install, in which case found version of SuiteSparse cannot be used to link -# a shared library due to a bug (static linking is unaffected). +# on Ubuntu, SUITESPARSE_VERSION is 3.4.0 and found SuiteSparse is a system +# install, in which case found version of SuiteSparse cannot be used to link a +# shared library due to a bug (static linking is unaffected). # # The following variables control the behaviour of this module: # # SUITESPARSE_INCLUDE_DIR_HINTS: List of additional directories in which to -# search for SuiteSparse includes, -# e.g: /timbuktu/include. +# search for SuiteSparse includes, e.g: /timbuktu/include. # SUITESPARSE_LIBRARY_DIR_HINTS: List of additional directories in which to -# search for SuiteSparse libraries, -# e.g: /timbuktu/lib. +# search for SuiteSparse libraries, e.g: /timbuktu/lib. # # The following variables define the presence / includes & libraries for the # SuiteSparse components searched for, the SUITESPARSE_XX variables are the # union of the variables for all components. # -# == Symmetric Approximate Minimum Degree (AMD) -# AMD_FOUND -# AMD_INCLUDE_DIR +# == Symmetric Approximate Minimum Degree (AMD) AMD_FOUND AMD_INCLUDE_DIR # AMD_LIBRARY # -# == Constrained Approximate Minimum Degree (CAMD) -# CAMD_FOUND -# CAMD_INCLUDE_DIR +# == Constrained Approximate Minimum Degree (CAMD) CAMD_FOUND CAMD_INCLUDE_DIR # CAMD_LIBRARY # -# == Column Approximate Minimum Degree (COLAMD) -# COLAMD_FOUND -# COLAMD_INCLUDE_DIR +# == Column Approximate Minimum Degree (COLAMD) COLAMD_FOUND COLAMD_INCLUDE_DIR # COLAMD_LIBRARY # -# Constrained Column Approximate Minimum Degree (CCOLAMD) -# CCOLAMD_FOUND -# CCOLAMD_INCLUDE_DIR -# CCOLAMD_LIBRARY +# Constrained Column Approximate Minimum Degree (CCOLAMD) CCOLAMD_FOUND +# CCOLAMD_INCLUDE_DIR CCOLAMD_LIBRARY # # == Sparse Supernodal Cholesky Factorization and Update/Downdate (CHOLMOD) -# CHOLMOD_FOUND -# CHOLMOD_INCLUDE_DIR -# CHOLMOD_LIBRARY +# CHOLMOD_FOUND CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARY # -# == Multifrontal Sparse QR (SuiteSparseQR) -# SUITESPARSEQR_FOUND -# SUITESPARSEQR_INCLUDE_DIR -# SUITESPARSEQR_LIBRARY +# == Multifrontal Sparse QR (SuiteSparseQR) SUITESPARSEQR_FOUND +# SUITESPARSEQR_INCLUDE_DIR SUITESPARSEQR_LIBRARY # # == Common configuration for all but CSparse (SuiteSparse version >= 4). -# SUITESPARSE_CONFIG_FOUND -# SUITESPARSE_CONFIG_INCLUDE_DIR +# SUITESPARSE_CONFIG_FOUND SUITESPARSE_CONFIG_INCLUDE_DIR # SUITESPARSE_CONFIG_LIBRARY # # == Common configuration for all but CSparse (SuiteSparse version < 4). -# UFCONFIG_FOUND -# UFCONFIG_INCLUDE_DIR +# UFCONFIG_FOUND UFCONFIG_INCLUDE_DIR # # Optional SuiteSparse Dependencies: # # == Serial Graph Partitioning and Fill-reducing Matrix Ordering (METIS) -# METIS_FOUND -# METIS_LIBRARY +# METIS_FOUND METIS_LIBRARY -# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when -# FindSuiteSparse was invoked. +# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when FindSuiteSparse +# was invoked. macro(SUITESPARSE_RESET_FIND_LIBRARY_PREFIX) if(MSVC) set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") @@ -115,9 +95,9 @@ macro(SUITESPARSE_RESET_FIND_LIBRARY_PREFIX) endmacro() # Called if we failed to find SuiteSparse or any of it's required dependencies, -# unsets all public (designed to be used externally) variables and reports -# error message at priority depending upon [REQUIRED/QUIET/] argument. -macro(SUITESPARSE_REPORT_NOT_FOUND REASON_MSG) +# unsets all public (designed to be used externally) variables and reports error +# message at priority depending upon [REQUIRED/QUIET/] argument. +macro(SUITESPARSE_REPORT_NOT_FOUND reason_msg) unset(SUITESPARSE_FOUND) unset(SUITESPARSE_INCLUDE_DIRS) unset(SUITESPARSE_LIBRARIES) @@ -134,18 +114,18 @@ macro(SUITESPARSE_REPORT_NOT_FOUND REASON_MSG) # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() # use the camelcase library name, not uppercase. if(SuiteSparse_FIND_QUIETLY) - message(STATUS "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + message(STATUS "Failed to find SuiteSparse - " ${reason_msg} ${ARGN}) elseif(SuiteSparse_FIND_REQUIRED) - message(FATAL_ERROR "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + message(FATAL_ERROR "Failed to find SuiteSparse - " ${reason_msg} ${ARGN}) else() - # Neither QUIETLY nor REQUIRED, use no priority which emits a message - # but continues configuration and allows generation. - message("-- Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + # Neither QUIETLY nor REQUIRED, use no priority which emits a message but + # continues configuration and allows generation. + message("-- Failed to find SuiteSparse - " ${reason_msg} ${ARGN}) endif() - # Do not call return(), s/t we keep processing if not called with REQUIRED - # and report all missing components, rather than bailing after failing to find - # the first. + # Do not call return(), s/t we keep processing if not called with REQUIRED and + # report all missing components, rather than bailing after failing to find the + # first. endmacro() # Protect against any alternative find_package scripts for this library having @@ -154,108 +134,115 @@ endmacro() # logic here to fail. unset(SUITESPARSE_FOUND) -# Handle possible presence of lib prefix for libraries on MSVC, see -# also SUITESPARSE_RESET_FIND_LIBRARY_PREFIX(). +# Handle possible presence of lib prefix for libraries on MSVC, see also +# SUITESPARSE_RESET_FIND_LIBRARY_PREFIX(). if(MSVC) - # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES - # s/t we can set it back before returning. + # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES s/t we + # can set it back before returning. set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") - # The empty string in this list is important, it represents the case when - # the libraries have no prefix (shared libraries / DLLs). + # The empty string in this list is important, it represents the case when the + # libraries have no prefix (shared libraries / DLLs). set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") endif() # Specify search directories for include files and libraries (this is the union # of the search directories for all OSs). Search user-specified hint -# directories first if supplied, and search user-installed locations first -# so that we prefer user installs to system installs where both exist. -list(APPEND SUITESPARSE_CHECK_INCLUDE_DIRS +# directories first if supplied, and search user-installed locations first so +# that we prefer user installs to system installs where both exist. +list( + APPEND + SUITESPARSE_CHECK_INCLUDE_DIRS /opt/local/include /opt/local/include/ufsparse # Mac OS X /usr/local/homebrew/include # Mac OS X /usr/local/include /usr/include) -list(APPEND SUITESPARSE_CHECK_LIBRARY_DIRS +list( + APPEND + SUITESPARSE_CHECK_LIBRARY_DIRS /opt/local/lib /opt/local/lib/ufsparse # Mac OS X /usr/local/homebrew/lib # Mac OS X /usr/local/lib /usr/lib) # Additional suffixes to try appending to each search path. -list(APPEND SUITESPARSE_CHECK_PATH_SUFFIXES - suitesparse) # Windows/Ubuntu +list(APPEND SUITESPARSE_CHECK_PATH_SUFFIXES suitesparse) # Windows/Ubuntu # Wrappers to find_path/library that pass the SuiteSparse search hints/paths. # -# suitesparse_find_component( [FILES name1 [name2 ...]] -# [LIBRARIES name1 [name2 ...]] -# [REQUIRED]) -macro(suitesparse_find_component COMPONENT) +# SUITESPARSE_FIND_COMPONENT( [FILES name1 [name2 ...]] [LIBRARIES +# name1 [name2 ...]] [REQUIRED]) + +# cmake-lint: disable=C0103 +macro(SUITESPARSE_FIND_COMPONENT component) include(CMakeParseArguments) set(OPTIONS REQUIRED) set(MULTI_VALUE_ARGS FILES LIBRARIES) - cmake_parse_arguments(SUITESPARSE_FIND_${COMPONENT} - "${OPTIONS}" "" "${MULTI_VALUE_ARGS}" ${ARGN}) + cmake_parse_arguments(SUITESPARSE_FIND_${component} "${OPTIONS}" "" + "${MULTI_VALUE_ARGS}" ${ARGN}) - if(SUITESPARSE_FIND_${COMPONENT}_REQUIRED) - list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS ${COMPONENT}_FOUND) + if(SUITESPARSE_FIND_${component}_REQUIRED) + list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS ${component}_FOUND) endif() - set(${COMPONENT}_FOUND TRUE) - if(SUITESPARSE_FIND_${COMPONENT}_FILES) - find_path(${COMPONENT}_INCLUDE_DIR - NAMES ${SUITESPARSE_FIND_${COMPONENT}_FILES} + set(${component}_FOUND TRUE) + if(SUITESPARSE_FIND_${component}_FILES) + find_path( + ${component}_INCLUDE_DIR + NAMES ${SUITESPARSE_FIND_${component}_FILES} HINTS ${SUITESPARSE_INCLUDE_DIR_HINTS} PATHS ${SUITESPARSE_CHECK_INCLUDE_DIRS} PATH_SUFFIXES ${SUITESPARSE_CHECK_PATH_SUFFIXES}) - if(${COMPONENT}_INCLUDE_DIR) - message(STATUS "Found ${COMPONENT} headers in: " - "${${COMPONENT}_INCLUDE_DIR}") - mark_as_advanced(${COMPONENT}_INCLUDE_DIR) + if(${component}_INCLUDE_DIR) + message(STATUS "Found ${component} headers in: " + "${${component}_INCLUDE_DIR}") + mark_as_advanced(${component}_INCLUDE_DIR) else() # Specified headers not found. - set(${COMPONENT}_FOUND FALSE) - if(SUITESPARSE_FIND_${COMPONENT}_REQUIRED) + set(${component}_FOUND FALSE) + if(SUITESPARSE_FIND_${component}_REQUIRED) suitesparse_report_not_found( - "Did not find ${COMPONENT} header (required SuiteSparse component).") + "Did not find ${component} header (required SuiteSparse component).") else() - message(STATUS "Did not find ${COMPONENT} header (optional " - "SuiteSparse component).") + message(STATUS "Did not find ${component} header (optional " + "SuiteSparse component).") # Hide optional vars from CMake GUI even if not found. - mark_as_advanced(${COMPONENT}_INCLUDE_DIR) + mark_as_advanced(${component}_INCLUDE_DIR) endif() endif() endif() - if(SUITESPARSE_FIND_${COMPONENT}_LIBRARIES) - find_library(${COMPONENT}_LIBRARY - NAMES ${SUITESPARSE_FIND_${COMPONENT}_LIBRARIES} + if(SUITESPARSE_FIND_${component}_LIBRARIES) + find_library( + ${component}_LIBRARY + NAMES ${SUITESPARSE_FIND_${component}_LIBRARIES} HINTS ${SUITESPARSE_LIBRARY_DIR_HINTS} PATHS ${SUITESPARSE_CHECK_LIBRARY_DIRS} PATH_SUFFIXES ${SUITESPARSE_CHECK_PATH_SUFFIXES}) - if(${COMPONENT}_LIBRARY) - message(STATUS "Found ${COMPONENT} library: ${${COMPONENT}_LIBRARY}") - mark_as_advanced(${COMPONENT}_LIBRARY) + if(${component}_LIBRARY) + message(STATUS "Found ${component} library: ${${component}_LIBRARY}") + mark_as_advanced(${component}_LIBRARY) else() # Specified libraries not found. - set(${COMPONENT}_FOUND FALSE) - if(SUITESPARSE_FIND_${COMPONENT}_REQUIRED) + set(${component}_FOUND FALSE) + if(SUITESPARSE_FIND_${component}_REQUIRED) suitesparse_report_not_found( - "Did not find ${COMPONENT} library (required SuiteSparse component).") + "Did not find ${component} library (required SuiteSparse component).") else() - message(STATUS "Did not find ${COMPONENT} library (optional SuiteSparse " - "dependency)") + message( + STATUS "Did not find ${component} library (optional SuiteSparse " + "dependency)") # Hide optional vars from CMake GUI even if not found. - mark_as_advanced(${COMPONENT}_LIBRARY) + mark_as_advanced(${component}_LIBRARY) endif() endif() endif() endmacro() # Given the number of components of SuiteSparse, and to ensure that the -# automatic failure message generated by FindPackageHandleStandardArgs() -# when not all required components are found is helpful, we maintain a list -# of all variables that must be defined for SuiteSparse to be considered found. +# automatic failure message generated by FindPackageHandleStandardArgs() when +# not all required components are found is helpful, we maintain a list of all +# variables that must be defined for SuiteSparse to be considered found. unset(SUITESPARSE_FOUND_REQUIRED_VARS) # BLAS. @@ -279,47 +266,51 @@ suitesparse_find_component(CAMD REQUIRED FILES camd.h LIBRARIES camd) suitesparse_find_component(COLAMD REQUIRED FILES colamd.h LIBRARIES colamd) suitesparse_find_component(CCOLAMD REQUIRED FILES ccolamd.h LIBRARIES ccolamd) suitesparse_find_component(CHOLMOD REQUIRED FILES cholmod.h LIBRARIES cholmod) -suitesparse_find_component( - SUITESPARSEQR REQUIRED FILES SuiteSparseQR.hpp LIBRARIES spqr) +suitesparse_find_component(SUITESPARSEQR REQUIRED FILES SuiteSparseQR.hpp + LIBRARIES spqr) if(SUITESPARSEQR_FOUND) - # SuiteSparseQR may be compiled with Intel Threading Building Blocks, - # we assume that if TBB is installed, SuiteSparseQR was compiled with - # support for it, this will do no harm if it wasn't. + # SuiteSparseQR may be compiled with Intel Threading Building Blocks, we + # assume that if TBB is installed, SuiteSparseQR was compiled with support for + # it, this will do no harm if it wasn't. find_package(TBB QUIET) if(TBB_FOUND) message(STATUS "Found Intel Thread Building Blocks (TBB) library " - "(${TBB_VERSION}) assuming SuiteSparseQR was compiled " - "with TBB.") - # Add the TBB libraries to the SuiteSparseQR libraries (the only - # libraries to optionally depend on TBB). + "(${TBB_VERSION}) assuming SuiteSparseQR was compiled " + "with TBB.") + # Add the TBB libraries to the SuiteSparseQR libraries (the only libraries + # to optionally depend on TBB). list(APPEND SUITESPARSEQR_LIBRARY ${TBB_LIBRARIES}) else() message(STATUS "Did not find Intel TBB library, assuming SuiteSparseQR was " - "not compiled with TBB.") + "not compiled with TBB.") endif() endif() # UFconfig / SuiteSparse_config. # -# If SuiteSparse version is >= 4 then SuiteSparse_config is required. -# For SuiteSparse 3, UFconfig.h is required. -suitesparse_find_component( - SUITESPARSE_CONFIG FILES SuiteSparse_config.h LIBRARIES suitesparseconfig) +# If SuiteSparse version is >= 4 then SuiteSparse_config is required. For +# SuiteSparse 3, UFconfig.h is required. +suitesparse_find_component(SUITESPARSE_CONFIG FILES SuiteSparse_config.h + LIBRARIES suitesparseconfig) if(SUITESPARSE_CONFIG_FOUND) # SuiteSparse_config (SuiteSparse version >= 4) requires librt library for - # timing by default when compiled on Linux or Unix, but not on OSX (which - # does not have librt). - if(CMAKE_SYSTEM_NAME MATCHES "Linux" OR UNIX AND NOT APPLE) + # timing by default when compiled on Linux or Unix, but not on OSX (which does + # not have librt). + if(CMAKE_SYSTEM_NAME MATCHES "Linux" + OR UNIX + AND NOT APPLE) suitesparse_find_component(LIBRT LIBRARIES rt) if(LIBRT_FOUND) - message(STATUS "Adding librt: ${LIBRT_LIBRARY} to " - "SuiteSparse_config libraries (required on Linux & Unix [not OSX] if " - "SuiteSparse is compiled with timing).") + message( + STATUS + "Adding librt: ${LIBRT_LIBRARY} to " + "SuiteSparse_config libraries (required on Linux & Unix [not OSX] if " + "SuiteSparse is compiled with timing).") list(APPEND SUITESPARSE_CONFIG_LIBRARY ${LIBRT_LIBRARY}) else() message(STATUS "Could not find librt, but found SuiteSparse_config, " - "assuming that SuiteSparse was compiled without timing.") + "assuming that SuiteSparse was compiled without timing.") endif() endif() else() @@ -328,16 +319,15 @@ else() suitesparse_find_component(UFCONFIG FILES UFconfig.h) endif() -if(NOT SUITESPARSE_CONFIG_FOUND AND - NOT UFCONFIG_FOUND) +if(NOT SUITESPARSE_CONFIG_FOUND AND NOT UFCONFIG_FOUND) suitesparse_report_not_found( "Failed to find either: SuiteSparse_config header & library (should be " "present in all SuiteSparse >= v4 installs), or UFconfig header (should " "be present in all SuiteSparse < v4 installs).") endif() -# Extract the SuiteSparse version from the appropriate header (UFconfig.h for -# <= v3, SuiteSparse_config.h for >= v4). +# Extract the SuiteSparse version from the appropriate header (UFconfig.h for <= +# v3, SuiteSparse_config.h for >= v4). list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS SUITESPARSE_VERSION) if(UFCONFIG_FOUND) @@ -352,31 +342,33 @@ if(UFCONFIG_FOUND) file(READ ${SUITESPARSE_VERSION_FILE} UFCONFIG_CONTENTS) string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION [0-9]+" - SUITESPARSE_MAIN_VERSION "${UFCONFIG_CONTENTS}") + SUITESPARSE_MAIN_VERSION "${UFCONFIG_CONTENTS}") string(REGEX REPLACE "#define SUITESPARSE_MAIN_VERSION ([0-9]+)" "\\1" - SUITESPARSE_MAIN_VERSION "${SUITESPARSE_MAIN_VERSION}") + SUITESPARSE_MAIN_VERSION "${SUITESPARSE_MAIN_VERSION}") string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION [0-9]+" - SUITESPARSE_SUB_VERSION "${UFCONFIG_CONTENTS}") + SUITESPARSE_SUB_VERSION "${UFCONFIG_CONTENTS}") string(REGEX REPLACE "#define SUITESPARSE_SUB_VERSION ([0-9]+)" "\\1" - SUITESPARSE_SUB_VERSION "${SUITESPARSE_SUB_VERSION}") + SUITESPARSE_SUB_VERSION "${SUITESPARSE_SUB_VERSION}") string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION [0-9]+" - SUITESPARSE_SUBSUB_VERSION "${UFCONFIG_CONTENTS}") - string(REGEX REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1" - SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_SUBSUB_VERSION}") + SUITESPARSE_SUBSUB_VERSION "${UFCONFIG_CONTENTS}") + string(REGEX + REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1" + SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_SUBSUB_VERSION}") # This is on a single line s/t CMake does not interpret it as a list of # elements and insert ';' separators which would result in 4.;2.;1 nonsense. set(SUITESPARSE_VERSION - "${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}") + "${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}" + ) endif() endif() if(SUITESPARSE_CONFIG_FOUND) # SuiteSparse version >= 4. set(SUITESPARSE_VERSION_FILE - ${SUITESPARSE_CONFIG_INCLUDE_DIR}/SuiteSparse_config.h) + ${SUITESPARSE_CONFIG_INCLUDE_DIR}/SuiteSparse_config.h) if(NOT EXISTS ${SUITESPARSE_VERSION_FILE}) suitesparse_report_not_found( "Could not find file: ${SUITESPARSE_VERSION_FILE} containing version " @@ -386,24 +378,26 @@ if(SUITESPARSE_CONFIG_FOUND) file(READ ${SUITESPARSE_VERSION_FILE} SUITESPARSE_CONFIG_CONTENTS) string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION [0-9]+" - SUITESPARSE_MAIN_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") + SUITESPARSE_MAIN_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") string(REGEX REPLACE "#define SUITESPARSE_MAIN_VERSION ([0-9]+)" "\\1" - SUITESPARSE_MAIN_VERSION "${SUITESPARSE_MAIN_VERSION}") + SUITESPARSE_MAIN_VERSION "${SUITESPARSE_MAIN_VERSION}") string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION [0-9]+" - SUITESPARSE_SUB_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") + SUITESPARSE_SUB_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") string(REGEX REPLACE "#define SUITESPARSE_SUB_VERSION ([0-9]+)" "\\1" - SUITESPARSE_SUB_VERSION "${SUITESPARSE_SUB_VERSION}") + SUITESPARSE_SUB_VERSION "${SUITESPARSE_SUB_VERSION}") string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION [0-9]+" - SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") - string(REGEX REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1" - SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_SUBSUB_VERSION}") + SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") + string(REGEX + REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1" + SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_SUBSUB_VERSION}") # This is on a single line s/t CMake does not interpret it as a list of # elements and insert ';' separators which would result in 4.;2.;1 nonsense. set(SUITESPARSE_VERSION - "${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}") + "${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}" + ) endif() endif() @@ -413,29 +407,29 @@ suitesparse_find_component(METIS LIBRARIES metis) # Only mark SuiteSparse as found if all required components and dependencies # have been found. set(SUITESPARSE_FOUND TRUE) -foreach(REQUIRED_VAR ${SUITESPARSE_FOUND_REQUIRED_VARS}) - if(NOT ${REQUIRED_VAR}) +foreach(required_var ${SUITESPARSE_FOUND_REQUIRED_VARS}) + if(NOT ${required_var}) set(SUITESPARSE_FOUND FALSE) endif() endforeach() if(SUITESPARSE_FOUND) - list(APPEND SUITESPARSE_INCLUDE_DIRS + list( + APPEND + SUITESPARSE_INCLUDE_DIRS ${AMD_INCLUDE_DIR} ${CAMD_INCLUDE_DIR} ${COLAMD_INCLUDE_DIR} ${CCOLAMD_INCLUDE_DIR} ${CHOLMOD_INCLUDE_DIR} ${SUITESPARSEQR_INCLUDE_DIR}) - # Handle config separately, as otherwise at least one of them will be set - # to NOTFOUND which would cause any check on SUITESPARSE_INCLUDE_DIRS to fail. + # Handle config separately, as otherwise at least one of them will be set to + # NOTFOUND which would cause any check on SUITESPARSE_INCLUDE_DIRS to fail. if(SUITESPARSE_CONFIG_FOUND) - list(APPEND SUITESPARSE_INCLUDE_DIRS - ${SUITESPARSE_CONFIG_INCLUDE_DIR}) + list(APPEND SUITESPARSE_INCLUDE_DIRS ${SUITESPARSE_CONFIG_INCLUDE_DIR}) endif() if(UFCONFIG_FOUND) - list(APPEND SUITESPARSE_INCLUDE_DIRS - ${UFCONFIG_INCLUDE_DIR}) + list(APPEND SUITESPARSE_INCLUDE_DIRS ${UFCONFIG_INCLUDE_DIR}) endif() # As SuiteSparse includes are often all in the same directory, remove any # repetitions. @@ -443,7 +437,9 @@ if(SUITESPARSE_FOUND) # Important: The ordering of these libraries is *NOT* arbitrary, as these # could potentially be static libraries their link ordering is important. - list(APPEND SUITESPARSE_LIBRARIES + list( + APPEND + SUITESPARSE_LIBRARIES ${SUITESPARSEQR_LIBRARY} ${CHOLMOD_LIBRARY} ${CCOLAMD_LIBRARY} @@ -453,36 +449,38 @@ if(SUITESPARSE_FOUND) ${LAPACK_LIBRARIES} ${BLAS_LIBRARIES}) if(SUITESPARSE_CONFIG_FOUND) - list(APPEND SUITESPARSE_LIBRARIES - ${SUITESPARSE_CONFIG_LIBRARY}) + list(APPEND SUITESPARSE_LIBRARIES ${SUITESPARSE_CONFIG_LIBRARY}) endif() if(METIS_FOUND) - list(APPEND SUITESPARSE_LIBRARIES - ${METIS_LIBRARY}) + list(APPEND SUITESPARSE_LIBRARIES ${METIS_LIBRARY}) endif() endif() # Determine if we are running on Ubuntu with the package install of SuiteSparse # which is broken and does not support linking a shared library. set(SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION FALSE) -if(CMAKE_SYSTEM_NAME MATCHES "Linux" AND SUITESPARSE_VERSION VERSION_EQUAL 3.4.0) +if(CMAKE_SYSTEM_NAME MATCHES "Linux" AND SUITESPARSE_VERSION VERSION_EQUAL + 3.4.0) find_program(LSB_RELEASE_EXECUTABLE lsb_release) if(LSB_RELEASE_EXECUTABLE) - # Any even moderately recent Ubuntu release (likely to be affected by - # this bug) should have lsb_release, if it isn't present we are likely - # on a different Linux distribution (should be fine). - execute_process(COMMAND ${LSB_RELEASE_EXECUTABLE} -si + # Any even moderately recent Ubuntu release (likely to be affected by this + # bug) should have lsb_release, if it isn't present we are likely on a + # different Linux distribution (should be fine). + execute_process( + COMMAND ${LSB_RELEASE_EXECUTABLE} -si OUTPUT_VARIABLE LSB_DISTRIBUTOR_ID OUTPUT_STRIP_TRAILING_WHITESPACE) - if(LSB_DISTRIBUTOR_ID MATCHES "Ubuntu" AND - SUITESPARSE_LIBRARIES MATCHES "/usr/lib/libamd") - # We are on Ubuntu, and the SuiteSparse version matches the broken - # system install version and is a system install. + if(LSB_DISTRIBUTOR_ID MATCHES "Ubuntu" AND SUITESPARSE_LIBRARIES MATCHES + "/usr/lib/libamd") + # We are on Ubuntu, and the SuiteSparse version matches the broken system + # install version and is a system install. set(SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION TRUE) - message(STATUS "Found system install of SuiteSparse " - "${SUITESPARSE_VERSION} running on Ubuntu, which has a known bug " - "preventing linking of shared libraries (static linking unaffected).") + message( + STATUS + "Found system install of SuiteSparse " + "${SUITESPARSE_VERSION} running on Ubuntu, which has a known bug " + "preventing linking of shared libraries (static linking unaffected).") endif() endif() endif() @@ -492,15 +490,17 @@ suitesparse_reset_find_library_prefix() # Handle REQUIRED and QUIET arguments to FIND_PACKAGE include(FindPackageHandleStandardArgs) if(SUITESPARSE_FOUND) - find_package_handle_standard_args(SUITESPARSE + find_package_handle_standard_args( + SUITESPARSE REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS} VERSION_VAR SUITESPARSE_VERSION FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse.") else() # Do not pass VERSION_VAR to FindPackageHandleStandardArgs() if we failed to - # find SuiteSparse to avoid a confusing autogenerated failure message - # that states 'not found (missing: FOO) (found version: x.y.z)'. - find_package_handle_standard_args(SUITESPARSE + # find SuiteSparse to avoid a confusing autogenerated failure message that + # states 'not found (missing: FOO) (found version: x.y.z)'. + find_package_handle_standard_args( + SUITESPARSE REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS} FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse.") endif() diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp index 8bb4ac4c7..0e02561bd 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp @@ -57,7 +57,6 @@ #include #include - namespace fuse_constraints { @@ -71,7 +70,7 @@ namespace fuse_constraints * prior map (scan-to-map measurements). This constraint holds the measured variable value and the * measurement uncertainty/covariance. */ -template +template class AbsoluteConstraint : public fuse_core::Constraint { public: @@ -90,11 +89,8 @@ class AbsoluteConstraint : public fuse_core::Constraint * @param[in] mean The measured/prior value of all variable dimensions * @param[in] covariance The measurement/prior uncertainty of all variable dimensions */ - AbsoluteConstraint( - const std::string & source, - const Variable & variable, - const fuse_core::VectorXd & mean, - const fuse_core::MatrixXd & covariance); + AbsoluteConstraint(const std::string& source, const Variable& variable, const fuse_core::VectorXd& mean, + const fuse_core::MatrixXd& covariance); /** * @brief Create a constraint using a measurement/prior of only a partial set of dimensions of the @@ -109,12 +105,8 @@ class AbsoluteConstraint : public fuse_core::Constraint * by \p indices. * @param[in] indices The set of indices corresponding to the measured dimensions */ - AbsoluteConstraint( - const std::string & source, - const Variable & variable, - const fuse_core::VectorXd & partial_mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & indices); + AbsoluteConstraint(const std::string& source, const Variable& variable, const fuse_core::VectorXd& partial_mean, + const fuse_core::MatrixXd& partial_covariance, const std::vector& indices); /** * @brief Destructor @@ -128,7 +120,10 @@ class AbsoluteConstraint : public fuse_core::Constraint * are in the order defined by the variable, not the order defined by the \p indices parameter. * All unmeasured variable dimensions are set to zero. */ - const fuse_core::VectorXd & mean() const {return mean_;} + const fuse_core::VectorXd& mean() const + { + return mean_; + } /** * @brief Read-only access to the square root information matrix. @@ -138,7 +133,10 @@ class AbsoluteConstraint : public fuse_core::Constraint * variable_dimensions. If only a partial set of dimensions are measured, then this matrix will * not be square. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -155,7 +153,7 @@ class AbsoluteConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -167,10 +165,10 @@ class AbsoluteConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::VectorXd mean_; //!< The measured/prior mean vector for this variable + fuse_core::VectorXd mean_; //!< The measured/prior mean vector for this variable fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix private: @@ -184,28 +182,23 @@ class AbsoluteConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& mean_; + archive& sqrt_information_; } }; // Define unique names for the different variations of the absolute constraint -using AbsoluteAccelerationAngular2DStampedConstraint = - AbsoluteConstraint; -using AbsoluteAccelerationLinear2DStampedConstraint = - AbsoluteConstraint; -using AbsoluteOrientation2DStampedConstraint = - AbsoluteConstraint; +using AbsoluteAccelerationAngular2DStampedConstraint = AbsoluteConstraint; +using AbsoluteAccelerationLinear2DStampedConstraint = AbsoluteConstraint; +using AbsoluteOrientation2DStampedConstraint = AbsoluteConstraint; using AbsolutePosition2DStampedConstraint = AbsoluteConstraint; using AbsolutePosition3DStampedConstraint = AbsoluteConstraint; -using AbsoluteVelocityAngular2DStampedConstraint = - AbsoluteConstraint; -using AbsoluteVelocityLinear2DStampedConstraint = - AbsoluteConstraint; +using AbsoluteVelocityAngular2DStampedConstraint = AbsoluteConstraint; +using AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint; } // namespace fuse_constraints // Include the template implementation diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp index cbb689b54..f28ceb3bf 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp @@ -42,33 +42,28 @@ #include - namespace fuse_constraints { -template -AbsoluteConstraint::AbsoluteConstraint( - const std::string & source, - const Variable & variable, - const fuse_core::VectorXd & mean, - const fuse_core::MatrixXd & covariance) -: fuse_core::Constraint(source, {variable.uuid()}), // NOLINT(whitespace/braces) - mean_(mean), - sqrt_information_(covariance.inverse().llt().matrixU()) +template +AbsoluteConstraint::AbsoluteConstraint(const std::string& source, const Variable& variable, + const fuse_core::VectorXd& mean, const fuse_core::MatrixXd& covariance) + : fuse_core::Constraint(source, { variable.uuid() }) + , // NOLINT(whitespace/braces) + mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) { assert(mean.rows() == static_cast(variable.size())); assert(covariance.rows() == static_cast(variable.size())); assert(covariance.cols() == static_cast(variable.size())); } -template -AbsoluteConstraint::AbsoluteConstraint( - const std::string & source, - const Variable & variable, - const fuse_core::VectorXd & partial_mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & indices) -: fuse_core::Constraint(source, {variable.uuid()}) // NOLINT(whitespace/braces) +template +AbsoluteConstraint::AbsoluteConstraint(const std::string& source, const Variable& variable, + const fuse_core::VectorXd& partial_mean, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& indices) + : fuse_core::Constraint(source, { variable.uuid() }) // NOLINT(whitespace/braces) { assert(partial_mean.rows() == static_cast(indices.size())); assert(partial_covariance.rows() == static_cast(indices.size())); @@ -86,13 +81,14 @@ AbsoluteConstraint::AbsoluteConstraint( // and the columns are in the order defined by the variable. mean_ = fuse_core::VectorXd::Zero(variable.size()); sqrt_information_ = fuse_core::MatrixXd::Zero(indices.size(), variable.size()); - for (size_t i = 0; i < indices.size(); ++i) { + for (size_t i = 0; i < indices.size(); ++i) + { mean_(indices[i]) = partial_mean(i); sqrt_information_.col(indices[i]) = partial_sqrt_information.col(i); } } -template +template fuse_core::MatrixXd AbsoluteConstraint::covariance() const { // We want to compute: @@ -107,8 +103,8 @@ fuse_core::MatrixXd AbsoluteConstraint::covariance() const return pinv * pinv.transpose(); } -template -void AbsoluteConstraint::print(std::ostream & stream) const +template +void AbsoluteConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -117,14 +113,15 @@ void AbsoluteConstraint::print(std::ostream & stream) const << " mean: " << mean().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -template -ceres::CostFunction * AbsoluteConstraint::costFunction() const +template +ceres::CostFunction* AbsoluteConstraint::costFunction() const { // Ceres ships with a "prior" cost function. Just use that here. return new ceres::NormalPrior(sqrt_information_, mean_); @@ -133,53 +130,50 @@ ceres::CostFunction * AbsoluteConstraint::costFunction() const // Specialization for Orientation2D // We need to handle the 2*pi rollover for 2D orientations, so simple subtraction does not produce // the correct cost -template<> -inline ceres::CostFunction * AbsoluteConstraint< - fuse_variables::Orientation2DStamped ->::costFunction() -const +template <> +inline ceres::CostFunction* AbsoluteConstraint::costFunction() const { return new NormalPriorOrientation2D(sqrt_information_(0, 0), mean_(0)); } // Specialize the type() method to return the name that is registered with the plugins -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint"; } -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint"; } -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsoluteOrientation2DStampedConstraint"; } -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsolutePosition2DStampedConstraint"; } -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsolutePosition3DStampedConstraint"; } -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint"; } -template<> +template <> inline std::string AbsoluteConstraint::type() const { return "fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint"; diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h index e02d054f3..92eb503a2 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h +++ b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h @@ -35,9 +35,7 @@ #ifndef FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include \ - fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp instead +#warning This header is obsolete, please include fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp index 0f63bdf86..d3f7b02c0 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_constraints { @@ -80,11 +79,9 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] mean The measured/prior orientation as a quaternion (4x1 vector: w, x, y, z) * @param[in] covariance The measurement/prior covariance (3x3 matrix: qx, qy, qz) */ - AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector4d & mean, - const fuse_core::Matrix3d & covariance); + AbsoluteOrientation3DStampedConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::Vector4d& mean, const fuse_core::Matrix3d& covariance); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -94,11 +91,9 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] mean The measured/prior orientation as an Eigen quaternion * @param[in] covariance The measurement/prior covariance (3x3 matrix: qx, qy, qz) */ - AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const Eigen::Quaterniond & mean, - const fuse_core::Matrix3d & covariance); + AbsoluteOrientation3DStampedConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation, + const Eigen::Quaterniond& mean, const fuse_core::Matrix3d& covariance); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -108,11 +103,10 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] mean The measured/prior orientation as a ROS quaternion message * @param[in] covariance The measurement/prior covariance (3x3 matrix: qx, qy, qz) */ - AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const geometry_msgs::msg::Quaternion & mean, - const std::array & covariance); + AbsoluteOrientation3DStampedConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation, + const geometry_msgs::msg::Quaternion& mean, + const std::array& covariance); /** * @brief Destructor @@ -124,14 +118,20 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * * Order is (w, x, y, z) */ - const fuse_core::Vector4d & mean() const {return mean_;} + const fuse_core::Vector4d& mean() const + { + return mean_; + } /** * @brief Read-only access to the square root information matrix. * * Order is (x, y, z) */ - const fuse_core::Matrix3d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix3d& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -145,7 +145,7 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -157,7 +157,7 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: /** @@ -165,23 +165,23 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] quaternion - The input Eigen quaternion * @return The \p quaternion, converted to an Eigen Vector4d */ - static fuse_core::Vector4d toEigen(const Eigen::Quaterniond & quaternion); + static fuse_core::Vector4d toEigen(const Eigen::Quaterniond& quaternion); /** * @brief Utility method to convert an ROS quaternion message to an Eigen Vector4d * @param[in] quaternion - The input ROS quaternion message * @return The \p quaternion, converted to an Eigen Vector4d */ - static fuse_core::Vector4d toEigen(const geometry_msgs::msg::Quaternion & quaternion); + static fuse_core::Vector4d toEigen(const geometry_msgs::msg::Quaternion& quaternion); /** * @brief Utility method to convert a flat 1D array to a 3x3 Eigen matrix * @param[in] covariance - The input covariance array * @return The \p covariance, converted to an Eigen Matrix3d */ - static fuse_core::Matrix3d toEigen(const std::array & covariance); + static fuse_core::Matrix3d toEigen(const std::array& covariance); - fuse_core::Vector4d mean_; //!< The measured/prior mean vector for this variable + fuse_core::Vector4d mean_; //!< The measured/prior mean vector for this variable fuse_core::Matrix3d sqrt_information_; //!< The square root information matrix private: @@ -195,12 +195,12 @@ class AbsoluteOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& mean_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h index 4d388fa17..0511e627f 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h +++ b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h @@ -35,9 +35,7 @@ #ifndef FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_EULER_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_EULER_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include \ - fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp instead +#warning This header is obsolete, please include fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp index b224a2878..bcf527abc 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_constraints { @@ -87,12 +86,10 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * @param[in] axes Used to specify which of the Euler axes they want to include in the * constraint, e.g. "{ Euler::ROLL, EULER::YAW }" */ - AbsoluteOrientation3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::VectorXd & mean, - const fuse_core::MatrixXd & covariance, - const std::vector & axes); + AbsoluteOrientation3DStampedEulerConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::VectorXd& mean, const fuse_core::MatrixXd& covariance, + const std::vector& axes); /** * @brief Destructor @@ -103,7 +100,10 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * @brief Read-only access to the vector that dictates the order of the Euler axes in the \p mean, * \p covariance, and \p sqrtInformation. */ - const std::vector axes() const {return axes_;} + const std::vector axes() const + { + return axes_; + } /** * @brief Read-only access to the measured/prior vector of mean values. @@ -112,7 +112,10 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * other currently implemented constraints in that the order does _not_ match the order defined in * the variable. */ - const fuse_core::VectorXd & mean() const {return mean_;} + const fuse_core::VectorXd& mean() const + { + return mean_; + } /** * @brief Read-only access to the square root information matrix. @@ -121,7 +124,10 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * from all other currently implemented constraints in that the order does _not_ match the order * defined in the variable. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -137,7 +143,7 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -149,12 +155,12 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::VectorXd mean_; //!< The measured/prior mean vector for this variable + fuse_core::VectorXd mean_; //!< The measured/prior mean vector for this variable fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix - std::vector axes_; //!< Which Euler angle axes we want to measure + std::vector axes_; //!< Which Euler angle axes we want to measure private: // Allow Boost Serialization access to private methods @@ -167,13 +173,13 @@ class AbsoluteOrientation3DStampedEulerConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; - archive & axes_; + archive& boost::serialization::base_object(*this); + archive& mean_; + archive& sqrt_information_; + archive& axes_; } }; diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.h index 5c1e81bed..5be4cba8e 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.h +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__ABSOLUTE_POSE_2D_STAMPED_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__ABSOLUTE_POSE_2D_STAMPED_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include fuse_constraints/absolute_pose_2d_stamped_constraint.hpp \ +#warning This header is obsolete, please include fuse_constraints/absolute_pose_2d_stamped_constraint.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.hpp index e9ed10a56..00623124b 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_constraints { @@ -66,7 +65,7 @@ namespace fuse_constraints * Some sensors, such as GPS, provide direct measurements of the robot's pose in the global frame. * And localization systems often match laserscans to a prior map (scan-to-map measurements). This * constraint holds the measured 2D pose and the measurement uncertainty/covariance. It also permits - * measurement of a subset of the pose provided in the position and orientation varables. + * measurement of a subset of the pose provided in the position and orientation variables. */ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint { @@ -103,14 +102,12 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * dimensions e.g. "{fuse_variables::Orientation2DStamped::Yaw}" */ AbsolutePose2DStampedConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation, - const fuse_core::VectorXd & partial_mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & linear_indices = - {fuse_variables::Position2DStamped::X, fuse_variables::Position2DStamped::Y}, // NOLINT - const std::vector & angular_indices = {fuse_variables::Orientation2DStamped::YAW}); // NOLINT + const std::string& source, const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation, const fuse_core::VectorXd& partial_mean, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& linear_indices = { fuse_variables::Position2DStamped::X, + fuse_variables::Position2DStamped::Y }, // NOLINT + const std::vector& angular_indices = { fuse_variables::Orientation2DStamped::YAW }); // NOLINT /** * @brief Destructor @@ -123,7 +120,10 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * Order is (x, y, yaw). Note that the returned vector will be full sized (3x1) and in the stated * order. */ - const fuse_core::Vector3d & mean() const {return mean_;} + const fuse_core::Vector3d& mean() const + { + return mean_; + } /** * @brief Read-only access to the square root information matrix. @@ -131,7 +131,10 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * If only a partial covariance matrix was provided in the constructor, this covariance matrix * will not be square. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -147,7 +150,7 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -159,10 +162,10 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::Vector3d mean_; //!< The measured/prior mean vector for this variable + fuse_core::Vector3d mean_; //!< The measured/prior mean vector for this variable fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix private: @@ -176,12 +179,12 @@ class AbsolutePose2DStampedConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& mean_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.h index e5a3240ac..2f501a6f7 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.h +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include fuse_constraints/absolute_pose_3d_stamped_constraint.hpp \ +#warning This header is obsolete, please include fuse_constraints/absolute_pose_3d_stamped_constraint.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp index 51238c8b7..8fcc4925c 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_constraints { @@ -88,12 +87,9 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * (7x1 vector: x, y, z, qw, qx, qy, qz) * @param[in] covariance The measurement/prior covariance (6x6 matrix: x, y, z, qx, qy, qz) */ - AbsolutePose3DStampedConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector7d & mean, - const fuse_core::Matrix6d & covariance); + AbsolutePose3DStampedConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::Vector7d& mean, const fuse_core::Matrix6d& covariance); /** * @brief Destructor @@ -105,14 +101,20 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * * Order is (x, y, z, qw, qx, qy, qz) */ - const fuse_core::Vector7d & mean() const {return mean_;} + const fuse_core::Vector7d& mean() const + { + return mean_; + } /** * @brief Read-only access to the square root information matrix. * * Order is (x, y, z, qx, qy, qz) */ - const fuse_core::Matrix6d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix6d& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -129,7 +131,7 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -141,10 +143,10 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::Vector7d mean_; //!< The measured/prior mean vector for this variable + fuse_core::Vector7d mean_; //!< The measured/prior mean vector for this variable fuse_core::Matrix6d sqrt_information_; //!< The square root information matrix private: @@ -158,12 +160,12 @@ class AbsolutePose3DStampedConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& mean_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp b/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp index 15f729ab7..f9c8ed372 100644 --- a/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/marginal_constraint.hpp @@ -65,7 +65,6 @@ #include #include - namespace fuse_constraints { @@ -103,14 +102,9 @@ class MarginalConstraint : public fuse_core::Constraint * @param[in] last_A Iterator pointing to one past the last A matrix * @param[in] b The b vector of the marginal cost (of the form A*(x - x_bar) + b) */ - template - MarginalConstraint( - const std::string & source, - VariableIterator first_variable, - VariableIterator last_variable, - MatrixIterator first_A, - MatrixIterator last_A, - const fuse_core::VectorXd & b); + template + MarginalConstraint(const std::string& source, VariableIterator first_variable, VariableIterator last_variable, + MatrixIterator first_A, MatrixIterator last_A, const fuse_core::VectorXd& b); /** * @brief Destructor @@ -120,23 +114,32 @@ class MarginalConstraint : public fuse_core::Constraint /** * @brief Read-only access to the A matrices of the marginal constraint */ - const std::vector & A() const {return A_;} + const std::vector& A() const + { + return A_; + } /** * @brief Read-only access to the b vector of the marginal constraint */ - const fuse_core::VectorXd & b() const {return b_;} + const fuse_core::VectorXd& b() const + { + return b_; + } /** * @brief Read-only access to the variable linearization points, x_bar */ - const std::vector & x_bar() const {return x_bar_;} + const std::vector& x_bar() const + { + return x_bar_; + } #if !CERES_SUPPORTS_MANIFOLDS /** * @brief Read-only access to the variable local parameterizations */ - const std::vector & localParameterizations() const + const std::vector& localParameterizations() const { return local_parameterizations_; } @@ -144,7 +147,10 @@ class MarginalConstraint : public fuse_core::Constraint /** * @brief Read-only access to the variable manifolds */ - const std::vector & manifolds() const {return manifolds_;} + const std::vector& manifolds() const + { + return manifolds_; + } #endif /** @@ -152,7 +158,7 @@ class MarginalConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -164,11 +170,11 @@ class MarginalConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: std::vector A_; //!< The A matrices of the marginal constraint - fuse_core::VectorXd b_; //!< The b vector of the marginal constraint + fuse_core::VectorXd b_; //!< The b vector of the marginal constraint #if !CERES_SUPPORTS_MANIFOLDS //!< The local parameterizations std::vector local_parameterizations_; @@ -187,8 +193,8 @@ class MarginalConstraint : public fuse_core::Constraint * @param[out] archive - The archive object into which class members will be serialized * @param[in] version - The version of the archive being written. */ - template - void save(Archive & archive, const unsigned int /* version */) const + template + void save(Archive& archive, const unsigned int /* version */) const { archive << boost::serialization::base_object(*this); archive << A_; @@ -207,13 +213,14 @@ class MarginalConstraint : public fuse_core::Constraint * @param[in] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read. */ - template - void load(Archive & archive, const unsigned int version) + template + void load(Archive& archive, const unsigned int version) { archive >> boost::serialization::base_object(*this); archive >> A_; archive >> b_; - if (version == 0) { + if (version == 0) + { // Version 0 serialization files will contain a std::vector of LocalParameterization // shared pointers. If the current version of Ceres Solver does not support Manifolds, // then the serialized LocalParameterization pointers can be deserialized directly into @@ -224,26 +231,25 @@ class MarginalConstraint : public fuse_core::Constraint #else auto local_parameterizations = std::vector(); archive >> local_parameterizations; - std::transform( - std::make_move_iterator(local_parameterizations.begin()), - std::make_move_iterator(local_parameterizations.end()), - std::back_inserter(manifolds_), - [](fuse_core::LocalParameterization::SharedPtr local_parameterization) - {return fuse_core::ManifoldAdapter::make_shared(std::move(local_parameterization));}); + std::transform(std::make_move_iterator(local_parameterizations.begin()), + std::make_move_iterator(local_parameterizations.end()), std::back_inserter(manifolds_), + [](fuse_core::LocalParameterization::SharedPtr local_parameterization) { + return fuse_core::ManifoldAdapter::make_shared(std::move(local_parameterization)); + }); #endif - } else { // (version >= 1) - // Version 1 serialization files will contain a std::vector of Manifold shared pointers. If - // the current version of Ceres Solver does not support Manifolds, then there is no way to - // deserialize the requested data. But if the current version of Ceres Solver does support - // manifolds, then the serialized Manifold pointers can be deserialized directly into the - // class member. + } + else + { // (version >= 1) + // Version 1 serialization files will contain a std::vector of Manifold shared pointers. If + // the current version of Ceres Solver does not support Manifolds, then there is no way to + // deserialize the requested data. But if the current version of Ceres Solver does support + // manifolds, then the serialized Manifold pointers can be deserialized directly into the + // class member. #if !CERES_SUPPORTS_MANIFOLDS - throw std::runtime_error( - "Attempting to deserialize an archive saved in Version " + - std::to_string( - version) + " format. However, the current version of Ceres Solver (" + - CERES_VERSION_STRING + ") does not support manifolds. Ceres Solver version 2.1.0 " - "or later is required to load this file."); + throw std::runtime_error("Attempting to deserialize an archive saved in Version " + std::to_string(version) + + " format. However, the current version of Ceres Solver (" + CERES_VERSION_STRING + + ") does not support manifolds. Ceres Solver version 2.1.0 " + "or later is required to load this file."); #else archive >> manifolds_; #endif @@ -259,7 +265,7 @@ namespace detail /** * @brief Return the UUID of the provided variable */ -inline const fuse_core::UUID getUuid(const fuse_core::Variable & variable) +inline const fuse_core::UUID getUuid(const fuse_core::Variable& variable) { return variable.uuid(); } @@ -267,7 +273,7 @@ inline const fuse_core::UUID getUuid(const fuse_core::Variable & variable) /** * @brief Return the current value of the provided variable */ -inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable & variable) +inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable& variable) { return Eigen::Map(variable.data(), variable.size()); } @@ -276,8 +282,7 @@ inline const fuse_core::VectorXd getCurrentValue(const fuse_core::Variable & var /** * @brief Return the local parameterization of the provided variable */ -inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization( - const fuse_core::Variable & variable) +inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization(const fuse_core::Variable& variable) { return fuse_core::LocalParameterization::SharedPtr(variable.localParameterization()); } @@ -285,7 +290,7 @@ inline fuse_core::LocalParameterization::SharedPtr getLocalParameterization( /** * @brief Return the manifold of the provided variable */ -inline fuse_core::Manifold::SharedPtr getManifold(const fuse_core::Variable & variable) +inline fuse_core::Manifold::SharedPtr getManifold(const fuse_core::Variable& variable) { return fuse_core::Manifold::SharedPtr(variable.manifold()); } @@ -293,31 +298,27 @@ inline fuse_core::Manifold::SharedPtr getManifold(const fuse_core::Variable & va } // namespace detail -template -MarginalConstraint::MarginalConstraint( - const std::string & source, - VariableIterator first_variable, - VariableIterator last_variable, - MatrixIterator first_A, - MatrixIterator last_A, - const fuse_core::VectorXd & b) -: Constraint(source, - boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getUuid), - boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getUuid)), - A_(first_A, last_A), - b_(b), +template +MarginalConstraint::MarginalConstraint(const std::string& source, VariableIterator first_variable, + VariableIterator last_variable, MatrixIterator first_A, MatrixIterator last_A, + const fuse_core::VectorXd& b) + : Constraint(source, boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getUuid), + boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getUuid)) + , A_(first_A, last_A) + , b_(b) + , #if !CERES_SUPPORTS_MANIFOLDS - local_parameterizations_(boost::make_transform_iterator(first_variable, - &fuse_constraints::detail::getLocalParameterization), - boost::make_transform_iterator(last_variable, - &fuse_constraints::detail::getLocalParameterization)), + local_parameterizations_( + boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getLocalParameterization), + boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getLocalParameterization)) + , #else - manifolds_( - boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getManifold), - boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getManifold)), + manifolds_(boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getManifold), + boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getManifold)) + , #endif x_bar_(boost::make_transform_iterator(first_variable, &fuse_constraints::detail::getCurrentValue), - boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getCurrentValue)) + boost::make_transform_iterator(last_variable, &fuse_constraints::detail::getCurrentValue)) { assert(!A_.empty()); assert(A_.size() == x_bar_.size()); @@ -327,19 +328,11 @@ MarginalConstraint::MarginalConstraint( assert(A_.size() == manifolds_.size()); #endif assert(b_.rows() > 0); - assert( - std::all_of( - A_.begin(), A_.end(), [this](const auto & A) { - return A.rows() == this->b_.rows(); - })); // NOLINT - assert( - std::all_of( - boost::make_zip_iterator(boost::make_tuple(A_.begin(), first_variable)), - boost::make_zip_iterator(boost::make_tuple(A_.end(), last_variable)), - [](const boost::tuple & tuple) // NOLINT - { - return static_cast(tuple.get<0>().cols()) == tuple.get<1>().localSize(); - })); // NOLINT + assert(std::all_of(A_.begin(), A_.end(), [this](const auto& A) { return A.rows() == this->b_.rows(); })); // NOLINT + assert(std::all_of(boost::make_zip_iterator(boost::make_tuple(A_.begin(), first_variable)), + boost::make_zip_iterator(boost::make_tuple(A_.end(), last_variable)), + [](const boost::tuple& tuple) // NOLINT + { return static_cast(tuple.get<0>().cols()) == tuple.get<1>().localSize(); })); // NOLINT } } // namespace fuse_constraints diff --git a/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp b/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp index 671ebf380..c8a734221 100644 --- a/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp +++ b/fuse_constraints/include/fuse_constraints/marginal_cost_function.hpp @@ -78,11 +78,9 @@ class MarginalCostFunction : public ceres::CostFunction * @param[in] x_bar The linearization point of the involved variables * @param[in] local_parameterizations The local parameterization associated with the variable */ - MarginalCostFunction( - const std::vector & A, - const fuse_core::VectorXd & b, - const std::vector & x_bar, - const std::vector & local_parameterizations); + MarginalCostFunction(const std::vector& A, const fuse_core::VectorXd& b, + const std::vector& x_bar, + const std::vector& local_parameterizations); #else /** * @brief Construct a cost function instance @@ -92,11 +90,9 @@ class MarginalCostFunction : public ceres::CostFunction * @param[in] x_bar The linearization point of the involved variables * @param[in] manifolds The manifold associated with the variable */ - MarginalCostFunction( - const std::vector & A, - const fuse_core::VectorXd & b, - const std::vector & x_bar, - const std::vector & manifolds); + MarginalCostFunction(const std::vector& A, const fuse_core::VectorXd& b, + const std::vector& x_bar, + const std::vector& manifolds); #endif /** @@ -108,21 +104,18 @@ class MarginalCostFunction : public ceres::CostFunction * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const override; + bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override; private: - const std::vector & A_; //!< The A matrices of the marginal cost - const fuse_core::VectorXd & b_; //!< The b vector of the marginal cost + const std::vector& A_; //!< The A matrices of the marginal cost + const fuse_core::VectorXd& b_; //!< The b vector of the marginal cost #if !CERES_SUPPORTS_MANIFOLDS //!< Parameterizations - const std::vector & local_parameterizations_; + const std::vector& local_parameterizations_; #else - const std::vector & manifolds_; //!< Manifolds + const std::vector& manifolds_; //!< Manifolds #endif - const std::vector & x_bar_; //!< The linearization point of each variable + const std::vector& x_bar_; //!< The linearization point of each variable }; } // namespace fuse_constraints diff --git a/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp b/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp index 1cc666243..6911376e6 100644 --- a/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp +++ b/fuse_constraints/include/fuse_constraints/marginalize_variables.hpp @@ -54,7 +54,6 @@ #include - namespace fuse_constraints { @@ -75,9 +74,8 @@ namespace fuse_constraints * least one marginalized variable * @return The mapping from variable UUID to the computed elimination order */ -UuidOrdering computeEliminationOrder( - const std::vector & marginalized_variables, - const fuse_core::Graph & graph); +UuidOrdering computeEliminationOrder(const std::vector& marginalized_variables, + const fuse_core::Graph& graph); /** * @brief Generate a transaction that, when applied to the graph, will marginalize out the requested @@ -99,10 +97,9 @@ UuidOrdering computeEliminationOrder( * @return A transaction object containing the computed marginal constraints to be added, as well as * the set of variables and constraints to be removed. */ -fuse_core::Transaction marginalizeVariables( - const std::string & source, - const std::vector & marginalized_variables, - const fuse_core::Graph & graph); +fuse_core::Transaction marginalizeVariables(const std::string& source, + const std::vector& marginalized_variables, + const fuse_core::Graph& graph); /** * @brief Generate a transaction that, when applied to the graph, will marginalize out the requested @@ -126,11 +123,10 @@ fuse_core::Transaction marginalizeVariables( * @return A transaction object containing the computed marginal constraints to be added, as well as * the set of variables and constraints to be removed. */ -fuse_core::Transaction marginalizeVariables( - const std::string & source, - const std::vector & marginalized_variables, - const fuse_core::Graph & graph, - const fuse_constraints::UuidOrdering & elimination_order); +fuse_core::Transaction marginalizeVariables(const std::string& source, + const std::vector& marginalized_variables, + const fuse_core::Graph& graph, + const fuse_constraints::UuidOrdering& elimination_order); namespace detail { @@ -160,10 +156,8 @@ struct LinearTerm * @return A LinearTerm consisting of Jacobian blocks associated with each involved variable in * elimination order */ -LinearTerm linearize( - const fuse_core::Constraint & constraint, - const fuse_core::Graph & graph, - const UuidOrdering & elimination_order); +LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::Graph& graph, + const UuidOrdering& elimination_order); /** * @brief Marginalize out the lowest-ordered variable from the provided set of linear terms @@ -175,7 +169,7 @@ LinearTerm linearize( * index * @return A LinearTerm object containing the information on the remaining variables */ -LinearTerm marginalizeNext(const std::vector & linear_terms); +LinearTerm marginalizeNext(const std::vector& linear_terms); /** * @brief Convert the provided linear term into a MarginalConstraint @@ -187,11 +181,9 @@ LinearTerm marginalizeNext(const std::vector & linear_terms); * @param[in] elimination_order The mapping from variable UUID to LinearTerm variable index * @return An equivalent MarginalConstraint object */ -MarginalConstraint::SharedPtr createMarginalConstraint( - const std::string & source, - const LinearTerm & linear_term, - const fuse_core::Graph & graph, - const UuidOrdering & elimination_order); +MarginalConstraint::SharedPtr createMarginalConstraint(const std::string& source, const LinearTerm& linear_term, + const fuse_core::Graph& graph, + const UuidOrdering& elimination_order); } // namespace detail } // namespace fuse_constraints diff --git a/fuse_constraints/include/fuse_constraints/normal_delta.hpp b/fuse_constraints/include/fuse_constraints/normal_delta.hpp index c9aad461d..8cf4ebd85 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta.hpp @@ -38,7 +38,6 @@ #include - namespace fuse_constraints { @@ -73,7 +72,7 @@ class NormalDelta : public ceres::CostFunction * these are the same type of variable. At a minimum, they must have the same * dimensions and the per-element subtraction operator must be valid. */ - NormalDelta(const fuse_core::MatrixXd & A, const fuse_core::VectorXd & b); + NormalDelta(const fuse_core::MatrixXd& A, const fuse_core::VectorXd& b); /** * @brief Destructor @@ -84,10 +83,7 @@ class NormalDelta : public ceres::CostFunction * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - virtual bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const; + virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; private: fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.h b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.h index 1a54e6c9a..b443ece34 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.h +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_2D_H_ #define FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_2D_H_ -#warning \ - This header is obsolete, please include fuse_constraints/normal_delta_orientation_2d.hpp instead +#warning This header is obsolete, please include fuse_constraints/normal_delta_orientation_2d.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.hpp index b8e9e0505..5864f1962 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.hpp @@ -36,7 +36,6 @@ #include - namespace fuse_constraints { @@ -80,10 +79,7 @@ class NormalDeltaOrientation2D : public ceres::SizedCostFunction<1, 1, 1> * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - virtual bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const; + virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; private: double A_; //!< The residual weighting matrix, most likely the square root information matrix diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.h index 5e812868b..ae0a55424 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.h @@ -35,9 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_COST_FUNCTOR_H_ #define FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include \ - fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp instead +#warning This header is obsolete, please include fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp index 988c623bb..dcdebdba9 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp @@ -42,7 +42,6 @@ #include #include - namespace fuse_constraints { @@ -80,37 +79,21 @@ class NormalDeltaOrientation3DCostFunctor * order (x, y, z) * @param[in] b The measured change between the two orientation variables */ - NormalDeltaOrientation3DCostFunctor( - const fuse_core::Matrix3d & A, - const fuse_core::Vector4d & b) - : A_(A), - b_(b) + NormalDeltaOrientation3DCostFunctor(const fuse_core::Matrix3d& A, const fuse_core::Vector4d& b) : A_(A), b_(b) { } /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. */ - template - bool operator()(const T * const orientation1, const T * const orientation2, T * residuals) const + template + bool operator()(const T* const orientation1, const T* const orientation2, T* residuals) const { using fuse_variables::Orientation3DStamped; - T orientation1_inverse[4] = - { - orientation1[0], - -orientation1[1], - -orientation1[2], - -orientation1[3] - }; + T orientation1_inverse[4] = { orientation1[0], -orientation1[1], -orientation1[2], -orientation1[3] }; - T observation_inverse[4] = - { - T(b_(0)), - T(-b_(1)), - T(-b_(2)), - T(-b_(3)) - }; + T observation_inverse[4] = { T(b_(0)), T(-b_(1)), T(-b_(2)), T(-b_(3)) }; T difference[4]; ceres::QuaternionProduct(orientation1_inverse, orientation2, difference); diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.hpp index c69acb90a..5f41b980a 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.hpp @@ -38,7 +38,6 @@ #include - namespace fuse_constraints { @@ -83,16 +82,13 @@ class NormalDeltaPose2D : public ceres::SizedCostFunction diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.hpp index 5badc5c7b..ddbf7ddf6 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.hpp @@ -40,7 +40,6 @@ #include #include - namespace fuse_constraints { @@ -85,18 +84,14 @@ class NormalDeltaPose2DCostFunctor * order (x, y, yaw) * @param[in] b The exposed pose difference in order (x, y, yaw) */ - NormalDeltaPose2DCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector3d & b); + NormalDeltaPose2DCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b); /** * @brief Compute the cost values/residuals using the provided variable/parameter values */ - template - bool operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const; + template + bool operator()(const T* const position1, const T* const orientation1, const T* const position2, + const T* const orientation2, T* residual) const; private: fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root @@ -104,30 +99,22 @@ class NormalDeltaPose2DCostFunctor fuse_core::Vector3d b_; //!< The measured difference between variable x0 and variable x1 }; -NormalDeltaPose2DCostFunctor::NormalDeltaPose2DCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::Vector3d & b) -: A_(A), - b_(b) +NormalDeltaPose2DCostFunctor::NormalDeltaPose2DCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) + : A_(A), b_(b) { } -template -bool NormalDeltaPose2DCostFunctor::operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const +template +bool NormalDeltaPose2DCostFunctor::operator()(const T* const position1, const T* const orientation1, + const T* const position2, const T* const orientation2, T* residual) const { Eigen::Map> position1_vector(position1); Eigen::Map> position2_vector(position2); Eigen::Matrix full_residuals_vector; full_residuals_vector.template head<2>() = - fuse_core::rotationMatrix2D(orientation1[0]).transpose() * - (position2_vector - position1_vector) - - b_.head<2>().template cast(); + fuse_core::rotationMatrix2D(orientation1[0]).transpose() * (position2_vector - position1_vector) - + b_.head<2>().template cast(); full_residuals_vector(2) = fuse_core::wrapAngle2D(orientation2[0] - orientation1[0] - T(b_(2))); // Scale the residuals by the square root information matrix to account for diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h index 3d9c34dc7..cad6467d2 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H_ #define FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include fuse_constraints/normal_delta_pose_3d_cost_functor.hpp \ +#warning This header is obsolete, please include fuse_constraints/normal_delta_pose_3d_cost_functor.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp index ace413e51..3a939ea64 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.hpp @@ -41,7 +41,6 @@ #include #include - namespace fuse_constraints { @@ -78,18 +77,14 @@ class NormalDeltaPose3DCostFunctor * order (dx, dy, dz, dqx, dqy, dqz) * @param[in] b The exposed pose difference in order (dx, dy, dz, dqw, dqx, dqy, dqz) */ - NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); + NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b); /** * @brief Compute the cost values/residuals using the provided variable/parameter values */ - template - bool operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const; + template + bool operator()(const T* const position1, const T* const orientation1, const T* const position2, + const T* const orientation2, T* residual) const; private: fuse_core::Matrix6d A_; //!< The residual weighting matrix, most likely the square root @@ -99,43 +94,21 @@ class NormalDeltaPose3DCostFunctor NormalDeltaOrientation3DCostFunctor orientation_functor_; }; -NormalDeltaPose3DCostFunctor::NormalDeltaPose3DCostFunctor( - const fuse_core::Matrix6d & A, - const fuse_core::Vector7d & b) -: A_(A), - b_(b), - orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Orientation residuals will - // not be scaled +NormalDeltaPose3DCostFunctor::NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b) + : A_(A), b_(b), orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Orientation residuals will + // not be scaled { } -template -bool NormalDeltaPose3DCostFunctor::operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const +template +bool NormalDeltaPose3DCostFunctor::operator()(const T* const position1, const T* const orientation1, + const T* const position2, const T* const orientation2, T* residual) const { // Compute the position delta between pose1 and pose2 - T orientation1_inverse[4] = - { - orientation1[0], - -orientation1[1], - -orientation1[2], - -orientation1[3] - }; - T position_delta[3] = - { - position2[0] - position1[0], - position2[1] - position1[1], - position2[2] - position1[2] - }; + T orientation1_inverse[4] = { orientation1[0], -orientation1[1], -orientation1[2], -orientation1[3] }; + T position_delta[3] = { position2[0] - position1[0], position2[1] - position1[1], position2[2] - position1[2] }; T position_delta_rotated[3]; - ceres::QuaternionRotatePoint( - orientation1_inverse, - position_delta, - position_delta_rotated); + ceres::QuaternionRotatePoint(orientation1_inverse, position_delta, position_delta_rotated); // Compute the first three residual terms as (position_delta - b) residual[0] = position_delta_rotated[0] - T(b_[0]); diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.h index 3feea4b61..d06011234 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_2D_H_ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_2D_H_ -#warning \ - This header is obsolete, please include fuse_constraints/normal_prior_orientation_2d.hpp instead +#warning This header is obsolete, please include fuse_constraints/normal_prior_orientation_2d.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.hpp index c20723420..a870c59d9 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.hpp @@ -36,7 +36,6 @@ #include - namespace fuse_constraints { @@ -82,10 +81,7 @@ class NormalPriorOrientation2D : public ceres::SizedCostFunction<1, 1> * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - virtual bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const; + virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; private: double A_; //!< The residual weighting matrix, most likely the square root information matrix diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h index 6e7add49e..6ac0a9810 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h @@ -35,9 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_COST_FUNCTOR_H_ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include \ - fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp instead +#warning This header is obsolete, please include fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp index b2702ee2d..1ae1ad7c6 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp @@ -42,7 +42,6 @@ #include #include - namespace fuse_constraints { @@ -79,38 +78,22 @@ class NormalPriorOrientation3DCostFunctor * order (quaternion_x, quaternion_y, quaternion_z) * @param[in] b The orientation measurement or prior in order (w, x, y, z) */ - NormalPriorOrientation3DCostFunctor( - const fuse_core::Matrix3d & A, - const fuse_core::Vector4d & b) - : A_(A), - b_(b) + NormalPriorOrientation3DCostFunctor(const fuse_core::Matrix3d& A, const fuse_core::Vector4d& b) : A_(A), b_(b) { } /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. */ - template - bool operator()(const T * const orientation, T * residuals) const + template + bool operator()(const T* const orientation, T* residuals) const { using fuse_variables::Orientation3DStamped; // Compute the delta quaternion - T variable[4] = - { - orientation[0], - orientation[1], - orientation[2], - orientation[3] - }; + T variable[4] = { orientation[0], orientation[1], orientation[2], orientation[3] }; - T observation_inverse[4] = - { - T(b_(0)), - T(-b_(1)), - T(-b_(2)), - T(-b_(3)) - }; + T observation_inverse[4] = { T(b_(0)), T(-b_(1)), T(-b_(2)), T(-b_(3)) }; T difference[4]; ceres::QuaternionProduct(observation_inverse, variable, difference); diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h index 33d3ab943..5abe413fe 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h @@ -35,9 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_EULER_COST_FUNCTOR_H_ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_EULER_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include \ - fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp instead +#warning This header is obsolete, please include fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp index fd49f06c5..851d65a58 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_constraints { @@ -86,52 +85,48 @@ class NormalPriorOrientation3DEulerCostFunctor * @param[in] b The orientation measurement or prior. Its order must match the values in \p axes. * @param[in] axes The Euler angle axes for which we want to compute errors. Defaults to all axes. */ - NormalPriorOrientation3DEulerCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::VectorXd & b, - const std::vector & axes = {Euler::ROLL, Euler::PITCH, Euler::YAW}) : //NOLINT - A_(A), - b_(b), - axes_(axes) + NormalPriorOrientation3DEulerCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::VectorXd& b, + const std::vector& axes = { Euler::ROLL, Euler::PITCH, Euler::YAW }) + : // NOLINT + A_(A) + , b_(b) + , axes_(axes) { } /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. */ - template - bool operator()(const T * const orientation, T * residuals) const + template + bool operator()(const T* const orientation, T* residuals) const { using fuse_variables::Orientation3DStamped; - for (size_t i = 0; i < axes_.size(); ++i) { + for (size_t i = 0; i < axes_.size(); ++i) + { T angle; - switch (axes_[i]) { + switch (axes_[i]) + { case Euler::ROLL: - { - angle = fuse_core::getRoll( - orientation[0], orientation[1], orientation[2], - orientation[3]); - break; - } + { + angle = fuse_core::getRoll(orientation[0], orientation[1], orientation[2], orientation[3]); + break; + } case Euler::PITCH: - { - angle = - fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]); - break; - } + { + angle = fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]); + break; + } case Euler::YAW: - { - angle = - fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]); - break; - } + { + angle = fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]); + break; + } default: - { - throw std::runtime_error( - "The provided axis specified is unknown. " - "I should probably be more informative here"); - } + { + throw std::runtime_error("The provided axis specified is unknown. " + "I should probably be more informative here"); + } } residuals[i] = angle - T(b_[i]); } @@ -143,9 +138,9 @@ class NormalPriorOrientation3DEulerCostFunctor } private: - fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root - //!< information matrix - fuse_core::VectorXd b_; //!< The measured 3D orientation (quaternion) value + fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::VectorXd b_; //!< The measured 3D orientation (quaternion) value std::vector axes_; //!< The Euler angle axes that we're measuring }; diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.hpp index bb25137f2..c8f262460 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.hpp @@ -38,7 +38,6 @@ #include - namespace fuse_constraints { @@ -78,7 +77,7 @@ class NormalPriorPose2D : public ceres::SizedCostFunction * order (x, y, yaw) * @param[in] b The pose measurement or prior in order (x, y, yaw) */ - NormalPriorPose2D(const fuse_core::MatrixXd & A, const fuse_core::Vector3d & b); + NormalPriorPose2D(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b); /** * @brief Destructor @@ -89,10 +88,7 @@ class NormalPriorPose2D : public ceres::SizedCostFunction * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - virtual bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const; + virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; private: fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.h index d2760c63c..83fd97112 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_2D_COST_FUNCTOR_H_ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_2D_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include fuse_constraints/normal_prior_pose_2d_cost_functor.hpp \ +#warning This header is obsolete, please include fuse_constraints/normal_prior_pose_2d_cost_functor.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.hpp index 586a8146e..0d24bdb8f 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.hpp @@ -40,7 +40,6 @@ #include #include - namespace fuse_constraints { @@ -80,13 +79,13 @@ class NormalPriorPose2DCostFunctor * order (x, y, yaw) * @param[in] b The pose measurement or prior in order (x, y, yaw) */ - NormalPriorPose2DCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector3d & b); + NormalPriorPose2DCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. */ - template - bool operator()(const T * const position, const T * const orientation, T * residual) const; + template + bool operator()(const T* const position, const T* const orientation, T* residual) const; private: fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root @@ -94,18 +93,13 @@ class NormalPriorPose2DCostFunctor fuse_core::Vector3d b_; //!< The measured 2D pose value }; -NormalPriorPose2DCostFunctor::NormalPriorPose2DCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::Vector3d & b) -: A_(A), - b_(b) +NormalPriorPose2DCostFunctor::NormalPriorPose2DCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) + : A_(A), b_(b) { } -template -bool NormalPriorPose2DCostFunctor::operator()( - const T * const position, const T * const orientation, - T * residual) const +template +bool NormalPriorPose2DCostFunctor::operator()(const T* const position, const T* const orientation, T* residual) const { Eigen::Matrix full_residuals_vector; full_residuals_vector(0) = position[0] - T(b_(0)); diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h index 1645808cf..a302c592b 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_H_ #define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include fuse_constraints/normal_prior_pose_3d_cost_functor.hpp \ +#warning This header is obsolete, please include fuse_constraints/normal_prior_pose_3d_cost_functor.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp index f8d20f96f..75bcfce05 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.hpp @@ -41,7 +41,6 @@ #include #include - namespace fuse_constraints { @@ -79,13 +78,13 @@ class NormalPriorPose3DCostFunctor * order (x, y, z, qx, qy, qz) * @param[in] b The 3D pose measurement or prior in order (x, y, z, qw, qx, qy, qz) */ - NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); + NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. */ - template - bool operator()(const T * const position, const T * const orientation, T * residual) const; + template + bool operator()(const T* const position, const T* const orientation, T* residual) const; private: fuse_core::Matrix6d A_; @@ -94,19 +93,13 @@ class NormalPriorPose3DCostFunctor NormalPriorOrientation3DCostFunctor orientation_functor_; }; -NormalPriorPose3DCostFunctor::NormalPriorPose3DCostFunctor( - const fuse_core::Matrix6d & A, - const fuse_core::Vector7d & b) -: A_(A), - b_(b), - orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Delta will not be scaled +NormalPriorPose3DCostFunctor::NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b) + : A_(A), b_(b), orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Delta will not be scaled { } -template -bool NormalPriorPose3DCostFunctor::operator()( - const T * const position, const T * const orientation, - T * residual) const +template +bool NormalPriorPose3DCostFunctor::operator()(const T* const position, const T* const orientation, T* residual) const { // Compute the position error residual[0] = position[0] - T(b_(0)); diff --git a/fuse_constraints/include/fuse_constraints/relative_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_constraint.hpp index 019a94e42..dda5d5a95 100644 --- a/fuse_constraints/include/fuse_constraints/relative_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_constraint.hpp @@ -57,7 +57,6 @@ #include #include - namespace fuse_constraints { @@ -73,7 +72,7 @@ namespace fuse_constraints * is not the correct operation for a specific variable type (e.g. 3D rotations), a custom * constraint or template specialization will be needed. */ -template +template class RelativeConstraint : public fuse_core::Constraint { public: @@ -93,12 +92,8 @@ class RelativeConstraint : public fuse_core::Constraint * @param[in] delta The measured change between variable1 and variable2 * @param[in] covariance The measurement uncertainty */ - RelativeConstraint( - const std::string & source, - const Variable & variable1, - const Variable & variable2, - const fuse_core::VectorXd & delta, - const fuse_core::MatrixXd & covariance); + RelativeConstraint(const std::string& source, const Variable& variable1, const Variable& variable2, + const fuse_core::VectorXd& delta, const fuse_core::MatrixXd& covariance); /** * @brief Constructor @@ -116,13 +111,9 @@ class RelativeConstraint : public fuse_core::Constraint * order defined by \p indices. * @param[in] indices The set of indices corresponding to the measured dimensions */ - RelativeConstraint( - const std::string & source, - const Variable & variable1, - const Variable & variable2, - const fuse_core::VectorXd & delta, - const fuse_core::MatrixXd & covariance, - const std::vector & indices); + RelativeConstraint(const std::string& source, const Variable& variable1, const Variable& variable2, + const fuse_core::VectorXd& delta, const fuse_core::MatrixXd& covariance, + const std::vector& indices); /** * @brief Destructor @@ -136,7 +127,10 @@ class RelativeConstraint : public fuse_core::Constraint * are in the order defined by the variable, not the order defined by the \p indices parameter. * All unmeasured variable dimensions are set to zero. */ - const fuse_core::VectorXd & delta() const {return delta_;} + const fuse_core::VectorXd& delta() const + { + return delta_; + } /** * @brief Read-only access to the square root information matrix. @@ -146,7 +140,10 @@ class RelativeConstraint : public fuse_core::Constraint * variable_dimensions. If only a partial set of dimensions are measured, then this matrix will * not be square. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -163,7 +160,7 @@ class RelativeConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -175,10 +172,10 @@ class RelativeConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::VectorXd delta_; //!< The measured change between the two variables + fuse_core::VectorXd delta_; //!< The measured change between the two variables fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix private: @@ -192,28 +189,23 @@ class RelativeConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & delta_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& delta_; + archive& sqrt_information_; } }; // Define unique names for the different variations of the absolute constraint -using RelativeAccelerationAngular2DStampedConstraint = - RelativeConstraint; -using RelativeAccelerationLinear2DStampedConstraint = - RelativeConstraint; -using RelativeOrientation2DStampedConstraint = - RelativeConstraint; +using RelativeAccelerationAngular2DStampedConstraint = RelativeConstraint; +using RelativeAccelerationLinear2DStampedConstraint = RelativeConstraint; +using RelativeOrientation2DStampedConstraint = RelativeConstraint; using RelativePosition2DStampedConstraint = RelativeConstraint; using RelativePosition3DStampedConstraint = RelativeConstraint; -using RelativeVelocityAngular2DStampedConstraint = - RelativeConstraint; -using RelativeVelocityLinear2DStampedConstraint = - RelativeConstraint; +using RelativeVelocityAngular2DStampedConstraint = RelativeConstraint; +using RelativeVelocityLinear2DStampedConstraint = RelativeConstraint; } // namespace fuse_constraints // Include the template implementation diff --git a/fuse_constraints/include/fuse_constraints/relative_constraint_impl.hpp b/fuse_constraints/include/fuse_constraints/relative_constraint_impl.hpp index 46f8596eb..b39b83535 100644 --- a/fuse_constraints/include/fuse_constraints/relative_constraint_impl.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_constraint_impl.hpp @@ -42,20 +42,17 @@ #include #include - namespace fuse_constraints { -template -RelativeConstraint::RelativeConstraint( - const std::string & source, - const Variable & variable1, - const Variable & variable2, - const fuse_core::VectorXd & delta, - const fuse_core::MatrixXd & covariance) -: fuse_core::Constraint(source, {variable1.uuid(), variable2.uuid()}), // NOLINT(whitespace/braces) - delta_(delta), - sqrt_information_(covariance.inverse().llt().matrixU()) +template +RelativeConstraint::RelativeConstraint(const std::string& source, const Variable& variable1, + const Variable& variable2, const fuse_core::VectorXd& delta, + const fuse_core::MatrixXd& covariance) + : fuse_core::Constraint(source, { variable1.uuid(), variable2.uuid() }) + , // NOLINT(whitespace/braces) + delta_(delta) + , sqrt_information_(covariance.inverse().llt().matrixU()) { assert(variable1.size() == variable2.size()); assert(delta.rows() == static_cast(variable1.size())); @@ -63,15 +60,12 @@ RelativeConstraint::RelativeConstraint( assert(covariance.cols() == static_cast(variable1.size())); } -template -RelativeConstraint::RelativeConstraint( - const std::string & source, - const Variable & variable1, - const Variable & variable2, - const fuse_core::VectorXd & partial_delta, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & indices) -: fuse_core::Constraint(source, {variable1.uuid(), variable2.uuid()}) // NOLINT(whitespace/braces) +template +RelativeConstraint::RelativeConstraint(const std::string& source, const Variable& variable1, + const Variable& variable2, const fuse_core::VectorXd& partial_delta, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& indices) + : fuse_core::Constraint(source, { variable1.uuid(), variable2.uuid() }) // NOLINT(whitespace/braces) { assert(variable1.size() == variable2.size()); assert(partial_delta.rows() == static_cast(indices.size())); @@ -90,13 +84,14 @@ RelativeConstraint::RelativeConstraint( // dimensions, and the columns are in the order defined by the variable. delta_ = fuse_core::VectorXd::Zero(variable1.size()); sqrt_information_ = fuse_core::MatrixXd::Zero(indices.size(), variable1.size()); - for (size_t i = 0; i < indices.size(); ++i) { + for (size_t i = 0; i < indices.size(); ++i) + { delta_(indices[i]) = partial_delta(i); sqrt_information_.col(indices[i]) = partial_sqrt_information.col(i); } } -template +template fuse_core::MatrixXd RelativeConstraint::covariance() const { // We want to compute: @@ -111,8 +106,8 @@ fuse_core::MatrixXd RelativeConstraint::covariance() const return pinv * pinv.transpose(); } -template -void RelativeConstraint::print(std::ostream & stream) const +template +void RelativeConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -122,68 +117,66 @@ void RelativeConstraint::print(std::ostream & stream) const << " delta: " << delta().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -template -ceres::CostFunction * RelativeConstraint::costFunction() const +template +ceres::CostFunction* RelativeConstraint::costFunction() const { // Create a Gaussian/Normal Delta constraint return new fuse_constraints::NormalDelta(sqrt_information_, delta_); } // Specialization for Orientation2D -template<> -inline ceres::CostFunction * RelativeConstraint< - fuse_variables::Orientation2DStamped ->::costFunction() -const +template <> +inline ceres::CostFunction* RelativeConstraint::costFunction() const { // Create a Gaussian/Normal Delta constraint return new NormalDeltaOrientation2D(sqrt_information_(0, 0), delta_(0)); } // Specialize the type() method to return the name that is registered with the plugins -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativeAccelerationAngular2DStampedConstraint"; } -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativeAccelerationLinear2DStampedConstraint"; } -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativeOrientation2DStampedConstraint"; } -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativePosition2DStampedConstraint"; } -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativePosition3DStampedConstraint"; } -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativeVelocityAngular2DStampedConstraint"; } -template<> +template <> inline std::string RelativeConstraint::type() const { return "fuse_constraints::RelativeVelocityLinear2DStampedConstraint"; diff --git a/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h index 361b147f1..58e3d3e9b 100644 --- a/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h +++ b/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h @@ -35,9 +35,7 @@ #ifndef FUSE_CONSTRAINTS__RELATIVE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__RELATIVE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include \ - fuse_constraints/relative_orientation_3d_stamped_constraint.hpp instead +#warning This header is obsolete, please include fuse_constraints/relative_orientation_3d_stamped_constraint.hpp instead #include diff --git a/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.hpp index 6d68d424c..ba5c32f8f 100644 --- a/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.hpp @@ -53,7 +53,6 @@ #include #include - namespace fuse_constraints { @@ -83,12 +82,10 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * (4x1 vector: w, x, y, z) * @param[in] covariance The measurement covariance (3x3 matrix: qx, qy, qz) */ - RelativeOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector4d & delta, - const fuse_core::Matrix3d & covariance); + RelativeOrientation3DStampedConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_core::Vector4d& delta, const fuse_core::Matrix3d& covariance); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -99,12 +96,10 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] delta The measured orientation change as an Eigen quaternion * @param[in] covariance The measurement covariance (3x3 matrix: qx, qy, qz) */ - RelativeOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Orientation3DStamped & orientation2, - const Eigen::Quaterniond & delta, - const fuse_core::Matrix3d & covariance); + RelativeOrientation3DStampedConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Orientation3DStamped& orientation2, + const Eigen::Quaterniond& delta, const fuse_core::Matrix3d& covariance); /** * @brief Create a constraint using a measurement/prior of a 3D orientation @@ -115,12 +110,11 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] delta The measured orientation change as a ROS quaternion message * @param[in] covariance The measurement covariance (3x3 matrix: qx, qy, qz) */ - RelativeOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Orientation3DStamped & orientation2, - const geometry_msgs::msg::Quaternion & delta, - const std::array & covariance); + RelativeOrientation3DStampedConstraint(const std::string& source, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Orientation3DStamped& orientation2, + const geometry_msgs::msg::Quaternion& delta, + const std::array& covariance); /** * @brief Destructor @@ -132,14 +126,20 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * * Order is (w, x, y, z) */ - const fuse_core::Vector4d & delta() const {return delta_;} + const fuse_core::Vector4d& delta() const + { + return delta_; + } /** * @brief Read-only access to the square root information matrix. * * Order is (x, y, z) */ - const fuse_core::Matrix3d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix3d& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -153,7 +153,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -165,7 +165,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: /** @@ -174,7 +174,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] quaternion The input Eigen quaternion * @return The \p quaternion, converted to an Eigen Vector4d */ - static fuse_core::Vector4d toEigen(const Eigen::Quaterniond & quaternion); + static fuse_core::Vector4d toEigen(const Eigen::Quaterniond& quaternion); /** * @brief Utility method to convert an ROS quaternion message to an Eigen Vector4d @@ -182,7 +182,7 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] quaternion The input ROS quaternion message * @return The \p quaternion, converted to an Eigen Vector4d */ - static fuse_core::Vector4d toEigen(const geometry_msgs::msg::Quaternion & quaternion); + static fuse_core::Vector4d toEigen(const geometry_msgs::msg::Quaternion& quaternion); /** * @brief Utility method to convert a flat 1D array to a 3x3 Eigen matrix @@ -190,9 +190,9 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in] covariance The input covariance array * @return The \p covariance, converted to an Eigen Matrix3d */ - static fuse_core::Matrix3d toEigen(const std::array & covariance); + static fuse_core::Matrix3d toEigen(const std::array& covariance); - fuse_core::Vector4d delta_; //!< The measured/prior mean vector for this variable + fuse_core::Vector4d delta_; //!< The measured/prior mean vector for this variable fuse_core::Matrix3d sqrt_information_; //!< The square root information matrix private: @@ -206,12 +206,12 @@ class RelativeOrientation3DStampedConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & delta_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& delta_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.h index ddb8d837d..267ed2a15 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.h +++ b/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include fuse_constraints/relative_pose_2d_stamped_constraint.hpp \ +#warning This header is obsolete, please include fuse_constraints/relative_pose_2d_stamped_constraint.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.hpp index 44acf48f9..54113de52 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_constraints { @@ -63,7 +62,7 @@ namespace fuse_constraints * measurements (wheel encoders, inertial strap-down, visual odometry) measure the change in the * pose, not the pose directly. This constraint holds the measured 2D pose change and the * measurement uncertainty/covariance. It also permits measurement of a subset of the relative pose - * provided in the position and orientation varables. + * provided in the position and orientation variables. */ class RelativePose2DStampedConstraint : public fuse_core::Constraint { @@ -106,16 +105,13 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * dimensions e.g., "{fuse_variables::Orientation2DStamped::Yaw}" */ RelativePose2DStampedConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & position1, - const fuse_variables::Orientation2DStamped & orientation1, - const fuse_variables::Position2DStamped & position2, - const fuse_variables::Orientation2DStamped & orientation2, - const fuse_core::VectorXd & partial_delta, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & linear_indices = - {fuse_variables::Position2DStamped::X, fuse_variables::Position2DStamped::Y}, // NOLINT - const std::vector & angular_indices = {fuse_variables::Orientation2DStamped::YAW}); // NOLINT + const std::string& source, const fuse_variables::Position2DStamped& position1, + const fuse_variables::Orientation2DStamped& orientation1, const fuse_variables::Position2DStamped& position2, + const fuse_variables::Orientation2DStamped& orientation2, const fuse_core::VectorXd& partial_delta, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& linear_indices = { fuse_variables::Position2DStamped::X, + fuse_variables::Position2DStamped::Y }, // NOLINT + const std::vector& angular_indices = { fuse_variables::Orientation2DStamped::YAW }); // NOLINT /** * @brief Destructor @@ -128,7 +124,10 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * Order is (dx, dy, dyaw). Note that the returned vector will be full sized (3x1) and in the * stated order. */ - const fuse_core::Vector3d & delta() const {return delta_;} + const fuse_core::Vector3d& delta() const + { + return delta_; + } /** * @brief Read-only access to the square root information matrix. @@ -136,7 +135,10 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * If only a partial covariance matrix was provided in the constructor, this covariance matrix * will not be square. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -152,7 +154,7 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Access the cost function for this constraint @@ -164,10 +166,10 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::Vector3d delta_; //!< The measured pose change (dx, dy, dyaw) + fuse_core::Vector3d delta_; //!< The measured pose change (dx, dy, dyaw) fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix (derived from the //!< covariance matrix) @@ -182,12 +184,12 @@ class RelativePose2DStampedConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & delta_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& delta_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h index 96167dab2..a2dee13d6 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h @@ -35,8 +35,7 @@ #ifndef FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H_ #define FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include fuse_constraints/relative_pose_3d_stamped_constraint.hpp \ +#warning This header is obsolete, please include fuse_constraints/relative_pose_3d_stamped_constraint.hpp \ instead #include diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp index 21155fc4c..c0a5ff72e 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_constraints { @@ -85,14 +84,11 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint * (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz) * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, dqx, dqy, dqz) */ - RelativePose3DStampedConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector7d & delta, - const fuse_core::Matrix6d & covariance); + RelativePose3DStampedConstraint(const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_core::Vector7d& delta, const fuse_core::Matrix6d& covariance); /** * @brief Destructor @@ -102,12 +98,18 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint /** * @brief Read-only access to the measured pose change. */ - const fuse_core::Vector7d & delta() const {return delta_;} + const fuse_core::Vector7d& delta() const + { + return delta_; + } /** * @brief Read-only access to the square root information matrix. */ - const fuse_core::Matrix6d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix6d& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -122,7 +124,7 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Access the cost function for this constraint @@ -134,10 +136,10 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::Vector7d delta_; //!< The measured pose change (dx, dy, dz, dqw, dqx, dqy, dqz) + fuse_core::Vector7d delta_; //!< The measured pose change (dx, dy, dz, dqw, dqx, dqy, dqz) fuse_core::Matrix6d sqrt_information_; //!< The square root information matrix (derived from the //!< covariance matrix) @@ -152,12 +154,12 @@ class RelativePose3DStampedConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & delta_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& delta_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/uuid_ordering.hpp b/fuse_constraints/include/fuse_constraints/uuid_ordering.hpp old mode 100755 new mode 100644 index 401d8db06..1140c3884 --- a/fuse_constraints/include/fuse_constraints/uuid_ordering.hpp +++ b/fuse_constraints/include/fuse_constraints/uuid_ordering.hpp @@ -40,7 +40,6 @@ #include #include - namespace fuse_constraints { @@ -86,7 +85,7 @@ class UuidOrdering * @param[in] first Iterator pointing to the first UUID to add to the ordering * @param[in] last Iterator pointing to one past the last UUID to add to the ordering */ - template + template UuidOrdering(UuidConstIterator first, UuidConstIterator last); /** @@ -109,7 +108,7 @@ class UuidOrdering /** * @brief Return true if the UUID exists in the ordering */ - bool exists(const fuse_core::UUID & uuid) const; + bool exists(const fuse_core::UUID& uuid) const; /** * @brief Add a new UUID to the back of the ordering @@ -119,46 +118,47 @@ class UuidOrdering * @param[in] uuid The UUID to insert * @return True if the UUID was inserted, false if the UUID already existed */ - bool push_back(const fuse_core::UUID & uuid); + bool push_back(const fuse_core::UUID& uuid); /** * @brief Access the UUID stored at the provided index * * Accessing an index that does not exist results in undefined behavior */ - const fuse_core::UUID & operator[](const unsigned int index) const; + const fuse_core::UUID& operator[](const unsigned int index) const; /** * @brief Access the index associated with the provided UUID * * Accessing a UUID that does not exist results in the provided UUID being added to the ordering */ - unsigned int operator[](const fuse_core::UUID & uuid); + unsigned int operator[](const fuse_core::UUID& uuid); /** * @brief Access the UUID stored at the provided index * * If the requested index does not exist, an out_of_range exception will be thrown. */ - const fuse_core::UUID & at(const unsigned int index) const; + const fuse_core::UUID& at(const unsigned int index) const; /** * @brief Access the index associated with the provided UUID * * If the requested UUID does not exist, an out_of_range exception will be thrown. */ - unsigned int at(const fuse_core::UUID & uuid) const; + unsigned int at(const fuse_core::UUID& uuid) const; private: - using UuidOrderMapping = boost::bimaps::bimap, - boost::bimaps::unordered_set_of>; + using UuidOrderMapping = + boost::bimaps::bimap, boost::bimaps::unordered_set_of>; UuidOrderMapping order_; //!< Collection that contains the Index<-->UUID mapping }; -template +template UuidOrdering::UuidOrdering(UuidConstIterator first, UuidConstIterator last) { - for (; first != last; ++first) { + for (; first != last; ++first) + { order_.insert(order_.end(), UuidOrderMapping::value_type(order_.size(), *first)); } } diff --git a/fuse_constraints/include/fuse_constraints/variable_constraints.hpp b/fuse_constraints/include/fuse_constraints/variable_constraints.hpp old mode 100755 new mode 100644 index cd29f6c6c..c83d66708 --- a/fuse_constraints/include/fuse_constraints/variable_constraints.hpp +++ b/fuse_constraints/include/fuse_constraints/variable_constraints.hpp @@ -41,7 +41,6 @@ #include #include - namespace fuse_constraints { @@ -90,10 +89,8 @@ class VariableConstraints /** * @brief Add this constraint to all variables in the provided range */ - template - void insert( - const unsigned int constraint, VariableIndexIterator first, - VariableIndexIterator last); + template + void insert(const unsigned int constraint, VariableIndexIterator first, VariableIndexIterator last); /** * @brief Add a single orphan variable, i.e. a variable without constraints @@ -106,7 +103,7 @@ class VariableConstraints * * Accessing a variable id that is not part of this container results in undefined behavior */ - template + template OutputIterator getConstraints(const unsigned int variable_id, OutputIterator result) const; /** @@ -114,7 +111,7 @@ class VariableConstraints * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const; + void print(std::ostream& stream = std::cout) const; private: using ConstraintCollection = std::unordered_set; @@ -123,29 +120,26 @@ class VariableConstraints ConstraintsByVariable variable_constraints_; //!< The collection of constraints for each variable }; -template -void VariableConstraints::insert( - const unsigned int constraint, VariableIndexIterator first, - VariableIndexIterator last) +template +void VariableConstraints::insert(const unsigned int constraint, VariableIndexIterator first, VariableIndexIterator last) { - for (; first != last; ++first) { + for (; first != last; ++first) + { insert(constraint, *first); } } -template -OutputIterator VariableConstraints::getConstraints( - const unsigned int variable_id, - OutputIterator result) const +template +OutputIterator VariableConstraints::getConstraints(const unsigned int variable_id, OutputIterator result) const { - const auto & constraints = variable_constraints_[variable_id]; + const auto& constraints = variable_constraints_[variable_id]; return std::copy(std::begin(constraints), std::end(constraints), result); } /** * Stream operator for printing VariableConstraints objects. */ -std::ostream & operator<<(std::ostream & stream, const VariableConstraints & variable_constraints); +std::ostream& operator<<(std::ostream& stream, const VariableConstraints& variable_constraints); } // namespace fuse_constraints diff --git a/fuse_constraints/package.xml b/fuse_constraints/package.xml index bc7f9c847..99b1fb5b6 100644 --- a/fuse_constraints/package.xml +++ b/fuse_constraints/package.xml @@ -35,8 +35,6 @@ rclcpp ament_cmake_gtest - ament_lint_auto - ament_lint_common benchmark diff --git a/fuse_constraints/src/absolute_constraint.cpp b/fuse_constraints/src/absolute_constraint.cpp index 9aa7593fc..b30a4b77b 100644 --- a/fuse_constraints/src/absolute_constraint.cpp +++ b/fuse_constraints/src/absolute_constraint.cpp @@ -43,24 +43,10 @@ BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePosition3DStampedConstrai BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteOrientation2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsolutePosition2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsolutePosition3DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint, - fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteOrientation2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePosition2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePosition3DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp index 0131bfd20..2091e8527 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp @@ -45,31 +45,26 @@ namespace fuse_constraints { AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector4d & mean, - const fuse_core::Matrix3d & covariance) -: fuse_core::Constraint(source, {orientation.uuid()}), // NOLINT(whitespace/braces) - mean_(mean), - sqrt_information_(covariance.inverse().llt().matrixU()) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation, const fuse_core::Vector4d& mean, + const fuse_core::Matrix3d& covariance) + : fuse_core::Constraint(source, { orientation.uuid() }) + , // NOLINT(whitespace/braces) + mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const Eigen::Quaterniond & mean, - const fuse_core::Matrix3d & covariance) -: AbsoluteOrientation3DStampedConstraint(source, orientation, toEigen(mean), covariance) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation, const Eigen::Quaterniond& mean, + const fuse_core::Matrix3d& covariance) + : AbsoluteOrientation3DStampedConstraint(source, orientation, toEigen(mean), covariance) { } AbsoluteOrientation3DStampedConstraint::AbsoluteOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const geometry_msgs::msg::Quaternion & mean, - const std::array & covariance) -: AbsoluteOrientation3DStampedConstraint(source, orientation, toEigen(mean), toEigen(covariance)) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation, + const geometry_msgs::msg::Quaternion& mean, const std::array& covariance) + : AbsoluteOrientation3DStampedConstraint(source, orientation, toEigen(mean), toEigen(covariance)) { } @@ -78,7 +73,7 @@ fuse_core::Matrix3d AbsoluteOrientation3DStampedConstraint::covariance() const return (sqrt_information_.transpose() * sqrt_information_).inverse(); } -void AbsoluteOrientation3DStampedConstraint::print(std::ostream & stream) const +void AbsoluteOrientation3DStampedConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -87,37 +82,34 @@ void AbsoluteOrientation3DStampedConstraint::print(std::ostream & stream) const << " mean: " << mean().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * AbsoluteOrientation3DStampedConstraint::costFunction() const +ceres::CostFunction* AbsoluteOrientation3DStampedConstraint::costFunction() const { return new ceres::AutoDiffCostFunction( - new NormalPriorOrientation3DCostFunctor(sqrt_information_, mean_)); + new NormalPriorOrientation3DCostFunctor(sqrt_information_, mean_)); } -fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen( - const Eigen::Quaterniond & quaternion) +fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen(const Eigen::Quaterniond& quaternion) { fuse_core::Vector4d eigen_quaternion_vector; eigen_quaternion_vector << quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z(); return eigen_quaternion_vector; } -fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen( - const geometry_msgs::msg::Quaternion & quaternion) +fuse_core::Vector4d AbsoluteOrientation3DStampedConstraint::toEigen(const geometry_msgs::msg::Quaternion& quaternion) { fuse_core::Vector4d eigen_quaternion_vector; eigen_quaternion_vector << quaternion.w, quaternion.x, quaternion.y, quaternion.z; return eigen_quaternion_vector; } -fuse_core::Matrix3d AbsoluteOrientation3DStampedConstraint::toEigen( - const std::array & covariance) +fuse_core::Matrix3d AbsoluteOrientation3DStampedConstraint::toEigen(const std::array& covariance) { return fuse_core::Matrix3d(covariance.data()); } @@ -125,6 +117,4 @@ fuse_core::Matrix3d AbsoluteOrientation3DStampedConstraint::toEigen( } // namespace fuse_constraints BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteOrientation3DStampedConstraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteOrientation3DStampedConstraint, - fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteOrientation3DStampedConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp index d9fb6dae5..29ba0d652 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp @@ -46,15 +46,13 @@ namespace fuse_constraints { AbsoluteOrientation3DStampedEulerConstraint::AbsoluteOrientation3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::VectorXd & mean, - const fuse_core::MatrixXd & covariance, - const std::vector & axes) -: fuse_core::Constraint(source, {orientation.uuid()}), // NOLINT(whitespace/braces) - mean_(mean), - sqrt_information_(covariance.inverse().llt().matrixU()), - axes_(axes) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation, const fuse_core::VectorXd& mean, + const fuse_core::MatrixXd& covariance, const std::vector& axes) + : fuse_core::Constraint(source, { orientation.uuid() }) + , // NOLINT(whitespace/braces) + mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) + , axes_(axes) { assert(covariance.rows() == static_cast(axes.size())); assert(covariance.cols() == static_cast(axes.size())); @@ -66,7 +64,7 @@ fuse_core::MatrixXd AbsoluteOrientation3DStampedEulerConstraint::covariance() co return (sqrt_information_.transpose() * sqrt_information_).inverse(); } -void AbsoluteOrientation3DStampedEulerConstraint::print(std::ostream & stream) const +void AbsoluteOrientation3DStampedEulerConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -75,22 +73,20 @@ void AbsoluteOrientation3DStampedEulerConstraint::print(std::ostream & stream) c << " mean: " << mean().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * AbsoluteOrientation3DStampedEulerConstraint::costFunction() const +ceres::CostFunction* AbsoluteOrientation3DStampedEulerConstraint::costFunction() const { - return new ceres::AutoDiffCostFunction( - new NormalPriorOrientation3DEulerCostFunctor(sqrt_information_, mean_, axes_), axes_.size()); + return new ceres::AutoDiffCostFunction( + new NormalPriorOrientation3DEulerCostFunctor(sqrt_information_, mean_, axes_), axes_.size()); } } // namespace fuse_constraints BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteOrientation3DStampedEulerConstraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteOrientation3DStampedEulerConstraint, - fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteOrientation3DStampedEulerConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp index 1b84d5f1b..23bd3c566 100644 --- a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp @@ -46,14 +46,11 @@ namespace fuse_constraints { AbsolutePose2DStampedConstraint::AbsolutePose2DStampedConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation, - const fuse_core::VectorXd & partial_mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & linear_indices, - const std::vector & angular_indices) -: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}) // NOLINT(whitespace/braces) + const std::string& source, const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation, const fuse_core::VectorXd& partial_mean, + const fuse_core::MatrixXd& partial_covariance, const std::vector& linear_indices, + const std::vector& angular_indices) + : fuse_core::Constraint(source, { position.uuid(), orientation.uuid() }) // NOLINT(whitespace/braces) { size_t total_variable_size = position.size() + orientation.size(); size_t total_indices = linear_indices.size() + angular_indices.size(); @@ -75,12 +72,14 @@ AbsolutePose2DStampedConstraint::AbsolutePose2DStampedConstraint( // and the columns are in the order defined by the variable. mean_ = fuse_core::VectorXd::Zero(total_variable_size); sqrt_information_ = fuse_core::MatrixXd::Zero(total_indices, total_variable_size); - for (size_t i = 0; i < linear_indices.size(); ++i) { + for (size_t i = 0; i < linear_indices.size(); ++i) + { mean_(linear_indices[i]) = partial_mean(i); sqrt_information_.col(linear_indices[i]) = partial_sqrt_information.col(i); } - for (size_t i = linear_indices.size(); i < total_indices; ++i) { + for (size_t i = linear_indices.size(); i < total_indices; ++i) + { size_t final_index = position.size() + angular_indices[i - linear_indices.size()]; mean_(final_index) = partial_mean(i); sqrt_information_.col(final_index) = partial_sqrt_information.col(i); @@ -101,7 +100,7 @@ fuse_core::Matrix3d AbsolutePose2DStampedConstraint::covariance() const return pinv * pinv.transpose(); } -void AbsolutePose2DStampedConstraint::print(std::ostream & stream) const +void AbsolutePose2DStampedConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -111,13 +110,14 @@ void AbsolutePose2DStampedConstraint::print(std::ostream & stream) const << " mean: " << mean().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * AbsolutePose2DStampedConstraint::costFunction() const +ceres::CostFunction* AbsolutePose2DStampedConstraint::costFunction() const { return new NormalPriorPose2D(sqrt_information_, mean_); } diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index cc64e8d75..41d91c68f 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -44,19 +44,19 @@ namespace fuse_constraints { -AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector7d & mean, - const fuse_core::Matrix6d & covariance) -: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT - mean_(mean), - sqrt_information_(covariance.inverse().llt().matrixU()) +AbsolutePose3DStampedConstraint::AbsolutePose3DStampedConstraint(const std::string& source, + const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::Vector7d& mean, + const fuse_core::Matrix6d& covariance) + : fuse_core::Constraint(source, { position.uuid(), orientation.uuid() }) + , // NOLINT + mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } -void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const +void AbsolutePose3DStampedConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -66,16 +66,17 @@ void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const << " mean: " << mean().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * AbsolutePose3DStampedConstraint::costFunction() const +ceres::CostFunction* AbsolutePose3DStampedConstraint::costFunction() const { return new ceres::AutoDiffCostFunction( - new NormalPriorPose3DCostFunctor(sqrt_information_, mean_)); + new NormalPriorPose3DCostFunctor(sqrt_information_, mean_)); } } // namespace fuse_constraints diff --git a/fuse_constraints/src/marginal_constraint.cpp b/fuse_constraints/src/marginal_constraint.cpp index 0ee0e937e..a04225933 100644 --- a/fuse_constraints/src/marginal_constraint.cpp +++ b/fuse_constraints/src/marginal_constraint.cpp @@ -45,29 +45,34 @@ namespace fuse_constraints { -void MarginalConstraint::print(std::ostream & stream) const +void MarginalConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" << " uuid: " << uuid() << "\n" << " variable:\n"; - for (const auto & variable : variables()) { + for (const auto& variable : variables()) + { stream << " - " << variable << "\n"; } Eigen::IOFormat indent(4, 0, ", ", "\n", " [", "]"); - for (size_t i = 0; i < A().size(); ++i) { - stream << " A[" << i << "]:\n" << A()[i].format(indent) << "\n" - << " x_bar[" << i << "]:\n" << x_bar()[i].format(indent) << "\n"; + for (size_t i = 0; i < A().size(); ++i) + { + stream << " A[" << i << "]:\n" + << A()[i].format(indent) << "\n" + << " x_bar[" << i << "]:\n" + << x_bar()[i].format(indent) << "\n"; } stream << " b:\n" << b().format(indent) << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * MarginalConstraint::costFunction() const +ceres::CostFunction* MarginalConstraint::costFunction() const { #if !CERES_SUPPORTS_MANIFOLDS return new MarginalCostFunction(A_, b_, x_bar_, local_parameterizations_); diff --git a/fuse_constraints/src/marginal_cost_function.cpp b/fuse_constraints/src/marginal_cost_function.cpp index d148a5043..34274cca3 100644 --- a/fuse_constraints/src/marginal_cost_function.cpp +++ b/fuse_constraints/src/marginal_cost_function.cpp @@ -46,57 +46,53 @@ namespace fuse_constraints { #if !CERES_SUPPORTS_MANIFOLDS MarginalCostFunction::MarginalCostFunction( - const std::vector & A, - const fuse_core::VectorXd & b, - const std::vector & x_bar, - const std::vector & local_parameterizations) -: A_(A), - b_(b), - local_parameterizations_(local_parameterizations), - x_bar_(x_bar) + const std::vector& A, const fuse_core::VectorXd& b, + const std::vector& x_bar, + const std::vector& local_parameterizations) + : A_(A), b_(b), local_parameterizations_(local_parameterizations), x_bar_(x_bar) { set_num_residuals(b_.rows()); - for (const auto & x_bar : x_bar_) { + for (const auto& x_bar : x_bar_) + { mutable_parameter_block_sizes()->push_back(x_bar.size()); } } #else -MarginalCostFunction::MarginalCostFunction( - const std::vector & A, - const fuse_core::VectorXd & b, - const std::vector & x_bar, - const std::vector & manifolds) -: A_(A), - b_(b), - manifolds_(manifolds), - x_bar_(x_bar) +MarginalCostFunction::MarginalCostFunction(const std::vector& A, const fuse_core::VectorXd& b, + const std::vector& x_bar, + const std::vector& manifolds) + : A_(A), b_(b), manifolds_(manifolds), x_bar_(x_bar) { set_num_residuals(b_.rows()); - for (const auto & x_bar : x_bar_) { + for (const auto& x_bar : x_bar_) + { mutable_parameter_block_sizes()->push_back(x_bar.size()); } } #endif -bool MarginalCostFunction::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool MarginalCostFunction::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // Compute cost Eigen::Map residuals_map(residuals, num_residuals()); residuals_map = b_; - for (size_t i = 0; i < A_.size(); ++i) { + for (size_t i = 0; i < A_.size(); ++i) + { fuse_core::VectorXd delta(A_[i].cols()); #if !CERES_SUPPORTS_MANIFOLDS - if (local_parameterizations_[i]) { + if (local_parameterizations_[i]) + { local_parameterizations_[i]->Minus(x_bar_[i].data(), parameters[i], delta.data()); #else - if (manifolds_[i]) { + if (manifolds_[i]) + { manifolds_[i]->Minus(parameters[i], x_bar_[i].data(), delta.data()); #endif - } else { - for (int j = 0; j < x_bar_[i].rows(); ++j) { + } + else + { + for (int j = 0; j < x_bar_[i].rows(); ++j) + { delta[j] = parameters[i][j] - x_bar_[i][j]; } } @@ -104,29 +100,30 @@ bool MarginalCostFunction::Evaluate( } // Compute requested Jacobians - if (jacobians) { - for (size_t i = 0; i < A_.size(); ++i) { - if (jacobians[i]) { + if (jacobians) + { + for (size_t i = 0; i < A_.size(); ++i) + { + if (jacobians[i]) + { #if !CERES_SUPPORTS_MANIFOLDS - if (local_parameterizations_[i]) { - const auto & local_parameterization = local_parameterizations_[i]; - fuse_core::MatrixXd J_local(local_parameterization->LocalSize(), - local_parameterization->GlobalSize()); + if (local_parameterizations_[i]) + { + const auto& local_parameterization = local_parameterizations_[i]; + fuse_core::MatrixXd J_local(local_parameterization->LocalSize(), local_parameterization->GlobalSize()); local_parameterization->ComputeMinusJacobian(parameters[i], J_local.data()); #else - if (manifolds_[i]) { - const auto & manifold = manifolds_[i]; - fuse_core::MatrixXd J_local(manifold->TangentSize(), - manifold->AmbientSize()); + if (manifolds_[i]) + { + const auto& manifold = manifolds_[i]; + fuse_core::MatrixXd J_local(manifold->TangentSize(), manifold->AmbientSize()); manifold->MinusJacobian(parameters[i], J_local.data()); #endif - Eigen::Map( - jacobians[i], num_residuals(), - parameter_block_sizes()[i]) = A_[i] * J_local; - } else { - Eigen::Map( - jacobians[i], num_residuals(), - parameter_block_sizes()[i]) = A_[i]; + Eigen::Map(jacobians[i], num_residuals(), parameter_block_sizes()[i]) = A_[i] * J_local; + } + else + { + Eigen::Map(jacobians[i], num_residuals(), parameter_block_sizes()[i]) = A_[i]; } } } diff --git a/fuse_constraints/src/marginalize_variables.cpp b/fuse_constraints/src/marginalize_variables.cpp index 1614cfa5d..f3fa47a32 100644 --- a/fuse_constraints/src/marginalize_variables.cpp +++ b/fuse_constraints/src/marginalize_variables.cpp @@ -45,7 +45,7 @@ // The ROS 2 ament linter incorrectly recognizes Eigen includes as C instead of C++ #include #include -#include // NOLINT[build/include_order] +#include // NOLINT[build/include_order] #include // NOLINT[build/include_order] #include @@ -59,9 +59,8 @@ namespace fuse_constraints { -UuidOrdering computeEliminationOrder( - const std::vector & marginalized_variables, - const fuse_core::Graph & graph) +UuidOrdering computeEliminationOrder(const std::vector& marginalized_variables, + const fuse_core::Graph& graph) { // COLAMD wants a somewhat weird structure // Variables are numbered sequentially in some arbitrary order. We call this the "variable index" @@ -83,20 +82,26 @@ UuidOrdering computeEliminationOrder( auto variable_order = UuidOrdering(); auto constraint_order = UuidOrdering(); auto variable_constraints = VariableConstraints(); - for (const auto & variable_uuid : marginalized_variables) { + for (const auto& variable_uuid : marginalized_variables) + { // Get all connected constraints to this variable const auto constraints = graph.getConnectedConstraints(variable_uuid); // If the variable is orphan (it has no constraints), add it to the VariableConstraints object // without constraints New variable index is automatically generated - if (boost::empty(constraints)) { + if (boost::empty(constraints)) + { variable_constraints.insert(variable_order[variable_uuid]); - } else { + } + else + { // Add each constraint to the VariableConstraints object // New constraint and variable indices are automatically generated - for (const auto & constraint : constraints) { + for (const auto& constraint : constraints) + { unsigned int constraint_index = constraint_order[constraint.uuid()]; - for (const auto & constraint_variable_uuid : constraint.variables()) { + for (const auto& constraint_variable_uuid : constraint.variables()) + { variable_constraints.insert(constraint_index, variable_order[constraint_variable_uuid]); } } @@ -104,10 +109,8 @@ UuidOrdering computeEliminationOrder( } // Construct the CCOLAMD input structures - auto recommended_size = ccolamd_recommended( - variable_constraints.size(), - constraint_order.size(), - variable_order.size()); + auto recommended_size = + ccolamd_recommended(variable_constraints.size(), constraint_order.size(), variable_order.size()); auto A = std::vector(recommended_size); auto p = std::vector(variable_order.size() + 1); @@ -116,7 +119,8 @@ UuidOrdering computeEliminationOrder( auto p_iter = p.begin(); *p_iter = 0; ++p_iter; - for (unsigned int variable_index = 0u; variable_index < variable_order.size(); ++variable_index) { + 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); ++p_iter; @@ -125,7 +129,8 @@ UuidOrdering computeEliminationOrder( // Define the variable groups used by CCOLAMD. All of the marginalized variables should be group0, // all the rest should be group1. std::vector variable_groups(variable_order.size(), 1); // Default all variables to group1 - for (const auto & variable_uuid : marginalized_variables) { + for (const auto& variable_uuid : marginalized_variables) + { // Reassign the marginalized variables to group0 variable_groups[variable_order.at(variable_uuid)] = 0; } @@ -136,16 +141,10 @@ UuidOrdering computeEliminationOrder( 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(constraint_order.size(), variable_order.size(), recommended_size, A.data(), p.data(), knobs, + stats, variable_groups.data()); + if (!success) + { throw std::runtime_error("Failed to call CCOLAMD to generate the elimination order."); } @@ -153,30 +152,26 @@ UuidOrdering computeEliminationOrder( // CCOLAMD returns the elimination order by updating the values stored in p with the variable // index Remember that p is larger than variable_order.size() auto elimination_order = UuidOrdering(); - for (size_t i = 0ul; i < variable_order.size(); ++i) { + for (size_t i = 0ul; i < variable_order.size(); ++i) + { elimination_order.push_back(variable_order[p[i]]); } return elimination_order; } -fuse_core::Transaction marginalizeVariables( - const std::string & source, - const std::vector & marginalized_variables, - const fuse_core::Graph & graph) +fuse_core::Transaction marginalizeVariables(const std::string& source, + const std::vector& marginalized_variables, + const fuse_core::Graph& graph) { - return marginalizeVariables( - source, - marginalized_variables, - graph, - computeEliminationOrder(marginalized_variables, graph)); + return marginalizeVariables(source, marginalized_variables, graph, + computeEliminationOrder(marginalized_variables, graph)); } -fuse_core::Transaction marginalizeVariables( - const std::string & source, - const std::vector & marginalized_variables, - const fuse_core::Graph & graph, - const fuse_constraints::UuidOrdering & elimination_order) +fuse_core::Transaction marginalizeVariables(const std::string& source, + const std::vector& marginalized_variables, + const fuse_core::Graph& graph, + const fuse_constraints::UuidOrdering& elimination_order) { // TODO(swilliams) The method used to marginalize variables assumes that all variables are fully // constrained. However, with the introduction of "variables held constant", it is @@ -185,20 +180,17 @@ fuse_core::Transaction marginalizeVariables( // linearization and solve steps. A similar approach should be implemented here, // but that will require a major refactor. - assert( - std::all_of( - marginalized_variables.begin(), - marginalized_variables.end(), - [&elimination_order, &marginalized_variables](const fuse_core::UUID & variable_uuid) - { - return elimination_order.exists(variable_uuid) && - elimination_order.at(variable_uuid) < marginalized_variables.size(); - })); // NOLINT + assert(std::all_of(marginalized_variables.begin(), marginalized_variables.end(), + [&elimination_order, &marginalized_variables](const fuse_core::UUID& variable_uuid) { + return elimination_order.exists(variable_uuid) && + elimination_order.at(variable_uuid) < marginalized_variables.size(); + })); // NOLINT fuse_core::Transaction transaction; // Mark all of the marginalized variables for removal - for (const auto & variable_uuid : marginalized_variables) { + for (const auto& variable_uuid : marginalized_variables) + { transaction.removeVariable(variable_uuid); } @@ -208,13 +200,17 @@ fuse_core::Transaction marginalizeVariables( // Linearize all involved constraints, and store them with the variable where they will be used auto used_constraints = std::unordered_set(); std::vector> linear_terms(variable_order.size()); - for (size_t i = 0ul; i < marginalized_variables.size(); ++i) { + for (size_t i = 0ul; i < marginalized_variables.size(); ++i) + { const auto constraints = graph.getConnectedConstraints(variable_order[i]); - for (const auto & constraint : constraints) { - if (used_constraints.find(constraint.uuid()) == used_constraints.end()) { + for (const auto& constraint : constraints) + { + if (used_constraints.find(constraint.uuid()) == used_constraints.end()) + { used_constraints.insert(constraint.uuid()); // Ensure all connected variables are added to the ordering - for (const auto & variable_uuid : constraint.variables()) { + for (const auto& variable_uuid : constraint.variables()) + { variable_order.push_back(variable_uuid); } // Add the linearized constraint to the lowest-ordered connected variable @@ -233,20 +229,22 @@ fuse_core::Transaction marginalizeVariables( // Use the linearized constraints to marginalize each variable in order // Place the resulting marginal in the linear constraint bucket associated with the lowest-ordered // remaining variable - for (size_t i = 0ul; i < marginalized_variables.size(); ++i) { + for (size_t i = 0ul; i < marginalized_variables.size(); ++i) + { auto linear_marginal = detail::marginalizeNext(linear_terms[i]); - if (!linear_marginal.variables.empty()) { + if (!linear_marginal.variables.empty()) + { auto lowest_ordered_variable = linear_marginal.variables.front(); linear_terms[lowest_ordered_variable].push_back(std::move(linear_marginal)); } } // Convert all remaining linear marginals into marginal constraints - for (size_t i = marginalized_variables.size(); i < linear_terms.size(); ++i) { - for (const auto & linear_term : linear_terms[i]) { - auto marginal_constraint = detail::createMarginalConstraint( - source, linear_term, graph, - variable_order); + for (size_t i = marginalized_variables.size(); i < linear_terms.size(); ++i) + { + for (const auto& linear_term : linear_terms[i]) + { + auto marginal_constraint = detail::createMarginalConstraint(source, linear_term, graph, variable_order); transaction.addConstraint(std::move(marginal_constraint)); } } @@ -275,10 +273,8 @@ namespace detail * - https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/residual_block.cc * - https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/corrector.cc */ -LinearTerm linearize( - const fuse_core::Constraint & constraint, - const fuse_core::Graph & graph, - const UuidOrdering & elimination_order) +LinearTerm linearize(const fuse_core::Constraint& constraint, const fuse_core::Graph& graph, + const UuidOrdering& elimination_order) { LinearTerm result; @@ -290,16 +286,17 @@ LinearTerm linearize( // * Generate a vector of variable value pointers. This is needed for the Ceres API. // * Allocate a matrix for each jacobian block. We will have Ceres populate the matrix. // * Generate a vector of jacobian pointers. This is needed for the Ceres API. - const auto & variable_uuids = constraint.variables(); + const auto& variable_uuids = constraint.variables(); const size_t variable_count = variable_uuids.size(); - std::vector variable_values; + std::vector variable_values; variable_values.reserve(variable_count); - std::vector jacobians; + std::vector jacobians; jacobians.reserve(variable_count); result.variables.reserve(variable_count); result.A.reserve(variable_count); - for (const auto & variable_uuid : variable_uuids) { - const auto & variable = graph.getVariable(variable_uuid); + for (const auto& variable_uuid : variable_uuids) + { + 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())); @@ -311,15 +308,16 @@ LinearTerm linearize( 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(); } - if (!success) { - throw std::runtime_error( - "Error in evaluating the cost function. There are two possible reasons. " - "Either the CostFunction did not evaluate and fill all residual and jacobians " - "that were requested or there was a non-finite value (nan/infinite) generated " - "during the jacobian computation."); + if (!success) + { + throw std::runtime_error("Error in evaluating the cost function. There are two possible reasons. " + "Either the CostFunction did not evaluate and fill all residual and jacobians " + "that were requested or there was a non-finite value (nan/infinite) generated " + "during the jacobian computation."); } // Update the Jacobians with the local parameterizations. This potentially changes the size of the @@ -328,40 +326,50 @@ LinearTerm linearize( // local parameterization is applied. We also check for variables that have been marked as // constants. Since these variables cannot change value, their derivatives/Jacobians should be // zero. - for (size_t index = 0ul; index < variable_count; ++index) { - const auto & variable_uuid = variable_uuids[index]; - const auto & variable = graph.getVariable(variable_uuid); + for (size_t index = 0ul; index < variable_count; ++index) + { + const auto& variable_uuid = variable_uuids[index]; + const auto& variable = graph.getVariable(variable_uuid); #if !CERES_SUPPORTS_MANIFOLDS auto local_parameterization = variable.localParameterization(); - auto & jacobian = result.A[index]; - if (variable.holdConstant()) { - if (local_parameterization) { + auto& jacobian = result.A[index]; + if (variable.holdConstant()) + { + if (local_parameterization) + { jacobian.resize(Eigen::NoChange, local_parameterization->LocalSize()); } jacobian.setZero(); - } else if (local_parameterization) { - fuse_core::MatrixXd J(local_parameterization->GlobalSize(), - local_parameterization->LocalSize()); + } + else if (local_parameterization) + { + fuse_core::MatrixXd J(local_parameterization->GlobalSize(), local_parameterization->LocalSize()); local_parameterization->ComputeJacobian(variable_values[index], J.data()); jacobian *= J; } - if (local_parameterization) { + if (local_parameterization) + { delete local_parameterization; } #else auto manifold = variable.manifold(); - auto & jacobian = result.A[index]; - if (variable.holdConstant()) { - if (manifold) { + auto& jacobian = result.A[index]; + if (variable.holdConstant()) + { + if (manifold) + { jacobian.resize(Eigen::NoChange, manifold->TangentSize()); } jacobian.setZero(); - } else if (manifold) { + } + else if (manifold) + { fuse_core::MatrixXd J(manifold->AmbientSize(), manifold->TangentSize()); manifold->PlusJacobian(variable_values[index], J.data()); jacobian *= J; } - if (manifold) { + if (manifold) + { delete manifold; } #endif @@ -369,29 +377,35 @@ LinearTerm linearize( // Correct A and b for the effects of the loss function auto loss_function = constraint.lossFunction(); - if (loss_function) { + if (loss_function) + { double squared_norm = result.b.squaredNorm(); double rho[3]; loss_function->Evaluate(squared_norm, rho); - if (fuse_core::Loss::Ownership == ceres::Ownership::TAKE_OWNERSHIP) { + if (fuse_core::Loss::Ownership == ceres::Ownership::TAKE_OWNERSHIP) + { delete loss_function; } double sqrt_rho1 = std::sqrt(rho[1]); double alpha = 0.0; - if ((squared_norm > 0.0) && (rho[2] > 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); } // Correct the Jacobians - for (auto & jacobian : result.A) { - if (alpha == 0.0) { + for (auto& jacobian : result.A) + { + if (alpha == 0.0) + { jacobian *= sqrt_rho1; - } else { + } + else + { // TODO(swilliams) This may be inefficient, at least according to notes in the Ceres // codebase. - jacobian = sqrt_rho1 * - (jacobian - (alpha / squared_norm) * result.b * (result.b.transpose() * jacobian)); + jacobian = sqrt_rho1 * (jacobian - (alpha / squared_norm) * result.b * (result.b.transpose() * jacobian)); } } @@ -402,9 +416,10 @@ LinearTerm linearize( return result; } -LinearTerm marginalizeNext(const std::vector & linear_terms) +LinearTerm marginalizeNext(const std::vector& linear_terms) { - if (linear_terms.empty()) { + if (linear_terms.empty()) + { return {}; } @@ -415,19 +430,17 @@ LinearTerm marginalizeNext(const std::vector & linear_terms) // because the number of variables is assumed to be small. You need 1000s of variables before the // std::set outperforms the std::vector. auto dense_to_index = std::vector(); - for (const auto & linear_term : linear_terms) { - std::copy( - linear_term.variables.begin(), linear_term.variables.end(), - std::back_inserter(dense_to_index)); + for (const auto& linear_term : linear_terms) + { + std::copy(linear_term.variables.begin(), linear_term.variables.end(), std::back_inserter(dense_to_index)); } std::sort(dense_to_index.begin(), dense_to_index.end()); - dense_to_index.erase( - std::unique(dense_to_index.begin(), dense_to_index.end()), - dense_to_index.end()); + dense_to_index.erase(std::unique(dense_to_index.begin(), dense_to_index.end()), dense_to_index.end()); // Construct the inverse mapping auto index_to_dense = std::vector(dense_to_index.back() + 1, 0); - for (size_t dense = 0ul; dense < dense_to_index.size(); ++dense) { + for (size_t dense = 0ul; dense < dense_to_index.size(); ++dense) + { index_to_dense[dense_to_index[dense]] = dense; } @@ -435,14 +448,17 @@ LinearTerm marginalizeNext(const std::vector & linear_terms) auto row_offsets = std::vector(); row_offsets.reserve(linear_terms.size() + 1ul); row_offsets.push_back(0u); - for (const auto & linear_term : linear_terms) { + for (const auto& linear_term : linear_terms) + { row_offsets.push_back(row_offsets.back() + linear_term.b.rows()); } // Compute the column offsets auto index_to_cols = std::vector(dense_to_index.back() + 1u, 0u); - for (const auto & linear_term : linear_terms) { - for (size_t i = 0ul; i < linear_term.variables.size(); ++i) { + for (const auto& linear_term : linear_terms) + { + for (size_t i = 0ul; i < linear_term.variables.size(); ++i) + { auto index = linear_term.variables[i]; index_to_cols[index] = linear_term.A[i].cols(); } @@ -451,29 +467,34 @@ 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 (size_t dense = 0; dense < dense_to_index.size(); ++dense) + { column_offsets.push_back(column_offsets.back() + index_to_cols[dense_to_index[dense]]); } // Construct the Ab matrix - 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]; + 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]; + for (size_t i = 0ul; i < linear_term.variables.size(); ++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 col = 0; col < A.cols(); ++col) { + for (int row = 0; row < A.rows(); ++row) + { + for (int col = 0; col < A.cols(); ++col) + { Ab(row_offset + row, column_offset + col) = A(row, col); } } } - const auto & b = linear_term.b; + const auto& b = linear_term.b; int column_offset = column_offsets.back(); - for (int row = 0; row < b.rows(); ++row) { + for (int row = 0; row < b.rows(); ++row) + { Ab(row_offset + row, column_offset) = b(row); } } @@ -492,9 +513,7 @@ LinearTerm marginalizeNext(const std::vector & linear_terms) auto size = std::min(rows, cols); auto hCoeffs = HCoeffsType(size); auto temp = RowVectorType(cols); - Eigen::internal::householder_qr_inplace_blocked::run( - Ab, hCoeffs, 48, - temp.data()); + Eigen::internal::householder_qr_inplace_blocked::run(Ab, hCoeffs, 48, temp.data()); Ab.triangularView().setZero(); // Zero out the below-diagonal elements } @@ -506,18 +525,22 @@ LinearTerm marginalizeNext(const std::vector & linear_terms) 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) { + if (marginal_rows > 0) + { auto variable_count = dense_to_index.size() - 1; marginal_term.variables.reserve(variable_count); marginal_term.A.reserve(variable_count); // Skipping the marginalized variable - for (size_t dense = 1ul; dense < dense_to_index.size(); ++dense) { + for (size_t dense = 1ul; dense < dense_to_index.size(); ++dense) + { 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]); auto column_offset = column_offsets[dense]; - for (int row = 0; row < A.rows(); ++row) { - for (int col = 0; col < A.cols(); ++col) { + for (int row = 0; row < A.rows(); ++row) + { + for (int col = 0; col < A.cols(); ++col) + { A(row, col) = Ab(min_row + row, column_offset + col); } } @@ -525,32 +548,26 @@ LinearTerm marginalizeNext(const std::vector & linear_terms) } 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) { + for (int row = 0; row < marginal_term.b.rows(); ++row) + { marginal_term.b(row) = Ab(min_row + row, column_offset); } } return marginal_term; } -MarginalConstraint::SharedPtr createMarginalConstraint( - const std::string & source, - const LinearTerm & linear_term, - const fuse_core::Graph & graph, - const UuidOrdering & elimination_order) +MarginalConstraint::SharedPtr createMarginalConstraint(const std::string& source, const LinearTerm& linear_term, + const fuse_core::Graph& graph, + const UuidOrdering& elimination_order) { - auto index_to_variable = - [&graph, &elimination_order](const unsigned int index) -> const fuse_core::Variable & - { - return graph.getVariable(elimination_order.at(index)); - }; + auto index_to_variable = [&graph, &elimination_order](const unsigned int index) -> const fuse_core::Variable& { + return graph.getVariable(elimination_order.at(index)); + }; return MarginalConstraint::make_shared( - source, - boost::make_transform_iterator(linear_term.variables.begin(), index_to_variable), - boost::make_transform_iterator(linear_term.variables.end(), index_to_variable), - linear_term.A.begin(), - linear_term.A.end(), - linear_term.b); + source, boost::make_transform_iterator(linear_term.variables.begin(), index_to_variable), + boost::make_transform_iterator(linear_term.variables.end(), index_to_variable), linear_term.A.begin(), + linear_term.A.end(), linear_term.b); } } // namespace detail diff --git a/fuse_constraints/src/normal_delta.cpp b/fuse_constraints/src/normal_delta.cpp index 50b70b33c..469064043 100644 --- a/fuse_constraints/src/normal_delta.cpp +++ b/fuse_constraints/src/normal_delta.cpp @@ -39,9 +39,7 @@ namespace fuse_constraints { -NormalDelta::NormalDelta(const fuse_core::MatrixXd & A, const fuse_core::VectorXd & b) -: A_(A), - b_(b) +NormalDelta::NormalDelta(const fuse_core::MatrixXd& A, const fuse_core::VectorXd& b) : A_(A), b_(b) { CHECK_GT(b_.rows(), 0); CHECK_GT(A_.rows(), 0); @@ -51,25 +49,21 @@ NormalDelta::NormalDelta(const fuse_core::MatrixXd & A, const fuse_core::VectorX mutable_parameter_block_sizes()->push_back(b_.rows()); } -bool NormalDelta::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalDelta::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { Eigen::Map x0(parameters[0], parameter_block_sizes()[0]); Eigen::Map x1(parameters[1], parameter_block_sizes()[1]); Eigen::Map r(residuals, num_residuals()); r = A_ * (x1 - x0 - b_); - if (jacobians != nullptr) { - if (jacobians[0] != nullptr) { - Eigen::Map( - jacobians[0], num_residuals(), - parameter_block_sizes()[0]) = -A_; + if (jacobians != nullptr) + { + if (jacobians[0] != nullptr) + { + Eigen::Map(jacobians[0], num_residuals(), parameter_block_sizes()[0]) = -A_; } - if (jacobians[1] != nullptr) { - Eigen::Map( - jacobians[1], num_residuals(), - parameter_block_sizes()[1]) = A_; + if (jacobians[1] != nullptr) + { + Eigen::Map(jacobians[1], num_residuals(), parameter_block_sizes()[1]) = A_; } } return true; diff --git a/fuse_constraints/src/normal_delta_orientation_2d.cpp b/fuse_constraints/src/normal_delta_orientation_2d.cpp index b90625992..b78960b1e 100644 --- a/fuse_constraints/src/normal_delta_orientation_2d.cpp +++ b/fuse_constraints/src/normal_delta_orientation_2d.cpp @@ -37,27 +37,25 @@ namespace fuse_constraints { -NormalDeltaOrientation2D::NormalDeltaOrientation2D(const double A, const double b) -: A_(A), - b_(b) +NormalDeltaOrientation2D::NormalDeltaOrientation2D(const double A, const double b) : A_(A), b_(b) { } -bool NormalDeltaOrientation2D::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalDeltaOrientation2D::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // The following lines should read as // r = A_ * ((x1 - x0) - b_); // The wrap function handles the 2_pi wrap around issue with rotations double angle_diff = fuse_core::wrapAngle2D(parameters[1][0] - parameters[0][0] - b_); residuals[0] = A_ * angle_diff; - if (jacobians != nullptr) { - if (jacobians[0] != nullptr) { + if (jacobians != nullptr) + { + if (jacobians[0] != nullptr) + { jacobians[0][0] = -A_; } - if (jacobians[1] != nullptr) { + if (jacobians[1] != nullptr) + { jacobians[1][0] = A_; } } diff --git a/fuse_constraints/src/normal_delta_pose_2d.cpp b/fuse_constraints/src/normal_delta_pose_2d.cpp index 93677dbe4..66a0712e7 100644 --- a/fuse_constraints/src/normal_delta_pose_2d.cpp +++ b/fuse_constraints/src/normal_delta_pose_2d.cpp @@ -40,60 +40,53 @@ namespace fuse_constraints { -NormalDeltaPose2D::NormalDeltaPose2D(const fuse_core::MatrixXd & A, const fuse_core::Vector3d & b) -: A_(A), - b_(b) +NormalDeltaPose2D::NormalDeltaPose2D(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); CHECK_EQ(A_.cols(), 3); set_num_residuals(A_.rows()); } -bool NormalDeltaPose2D::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalDeltaPose2D::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { - const fuse_core::Matrix2d R1_transpose = - fuse_core::rotationMatrix2D(parameters[1][0]).transpose(); // orientation1 + const fuse_core::Matrix2d R1_transpose = fuse_core::rotationMatrix2D(parameters[1][0]).transpose(); // orientation1 const fuse_core::Vector2d position_delta = - R1_transpose * fuse_core::Vector2d( - parameters[2][0] - parameters[0][0], // position2.x - position1.x - parameters[2][1] - parameters[0][1]); // position2.y - position1.y + R1_transpose * fuse_core::Vector2d(parameters[2][0] - parameters[0][0], // position2.x - position1.x + parameters[2][1] - parameters[0][1]); // position2.y - position1.y - const fuse_core::Vector3d full_residuals_vector( - position_delta[0] - b_[0], position_delta[1] - b_[1], - // orientation2 - orientation1 - fuse_core::wrapAngle2D(parameters[3][0] - parameters[1][0] - b_[2])); + const fuse_core::Vector3d full_residuals_vector(position_delta[0] - b_[0], position_delta[1] - b_[1], + // orientation2 - orientation1 + fuse_core::wrapAngle2D(parameters[3][0] - parameters[1][0] - b_[2])); // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. Eigen::Map residuals_vector(residuals, num_residuals()); residuals_vector = A_ * full_residuals_vector; - if (jacobians != nullptr) { + if (jacobians != nullptr) + { // Jacobian wrt position1 - if (jacobians[0] != nullptr) { - Eigen::Map( - jacobians[0], num_residuals(), - 2) = -A_.leftCols<2>() * R1_transpose; + if (jacobians[0] != nullptr) + { + Eigen::Map(jacobians[0], num_residuals(), 2) = -A_.leftCols<2>() * R1_transpose; } // Jacobian wrt orientation1 - if (jacobians[1] != nullptr) { + if (jacobians[1] != nullptr) + { Eigen::Map(jacobians[1], num_residuals()) = - A_ * fuse_core::Vector3d(position_delta[1], -position_delta[0], -1); + A_ * fuse_core::Vector3d(position_delta[1], -position_delta[0], -1); } // Jacobian wrt position2 - if (jacobians[2] != nullptr) { - Eigen::Map( - jacobians[2], num_residuals(), - 2) = A_.leftCols<2>() * R1_transpose; + if (jacobians[2] != nullptr) + { + Eigen::Map(jacobians[2], num_residuals(), 2) = A_.leftCols<2>() * R1_transpose; } // Jacobian wrt orientation2 - if (jacobians[3] != nullptr) { + if (jacobians[3] != nullptr) + { Eigen::Map(jacobians[3], num_residuals()) = A_.col(2); } } diff --git a/fuse_constraints/src/normal_prior_orientation_2d.cpp b/fuse_constraints/src/normal_prior_orientation_2d.cpp index 6d5116bfb..faff1ad24 100644 --- a/fuse_constraints/src/normal_prior_orientation_2d.cpp +++ b/fuse_constraints/src/normal_prior_orientation_2d.cpp @@ -37,23 +37,19 @@ namespace fuse_constraints { -NormalPriorOrientation2D::NormalPriorOrientation2D(const double A, const double b) -: A_(A), - b_(b) +NormalPriorOrientation2D::NormalPriorOrientation2D(const double A, const double b) : A_(A), b_(b) { } -bool NormalPriorOrientation2D::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalPriorOrientation2D::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { // The following lines should read as // r = A_ * (x - b_); // The wrap function handles the 2_pi wrap around issue with rotations double angle_diff = fuse_core::wrapAngle2D(parameters[0][0] - b_); residuals[0] = A_ * angle_diff; - if ((jacobians != nullptr) && (jacobians[0] != nullptr)) { + if ((jacobians != nullptr) && (jacobians[0] != nullptr)) + { jacobians[0][0] = A_; } return true; diff --git a/fuse_constraints/src/normal_prior_pose_2d.cpp b/fuse_constraints/src/normal_prior_pose_2d.cpp index 32af6b37a..76de289f1 100644 --- a/fuse_constraints/src/normal_prior_pose_2d.cpp +++ b/fuse_constraints/src/normal_prior_pose_2d.cpp @@ -40,23 +40,18 @@ namespace fuse_constraints { -NormalPriorPose2D::NormalPriorPose2D(const fuse_core::MatrixXd & A, const fuse_core::Vector3d & b) -: A_(A), - b_(b) +NormalPriorPose2D::NormalPriorPose2D(const fuse_core::MatrixXd& A, const fuse_core::Vector3d& b) : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); CHECK_EQ(A_.cols(), 3); set_num_residuals(A_.rows()); } -bool NormalPriorPose2D::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalPriorPose2D::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { fuse_core::Vector3d full_residuals_vector; - full_residuals_vector[0] = parameters[0][0] - b_[0]; // position x - full_residuals_vector[1] = parameters[0][1] - b_[1]; // position y + full_residuals_vector[0] = parameters[0][0] - b_[0]; // position x + full_residuals_vector[1] = parameters[0][1] - b_[1]; // position y full_residuals_vector[2] = fuse_core::wrapAngle2D(parameters[1][0] - b_[2]); // orientation // Scale the residuals by the square root information matrix to account for the measurement @@ -64,14 +59,17 @@ bool NormalPriorPose2D::Evaluate( Eigen::Map residuals_vector(residuals, num_residuals()); residuals_vector = A_ * full_residuals_vector; - if (jacobians != nullptr) { + if (jacobians != nullptr) + { // Jacobian wrt position - if (jacobians[0] != nullptr) { + if (jacobians[0] != nullptr) + { Eigen::Map(jacobians[0], num_residuals(), 2) = A_.leftCols<2>(); } // Jacobian wrt orientation - if (jacobians[1] != nullptr) { + if (jacobians[1] != nullptr) + { Eigen::Map(jacobians[1], num_residuals()) = A_.col(2); } } diff --git a/fuse_constraints/src/relative_constraint.cpp b/fuse_constraints/src/relative_constraint.cpp index 4d55afb9c..ebfadfed5 100644 --- a/fuse_constraints/src/relative_constraint.cpp +++ b/fuse_constraints/src/relative_constraint.cpp @@ -43,24 +43,10 @@ BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativePosition3DStampedConstrai BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativeVelocityAngular2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativeVelocityLinear2DStampedConstraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativeAccelerationAngular2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativeAccelerationLinear2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativeOrientation2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativePosition2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativePosition3DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativeVelocityAngular2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativeVelocityLinear2DStampedConstraint, - fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeAccelerationAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeAccelerationLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeOrientation2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePosition2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePosition3DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeVelocityAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeVelocityLinear2DStampedConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp index c7cbf31bd..11891f3be 100644 --- a/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp @@ -45,36 +45,29 @@ namespace fuse_constraints { RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector4d & delta, - const fuse_core::Matrix3d & covariance) -: fuse_core::Constraint(source, {orientation1.uuid(), orientation2.uuid()}), // NOLINT - delta_(delta), - sqrt_information_(covariance.inverse().llt().matrixU()) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Orientation3DStamped& orientation2, const fuse_core::Vector4d& delta, + const fuse_core::Matrix3d& covariance) + : fuse_core::Constraint(source, { orientation1.uuid(), orientation2.uuid() }) + , // NOLINT + delta_(delta) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Orientation3DStamped & orientation2, - const Eigen::Quaterniond & delta, - const fuse_core::Matrix3d & covariance) -: RelativeOrientation3DStampedConstraint(source, orientation1, orientation2, toEigen( - delta), covariance) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Orientation3DStamped& orientation2, const Eigen::Quaterniond& delta, + const fuse_core::Matrix3d& covariance) + : RelativeOrientation3DStampedConstraint(source, orientation1, orientation2, toEigen(delta), covariance) { } RelativeOrientation3DStampedConstraint::RelativeOrientation3DStampedConstraint( - const std::string & source, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Orientation3DStamped & orientation2, - const geometry_msgs::msg::Quaternion & delta, - const std::array & covariance) -: RelativeOrientation3DStampedConstraint( - source, orientation1, orientation2, toEigen(delta), toEigen(covariance)) + const std::string& source, const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Orientation3DStamped& orientation2, const geometry_msgs::msg::Quaternion& delta, + const std::array& covariance) + : RelativeOrientation3DStampedConstraint(source, orientation1, orientation2, toEigen(delta), toEigen(covariance)) { } @@ -83,7 +76,7 @@ fuse_core::Matrix3d RelativeOrientation3DStampedConstraint::covariance() const return (sqrt_information_.transpose() * sqrt_information_).inverse(); } -void RelativeOrientation3DStampedConstraint::print(std::ostream & stream) const +void RelativeOrientation3DStampedConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -93,37 +86,34 @@ void RelativeOrientation3DStampedConstraint::print(std::ostream & stream) const << " delta: " << delta().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * RelativeOrientation3DStampedConstraint::costFunction() const +ceres::CostFunction* RelativeOrientation3DStampedConstraint::costFunction() const { return new ceres::AutoDiffCostFunction( - new NormalDeltaOrientation3DCostFunctor(sqrt_information_, delta_)); + new NormalDeltaOrientation3DCostFunctor(sqrt_information_, delta_)); } -fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen( - const Eigen::Quaterniond & quaternion) +fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen(const Eigen::Quaterniond& quaternion) { fuse_core::Vector4d eigen_quaternion_vector; eigen_quaternion_vector << quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z(); return eigen_quaternion_vector; } -fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen( - const geometry_msgs::msg::Quaternion & quaternion) +fuse_core::Vector4d RelativeOrientation3DStampedConstraint::toEigen(const geometry_msgs::msg::Quaternion& quaternion) { fuse_core::Vector4d eigen_quaternion_vector; eigen_quaternion_vector << quaternion.w, quaternion.x, quaternion.y, quaternion.z; return eigen_quaternion_vector; } -fuse_core::Matrix3d RelativeOrientation3DStampedConstraint::toEigen( - const std::array & covariance) +fuse_core::Matrix3d RelativeOrientation3DStampedConstraint::toEigen(const std::array& covariance) { return fuse_core::Matrix3d(covariance.data()); } @@ -131,6 +121,4 @@ fuse_core::Matrix3d RelativeOrientation3DStampedConstraint::toEigen( } // namespace fuse_constraints BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativeOrientation3DStampedConstraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::RelativeOrientation3DStampedConstraint, - fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeOrientation3DStampedConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp index 99150b987..dd968bee6 100644 --- a/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp @@ -45,18 +45,13 @@ namespace fuse_constraints { RelativePose2DStampedConstraint::RelativePose2DStampedConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & position1, - const fuse_variables::Orientation2DStamped & orientation1, - const fuse_variables::Position2DStamped & position2, - const fuse_variables::Orientation2DStamped & orientation2, - const fuse_core::VectorXd & partial_delta, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & linear_indices, - const std::vector & angular_indices) -: fuse_core::Constraint( - source, - {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}) // NOLINT + const std::string& source, const fuse_variables::Position2DStamped& position1, + const fuse_variables::Orientation2DStamped& orientation1, const fuse_variables::Position2DStamped& position2, + const fuse_variables::Orientation2DStamped& orientation2, const fuse_core::VectorXd& partial_delta, + const fuse_core::MatrixXd& partial_covariance, const std::vector& linear_indices, + const std::vector& angular_indices) + : fuse_core::Constraint(source, + { position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid() }) // NOLINT { size_t total_variable_size = position1.size() + orientation1.size(); size_t total_indices = linear_indices.size() + angular_indices.size(); @@ -80,12 +75,14 @@ RelativePose2DStampedConstraint::RelativePose2DStampedConstraint( delta_ = fuse_core::Vector3d::Zero(); sqrt_information_ = fuse_core::MatrixXd::Zero(total_indices, total_variable_size); - for (size_t i = 0; i < linear_indices.size(); ++i) { + for (size_t i = 0; i < linear_indices.size(); ++i) + { delta_(linear_indices[i]) = partial_delta(i); sqrt_information_.col(linear_indices[i]) = partial_sqrt_information.col(i); } - for (size_t i = linear_indices.size(); i < total_indices; ++i) { + for (size_t i = linear_indices.size(); i < total_indices; ++i) + { size_t final_index = position1.size() + angular_indices[i - linear_indices.size()]; delta_(final_index) = partial_delta(i); sqrt_information_.col(final_index) = partial_sqrt_information.col(i); @@ -106,7 +103,7 @@ fuse_core::Matrix3d RelativePose2DStampedConstraint::covariance() const return pinv * pinv.transpose(); } -void RelativePose2DStampedConstraint::print(std::ostream & stream) const +void RelativePose2DStampedConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -118,13 +115,14 @@ void RelativePose2DStampedConstraint::print(std::ostream & stream) const << " delta: " << delta().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * RelativePose2DStampedConstraint::costFunction() const +ceres::CostFunction* RelativePose2DStampedConstraint::costFunction() const { return new NormalDeltaPose2D(sqrt_information_, delta_); } diff --git a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp index 7d627e117..f4ca63a16 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp @@ -44,22 +44,18 @@ namespace fuse_constraints { RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector7d & delta, - const fuse_core::Matrix6d & covariance) -: fuse_core::Constraint( - source, - {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT - delta_(delta), - sqrt_information_(covariance.inverse().llt().matrixU()) + const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, const fuse_core::Vector7d& delta, + const fuse_core::Matrix6d& covariance) + : fuse_core::Constraint(source, { position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid() }) + , // NOLINT + delta_(delta) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } -void RelativePose3DStampedConstraint::print(std::ostream & stream) const +void RelativePose3DStampedConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -72,10 +68,10 @@ void RelativePose3DStampedConstraint::print(std::ostream & stream) const << " sqrt_info: " << sqrtInformation() << "\n"; } -ceres::CostFunction * RelativePose3DStampedConstraint::costFunction() const +ceres::CostFunction* RelativePose3DStampedConstraint::costFunction() const { return new ceres::AutoDiffCostFunction( - new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_)); + new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_)); } } // namespace fuse_constraints diff --git a/fuse_constraints/src/uuid_ordering.cpp b/fuse_constraints/src/uuid_ordering.cpp old mode 100755 new mode 100644 index 0608d17ff..baa9d89c7 --- a/fuse_constraints/src/uuid_ordering.cpp +++ b/fuse_constraints/src/uuid_ordering.cpp @@ -37,7 +37,7 @@ namespace fuse_constraints { UuidOrdering::UuidOrdering(std::initializer_list uuid_list) -: UuidOrdering(uuid_list.begin(), uuid_list.end()) + : UuidOrdering(uuid_list.begin(), uuid_list.end()) { } @@ -56,34 +56,34 @@ bool UuidOrdering::exists(const unsigned int index) const return index < order_.size(); } -bool UuidOrdering::exists(const fuse_core::UUID & uuid) const +bool UuidOrdering::exists(const fuse_core::UUID& uuid) const { return order_.right.find(uuid) != order_.right.end(); } -bool UuidOrdering::push_back(const fuse_core::UUID & uuid) +bool UuidOrdering::push_back(const fuse_core::UUID& uuid) { auto result = order_.insert(order_.end(), UuidOrderMapping::value_type(order_.size(), uuid)); return result.second; } -const fuse_core::UUID & UuidOrdering::operator[](const unsigned int index) const +const fuse_core::UUID& UuidOrdering::operator[](const unsigned int index) const { return order_.left[index].second; } -unsigned int UuidOrdering::operator[](const fuse_core::UUID & uuid) +unsigned int UuidOrdering::operator[](const fuse_core::UUID& uuid) { auto result = order_.insert(order_.end(), UuidOrderMapping::value_type(order_.size(), uuid)); return (*result.first).get_left(); } -const fuse_core::UUID & UuidOrdering::at(const unsigned int index) const +const fuse_core::UUID& UuidOrdering::at(const unsigned int index) const { return order_.left.at(index).second; } -unsigned int UuidOrdering::at(const fuse_core::UUID & uuid) const +unsigned int UuidOrdering::at(const fuse_core::UUID& uuid) const { return order_.right.at(uuid); } diff --git a/fuse_constraints/src/variable_constraints.cpp b/fuse_constraints/src/variable_constraints.cpp old mode 100755 new mode 100644 index 8be6b3f49..4ee2fec67 --- a/fuse_constraints/src/variable_constraints.cpp +++ b/fuse_constraints/src/variable_constraints.cpp @@ -51,10 +51,7 @@ bool VariableConstraints::empty() const size_t VariableConstraints::size() const { - auto sum_edges = [](const size_t input, const ConstraintCollection & edges) - { - return input + edges.size(); - }; + auto sum_edges = [](const size_t input, const ConstraintCollection& edges) { return input + edges.size(); }; return std::accumulate(variable_constraints_.begin(), variable_constraints_.end(), 0u, sum_edges); } @@ -65,33 +62,35 @@ unsigned int VariableConstraints::nextVariableIndex() const void VariableConstraints::insert(const unsigned int constraint, const unsigned int variable) { - if (variable >= variable_constraints_.size()) { + if (variable >= variable_constraints_.size()) + { variable_constraints_.resize(variable + 1); } variable_constraints_[variable].insert(constraint); } -void VariableConstraints::insert( - const unsigned int constraint, - std::initializer_list variable_list) +void VariableConstraints::insert(const unsigned int constraint, std::initializer_list variable_list) { return insert(constraint, variable_list.begin(), variable_list.end()); } void VariableConstraints::insert(const unsigned int variable) { - if (variable >= variable_constraints_.size()) { + if (variable >= variable_constraints_.size()) + { // This automatically create a new variable entry with an empty ConstraintCollection variable_constraints_.resize(variable + 1); } } -void VariableConstraints::print(std::ostream & stream) const +void VariableConstraints::print(std::ostream& stream) const { - for (size_t variable = 0; variable < variable_constraints_.size(); ++variable) { + for (size_t variable = 0; variable < variable_constraints_.size(); ++variable) + { stream << variable << ": ["; - for (const auto & constraint : variable_constraints_[variable]) { + for (const auto& constraint : variable_constraints_[variable]) + { stream << constraint << ", "; } @@ -99,7 +98,7 @@ void VariableConstraints::print(std::ostream & stream) const } } -std::ostream & operator<<(std::ostream & stream, const VariableConstraints & variable_constraints) +std::ostream& operator<<(std::ostream& stream, const VariableConstraints& variable_constraints) { variable_constraints.print(stream); return stream; diff --git a/fuse_constraints/suitesparse-extras.cmake b/fuse_constraints/suitesparse-extras.cmake index 97ec7bd5a..91b86da6b 100644 --- a/fuse_constraints/suitesparse-extras.cmake +++ b/fuse_constraints/suitesparse-extras.cmake @@ -3,28 +3,27 @@ # 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 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. +# * 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 Open Source Robotics Foundation, Inc. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# * Neither the name of the Open Source Robotics Foundation, Inc. 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. +# 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. if(DEFINED fuse_constraints_DIR) list(APPEND CMAKE_MODULE_PATH "${fuse_constraints_DIR}") diff --git a/fuse_constraints/test/CMakeLists.txt b/fuse_constraints/test/CMakeLists.txt index 426322c5c..276317e07 100644 --- a/fuse_constraints/test/CMakeLists.txt +++ b/fuse_constraints/test/CMakeLists.txt @@ -1,23 +1,24 @@ -# CORE GTESTS ====================================================================================== +# CORE GTESTS +# ====================================================================================== set(TEST_TARGETS - test_absolute_constraint - test_absolute_orientation_2d_stamped_constraint - test_absolute_orientation_3d_stamped_constraint - test_absolute_orientation_3d_stamped_euler_constraint - test_absolute_pose_2d_stamped_constraint - test_absolute_pose_3d_stamped_constraint - test_marginal_constraint - test_marginalize_variables - test_normal_delta_pose_2d - test_normal_prior_pose_2d - test_relative_constraint - test_relative_pose_2d_stamped_constraint - test_relative_pose_3d_stamped_constraint - test_uuid_ordering - test_variable_constraints -) + test_absolute_constraint + test_absolute_orientation_2d_stamped_constraint + test_absolute_orientation_3d_stamped_constraint + test_absolute_orientation_3d_stamped_euler_constraint + test_absolute_pose_2d_stamped_constraint + test_absolute_pose_3d_stamped_constraint + test_marginal_constraint + test_marginalize_variables + test_normal_delta_pose_2d + test_normal_prior_pose_2d + test_relative_constraint + test_relative_pose_2d_stamped_constraint + test_relative_pose_3d_stamped_constraint + test_uuid_ordering + test_variable_constraints) foreach(test_name ${TEST_TARGETS}) - ament_add_gtest("${test_name}" "${test_name}.cpp" WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) + ament_add_gtest("${test_name}" "${test_name}.cpp" WORKING_DIRECTORY + ${PROJECT_SOURCE_DIR}/test) target_link_libraries("${test_name}" ${PROJECT_NAME}) endforeach() diff --git a/fuse_constraints/test/cost_function_gtest.hpp b/fuse_constraints/test/cost_function_gtest.hpp index 3687d0504..e77f593e0 100644 --- a/fuse_constraints/test/cost_function_gtest.hpp +++ b/fuse_constraints/test/cost_function_gtest.hpp @@ -50,25 +50,25 @@ * @param[in] cost_function The expected cost function * @param[in] actual_cost_function The actual cost function */ -static void ExpectCostFunctionsAreEqual( - const ceres::CostFunction & cost_function, - const ceres::CostFunction & actual_cost_function) +static void ExpectCostFunctionsAreEqual(const ceres::CostFunction& cost_function, + const ceres::CostFunction& actual_cost_function) { EXPECT_EQ(cost_function.num_residuals(), actual_cost_function.num_residuals()); const size_t num_residuals = cost_function.num_residuals(); - const std::vector & parameter_block_sizes = cost_function.parameter_block_sizes(); - const std::vector & actual_parameter_block_sizes = - actual_cost_function.parameter_block_sizes(); + const std::vector& parameter_block_sizes = cost_function.parameter_block_sizes(); + const std::vector& actual_parameter_block_sizes = actual_cost_function.parameter_block_sizes(); EXPECT_EQ(parameter_block_sizes.size(), actual_parameter_block_sizes.size()); size_t num_parameters = 0; - for (size_t i = 0; i < parameter_block_sizes.size(); ++i) { + for (size_t i = 0; i < parameter_block_sizes.size(); ++i) + { EXPECT_EQ(parameter_block_sizes[i], actual_parameter_block_sizes[i]); num_parameters += parameter_block_sizes[i]; } std::unique_ptr parameters(new double[num_parameters]); - for (size_t i = 0; i < num_parameters; ++i) { + for (size_t i = 0; i < num_parameters; ++i) + { parameters[i] = static_cast(i) + 1.0; } @@ -78,12 +78,13 @@ static void ExpectCostFunctionsAreEqual( std::unique_ptr actual_residuals(new double[num_residuals]); std::unique_ptr actual_jacobians(new double[num_parameters * num_residuals]); - std::unique_ptr parameter_blocks(new double *[parameter_block_sizes.size()]); - std::unique_ptr jacobian_blocks(new double *[parameter_block_sizes.size()]); - std::unique_ptr actual_jacobian_blocks(new double *[parameter_block_sizes.size()]); + std::unique_ptr parameter_blocks(new double*[parameter_block_sizes.size()]); + std::unique_ptr jacobian_blocks(new double*[parameter_block_sizes.size()]); + std::unique_ptr actual_jacobian_blocks(new double*[parameter_block_sizes.size()]); num_parameters = 0; - for (size_t i = 0; i < parameter_block_sizes.size(); ++i) { + for (size_t i = 0; i < parameter_block_sizes.size(); ++i) + { parameter_blocks[i] = parameters.get() + num_parameters; jacobian_blocks[i] = jacobians.get() + num_parameters * num_residuals; actual_jacobian_blocks[i] = actual_jacobians.get() + num_parameters * num_residuals; @@ -91,29 +92,24 @@ static void ExpectCostFunctionsAreEqual( } EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.get(), residuals.get(), nullptr)); - EXPECT_TRUE( - actual_cost_function.Evaluate( - parameter_blocks.get(), actual_residuals.get(), - nullptr)); - for (size_t i = 0; i < num_residuals; ++i) { + EXPECT_TRUE(actual_cost_function.Evaluate(parameter_blocks.get(), actual_residuals.get(), nullptr)); + for (size_t i = 0; i < num_residuals; ++i) + { EXPECT_DOUBLE_EQ(residuals[i], actual_residuals[i]) << "residual id: " << i; } + EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.get(), residuals.get(), jacobian_blocks.get())); EXPECT_TRUE( - cost_function.Evaluate( - parameter_blocks.get(), residuals.get(), - jacobian_blocks.get())); - EXPECT_TRUE( - actual_cost_function.Evaluate( - parameter_blocks.get(), actual_residuals.get(), - actual_jacobian_blocks.get())); - for (size_t i = 0; i < num_residuals; ++i) { + actual_cost_function.Evaluate(parameter_blocks.get(), actual_residuals.get(), actual_jacobian_blocks.get())); + for (size_t i = 0; i < num_residuals; ++i) + { EXPECT_DOUBLE_EQ(residuals[i], actual_residuals[i]) << "residual : " << i; } - for (size_t i = 0; i < num_residuals * num_parameters; ++i) { + for (size_t i = 0; i < num_residuals * num_parameters; ++i) + { EXPECT_DOUBLE_EQ(jacobians[i], actual_jacobians[i]) - << "jacobian : " << i << " " << jacobians[i] << " " << actual_jacobians[i]; + << "jacobian : " << i << " " << jacobians[i] << " " << actual_jacobians[i]; } } diff --git a/fuse_constraints/test/test_absolute_constraint.cpp b/fuse_constraints/test/test_absolute_constraint.cpp index a9f7921e6..18583aab0 100644 --- a/fuse_constraints/test/test_absolute_constraint.cpp +++ b/fuse_constraints/test/test_absolute_constraint.cpp @@ -57,97 +57,76 @@ TEST(AbsoluteConstraint, Constructor) { // Construct a constraint for every type, just to make sure they compile. { - fuse_variables::AccelerationAngular2DStamped variable( - rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby")); + fuse_variables::AccelerationAngular2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby")); fuse_core::VectorXd mean(1); mean << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; EXPECT_NO_THROW( - fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint constraint( - "test", variable, mean, cov)); + fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint constraint("test", variable, mean, cov)); } { - fuse_variables::AccelerationLinear2DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("bender")); + fuse_variables::AccelerationLinear2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("bender")); fuse_core::VectorXd mean(2); mean << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; EXPECT_NO_THROW( - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint( - "test", variable, mean, cov)); + fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint("test", variable, mean, cov)); } { - fuse_variables::Orientation2DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("johnny5")); + fuse_variables::Orientation2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("johnny5")); fuse_core::VectorXd mean(1); mean << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - EXPECT_NO_THROW( - fuse_constraints::AbsoluteOrientation2DStampedConstraint constraint( - "test", variable, mean, cov)); + EXPECT_NO_THROW(fuse_constraints::AbsoluteOrientation2DStampedConstraint constraint("test", variable, mean, cov)); } { - fuse_variables::Position2DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("rosie")); + fuse_variables::Position2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("rosie")); fuse_core::VectorXd mean(2); mean << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; - EXPECT_NO_THROW( - fuse_constraints::AbsolutePosition2DStampedConstraint constraint( - "test", variable, mean, cov)); + EXPECT_NO_THROW(fuse_constraints::AbsolutePosition2DStampedConstraint constraint("test", variable, mean, cov)); } { - fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("clank")); + fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("clank")); fuse_core::VectorXd mean(3); mean << 1.0, 2.0, 3.0; fuse_core::MatrixXd cov(3, 3); cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - EXPECT_NO_THROW( - fuse_constraints::AbsolutePosition3DStampedConstraint constraint( - "test", variable, mean, cov)); + EXPECT_NO_THROW(fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov)); } { - fuse_variables::VelocityAngular2DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("gort")); + fuse_variables::VelocityAngular2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("gort")); fuse_core::VectorXd mean(1); mean << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; EXPECT_NO_THROW( - fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint constraint( - "test", variable, mean, cov)); + fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint constraint("test", variable, mean, cov)); } { - fuse_variables::VelocityLinear2DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("bishop")); + fuse_variables::VelocityLinear2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("bishop")); fuse_core::VectorXd mean(2); mean << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; - EXPECT_NO_THROW( - fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint constraint( - "test", variable, mean, - cov)); + EXPECT_NO_THROW(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint constraint("test", variable, mean, cov)); } } TEST(AbsoluteConstraint, PartialMeasurement) { - fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("vici")); + fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("vici")); fuse_core::VectorXd mean(2); mean << 3.0, 1.0; fuse_core::MatrixXd cov(2, 2); cov << 3.0, 0.2, 0.2, 1.0; - auto indices = std::vector{2, 0}; + auto indices = std::vector{ 2, 0 }; EXPECT_NO_THROW( - fuse_constraints::AbsolutePosition3DStampedConstraint constraint( - "test", variable, mean, cov, indices)); + fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov, indices)); } TEST(AbsoluteConstraint, Covariance) @@ -156,18 +135,16 @@ TEST(AbsoluteConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct fuse_variables::AccelerationLinear2DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("chappie")); + fuse_core::uuid::generate("chappie")); fuse_core::VectorXd mean(2); mean << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint("test", variable, - mean, cov); + fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint constraint("test", variable, mean, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix2d expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 1.002509414234171, -0.050125470711709, - 0.000000000000000, 0.707106781186547; + expected_sqrt_info << 1.002509414234171, -0.050125470711709, 0.000000000000000, 0.707106781186547; /* *INDENT-ON* */ fuse_core::Matrix2d expected_cov = cov; // Compare @@ -176,15 +153,13 @@ TEST(AbsoluteConstraint, Covariance) } // Test the covariance of a partial measurement { - fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("astroboy")); + fuse_variables::Position3DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("astroboy")); fuse_core::VectorXd mean(2); mean << 3.0, 1.0; fuse_core::MatrixXd cov(2, 2); cov << 3.0, 0.2, 0.2, 1.0; - auto indices = std::vector{2, 0}; - fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov, - indices); + auto indices = std::vector{ 2, 0 }; + fuse_constraints::AbsolutePosition3DStampedConstraint constraint("test", variable, mean, cov, indices); // Define the expected matrices fuse_core::Vector3d expected_mean; expected_mean << 1.0, 0.0, 3.0; @@ -192,8 +167,8 @@ TEST(AbsoluteConstraint, Covariance) expected_cov << 1.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.2, 0.0, 3.0; fuse_core::MatrixXd expected_sqrt_info(2, 3); /* *INDENT-OFF* */ - expected_sqrt_info << -0.116247638743819, 0.000000000000000, 0.581238193719096, - 1.000000000000000, 0.000000000000000, 0.000000000000000; + expected_sqrt_info << -0.116247638743819, 0.000000000000000, 0.581238193719096, 1.000000000000000, + 0.000000000000000, 0.000000000000000; /* *INDENT-ON* */ // Compare EXPECT_TRUE(expected_mean.isApprox(constraint.mean(), 1.0e-9)); @@ -208,9 +183,8 @@ TEST(AbsoluteConstraint, Optimization) { // Optimize a single variable and single constraint, verify the expected value and covariance // are generated. Create a variable - auto variable = fuse_variables::AccelerationLinear2DStamped::make_shared( - rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("t800")); + auto variable = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("t800")); variable->x() = 10.7; variable->y() = -3.2; // Create an absolute constraint @@ -218,29 +192,21 @@ TEST(AbsoluteConstraint, Optimization) mean << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; - auto constraint = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( - "test", - *variable, - mean, - cov); + auto constraint = + fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared("test", *variable, mean, cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - variable->data(), - variable->size(), + problem.AddParameterBlock(variable->data(), variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - variable->localParameterization()); + variable->localParameterization()); #else - variable->manifold()); + variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -249,7 +215,7 @@ TEST(AbsoluteConstraint, Optimization) EXPECT_NEAR(1.0, variable->x(), 1.0e-5); EXPECT_NEAR(2.0, variable->y(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(variable->data(), variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); @@ -266,8 +232,7 @@ TEST(AbsoluteConstraint, Optimization) // Optimize a single variable with a full measurement and a partial measurement // Verify the expected value and covariance are generated. // Create a variable - auto var = fuse_variables::Position3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("t1000")); + auto var = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("t1000")); var->x() = 10.7; var->y() = -3.2; var->z() = 0.9; @@ -276,47 +241,32 @@ TEST(AbsoluteConstraint, Optimization) mean1 << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov1; /* *INDENT-OFF* */ - cov1 << 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0; + cov1 << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; /* *INDENT-ON* */ - auto constraint1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( - "test", - *var, - mean1, - cov1); + auto constraint1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean1, cov1); fuse_core::Vector2d mean2; mean2 << 4.0, 2.0; fuse_core::Matrix2d cov2; /* *INDENT-OFF* */ - cov2 << 1.0, 0.0, - 0.0, 1.0; + cov2 << 1.0, 0.0, 0.0, 1.0; /* *INDENT-ON* */ - auto indices2 = std::vector{2, 0}; - auto constraint2 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( - "test", *var, mean2, cov2, indices2); + auto indices2 = std::vector{ 2, 0 }; + auto constraint2 = + fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean2, cov2, indices2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - var->data(), - var->size(), + problem.AddParameterBlock(var->data(), var->size(), #if !CERES_SUPPORTS_MANIFOLDS - var->localParameterization()); + var->localParameterization()); #else - var->manifold()); + var->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(var->data()); - problem.AddResidualBlock( - constraint1->costFunction(), - constraint1->lossFunction(), - parameter_blocks); - problem.AddResidualBlock( - constraint2->costFunction(), - constraint2->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint1->costFunction(), constraint1->lossFunction(), parameter_blocks); + problem.AddResidualBlock(constraint2->costFunction(), constraint2->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -326,7 +276,7 @@ TEST(AbsoluteConstraint, Optimization) EXPECT_NEAR(2.0, var->y(), 1.0e-5); EXPECT_NEAR(3.5, var->z(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(var->data(), var->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); @@ -336,9 +286,7 @@ TEST(AbsoluteConstraint, Optimization) fuse_core::Matrix3d actual_cov(covariance_vector.data()); fuse_core::Matrix3d expected_cov; /* *INDENT-OFF* */ - expected_cov << 0.5, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 0.5; + expected_cov << 0.5, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.5; /* *INDENT-ON* */ EXPECT_TRUE(expected_cov.isApprox(actual_cov, 1.0e-9)); } @@ -347,8 +295,7 @@ TEST(AbsoluteConstraint, Optimization) TEST(AbsoluteConstraint, PartialOptimization) { // Create a variable - auto var = fuse_variables::Position3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("t1000")); + auto var = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("t1000")); var->x() = 10.7; var->y() = -3.2; var->z() = 0.9; @@ -358,45 +305,35 @@ TEST(AbsoluteConstraint, PartialOptimization) mean1 << 1.0, 3.0; fuse_core::MatrixXd cov1(2, 2); /* *INDENT-OFF* */ - cov1 << 1.0, 0.0, - 0.0, 1.0; + cov1 << 1.0, 0.0, 0.0, 1.0; /* *INDENT-ON* */ - std::vector indices1 = - {fuse_variables::Position3DStamped::Z, fuse_variables::Position3DStamped::X}; - auto constraint1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( - "test", *var, mean1, cov1, indices1); + std::vector indices1 = { fuse_variables::Position3DStamped::Z, fuse_variables::Position3DStamped::X }; + auto constraint1 = + fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean1, cov1, indices1); // Create another constraint for the second index fuse_core::VectorXd mean2(1); mean2 << 2.0; fuse_core::MatrixXd cov2(1, 1); cov2 << 1.0; - std::vector indices2 = {fuse_variables::Position3DStamped::Y}; - auto constraint2 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( - "test", *var, mean2, cov2, indices2); + std::vector indices2 = { fuse_variables::Position3DStamped::Y }; + auto constraint2 = + fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *var, mean2, cov2, indices2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - var->data(), - var->size(), + problem.AddParameterBlock(var->data(), var->size(), #if !CERES_SUPPORTS_MANIFOLDS - var->localParameterization()); + var->localParameterization()); #else - var->manifold()); + var->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(var->data()); - problem.AddResidualBlock( - constraint1->costFunction(), - constraint1->lossFunction(), - parameter_blocks); - problem.AddResidualBlock( - constraint2->costFunction(), - constraint2->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint1->costFunction(), constraint1->lossFunction(), parameter_blocks); + problem.AddResidualBlock(constraint2->costFunction(), constraint2->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -411,35 +348,28 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) { // Optimize a single variable and single constraint, verify the expected value and covariance are // generated. Create a variable - auto variable = fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("tiktok")); + auto variable = + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("tiktok")); variable->yaw() = 0.7; // Create an absolute constraint fuse_core::VectorXd mean(1); mean << 7.0; fuse_core::MatrixXd cov(1, 1); cov << 0.10; - auto constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared( - "test", *variable, mean, cov); + auto constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared("test", *variable, mean, cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - variable->data(), - variable->size(), + problem.AddParameterBlock(variable->data(), variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - variable->localParameterization()); + variable->localParameterization()); #else - variable->manifold()); + variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -447,7 +377,7 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) // Check EXPECT_NEAR(7.0 - 2 * M_PI, variable->yaw(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(variable->data(), variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); @@ -461,14 +391,12 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) TEST(AbsoluteConstraint, Serialization) { // Construct a constraint - fuse_variables::AccelerationAngular2DStamped variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("robby")); + fuse_variables::AccelerationAngular2DStamped variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby")); fuse_core::VectorXd mean(1); mean << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint expected("test", variable, mean, - cov); + fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint expected("test", variable, mean, cov); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp index b745c8cc5..520ebb5f9 100644 --- a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp @@ -52,27 +52,21 @@ using fuse_constraints::AbsoluteOrientation2DStampedConstraint; using fuse_variables::Orientation2DStamped; - TEST(AbsoluteOrientation2DStampedConstraint, Constructor) { // Construct a constraint just to make sure it compiles. - Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::VectorXd mean(1); mean << 1.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - EXPECT_NO_THROW( - AbsoluteOrientation2DStampedConstraint constraint( - "test", orientation_variable, - mean, cov)); + EXPECT_NO_THROW(AbsoluteOrientation2DStampedConstraint constraint("test", orientation_variable, mean, cov)); } TEST(AbsoluteOrientation2DStampedConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct - Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); fuse_core::VectorXd mean(1); mean << 1.0; fuse_core::MatrixXd cov(1, 1); @@ -94,8 +88,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, Optimization) // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. // Create the variables - auto orientation_variable = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->yaw() = 1.0; // Create an absolute orientation constraint @@ -103,31 +96,22 @@ TEST(AbsoluteOrientation2DStampedConstraint, Optimization) mean << 1.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( - "test", - *orientation_variable, - mean, - cov); + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared("test", *orientation_variable, mean, cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -139,15 +123,14 @@ TEST(AbsoluteOrientation2DStampedConstraint, Optimization) EXPECT_NEAR(1.0, orientation_variable->yaw(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); - fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), - orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace(orientation_variable->data(), orientation_variable->data(), + actual_covariance.data()); // Define the expected covariance fuse_core::Matrix1d expected_covariance; @@ -161,9 +144,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) // covariance are generated. // Create the variables - auto orientation_variable = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), - fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->yaw() = 0.0; // Create an absolute orientation constraint @@ -171,31 +152,22 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) mean << 0.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( - "test", - *orientation_variable, - mean, - cov); + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared("test", *orientation_variable, mean, cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -207,15 +179,14 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) EXPECT_NEAR(0.0, orientation_variable->yaw(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); - fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), - orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace(orientation_variable->data(), orientation_variable->data(), + actual_covariance.data()); // Define the expected covariance fuse_core::Matrix1d expected_covariance; @@ -300,9 +271,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) // covariance are generated. // Create the variables - auto orientation_variable = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), - fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->yaw() = -M_PI; // Create an absolute orientation constraint @@ -310,31 +279,22 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) mean << -M_PI; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( - "test", - *orientation_variable, - mean, - cov); + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared("test", *orientation_variable, mean, cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -346,15 +306,14 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) EXPECT_NEAR(-M_PI, orientation_variable->yaw(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); - fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), - orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace(orientation_variable->data(), orientation_variable->data(), + actual_covariance.data()); // Define the expected covariance fuse_core::Matrix1d expected_covariance; @@ -365,8 +324,7 @@ TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) TEST(AbsoluteOrientation2DStampedConstraint, Serialization) { // Construct a constraint - Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::VectorXd mean(1); mean << 1.0; fuse_core::MatrixXd cov(1, 1); diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp index 0a10bbbb5..dade370af 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp @@ -52,41 +52,29 @@ using fuse_constraints::AbsoluteOrientation3DStampedConstraint; using fuse_variables::Orientation3DStamped; - TEST(AbsoluteOrientation3DStampedConstraint, Constructor) { // Construct a constraint just to make sure it compiles. - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector4d mean; mean << 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - EXPECT_NO_THROW( - AbsoluteOrientation3DStampedConstraint constraint( - "test", orientation_variable, - mean, cov)); + EXPECT_NO_THROW(AbsoluteOrientation3DStampedConstraint constraint("test", orientation_variable, mean, cov)); Eigen::Quaterniond quat_eigen(1.0, 0.0, 0.0, 0.0); - EXPECT_NO_THROW( - AbsoluteOrientation3DStampedConstraint constraint( - "test", orientation_variable, - quat_eigen, cov)); + EXPECT_NO_THROW(AbsoluteOrientation3DStampedConstraint constraint("test", orientation_variable, quat_eigen, cov)); geometry_msgs::msg::Quaternion quat_geom; quat_geom.w = 1.0; - std::array cov_arr = {1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0}; - EXPECT_NO_THROW( - AbsoluteOrientation3DStampedConstraint constraint( - "test", orientation_variable, - quat_geom, cov_arr)); + std::array cov_arr = { 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0 }; + EXPECT_NO_THROW(AbsoluteOrientation3DStampedConstraint constraint("test", orientation_variable, quat_geom, cov_arr)); } TEST(AbsoluteOrientation3DStampedConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); fuse_core::Vector4d mean; mean << 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix3d cov; @@ -96,9 +84,8 @@ TEST(AbsoluteOrientation3DStampedConstraint, Covariance) // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix3d expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 1.008395589795798, -0.040950074712520, -0.063131365181801, - 0.000000000000000, 0.712470499879096, -0.071247049987910, - 0.000000000000000, 0.000000000000000, 0.577350269189626; + expected_sqrt_info << 1.008395589795798, -0.040950074712520, -0.063131365181801, 0.000000000000000, 0.712470499879096, + -0.071247049987910, 0.000000000000000, 0.000000000000000, 0.577350269189626; /* *INDENT-ON* */ fuse_core::Matrix3d expected_cov = cov; @@ -111,8 +98,7 @@ TEST(AbsoluteOrientation3DStampedConstraint, Optimization) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto orientation_variable = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->w() = 0.952; orientation_variable->x() = 0.038; orientation_variable->y() = -0.189; @@ -123,35 +109,23 @@ TEST(AbsoluteOrientation3DStampedConstraint, Optimization) mean << 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix3d cov; - cov << - 1.0, 0.1, 0.2, - 0.1, 2.0, 0.3, - 0.2, 0.3, 3.0; - auto constraint = AbsoluteOrientation3DStampedConstraint::make_shared( - "test", - *orientation_variable, - mean, - cov); + cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; + auto constraint = AbsoluteOrientation3DStampedConstraint::make_shared("test", *orientation_variable, mean, cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -165,30 +139,25 @@ TEST(AbsoluteOrientation3DStampedConstraint, Optimization) EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); ceres::Covariance::Options cov_options; ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); - fuse_core::Matrix3d actual_covariance(orientation_variable->localSize(), - orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + fuse_core::Matrix3d actual_covariance(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace(orientation_variable->data(), orientation_variable->data(), + actual_covariance.data()); // Define the expected covariance fuse_core::Matrix3d expected_covariance; - expected_covariance << - 1.0, 0.1, 0.2, - 0.1, 2.0, 0.3, - 0.2, 0.3, 3.0; + expected_covariance << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); } TEST(AbsoluteOrientation3DStampedConstraint, Serialization) { // Construct a constraint - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector4d mean; mean << 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix3d cov; diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp index 9787d83f6..3929b84ce 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp @@ -50,46 +50,36 @@ using fuse_constraints::AbsoluteOrientation3DStampedEulerConstraint; using fuse_variables::Orientation3DStamped; - TEST(AbsoluteOrientation3DStampedEulerConstraint, Constructor) { // Construct a constraint just to make sure it compiles. - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector3d mean; mean << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - std::vector axes = - {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, - Orientation3DStamped::Euler::PITCH}; - EXPECT_NO_THROW( - AbsoluteOrientation3DStampedEulerConstraint constraint( - "test", orientation_variable, mean, cov, - axes)); + std::vector axes = { Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, + Orientation3DStamped::Euler::PITCH }; + EXPECT_NO_THROW(AbsoluteOrientation3DStampedEulerConstraint constraint("test", orientation_variable, mean, cov, axes)); } TEST(AbsoluteOrientation3DStampedEulerConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); fuse_core::Vector3d mean; mean << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - std::vector axes = - {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, - Orientation3DStamped::Euler::PITCH}; - AbsoluteOrientation3DStampedEulerConstraint constraint("test", orientation_variable, mean, cov, - axes); + std::vector axes = { Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, + Orientation3DStamped::Euler::PITCH }; + AbsoluteOrientation3DStampedEulerConstraint constraint("test", orientation_variable, mean, cov, axes); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix3d expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 1.008395589795798, -0.040950074712520, -0.063131365181801, - 0.000000000000000, 0.712470499879096, -0.071247049987910, - 0.000000000000000, 0.000000000000000, 0.577350269189626; + expected_sqrt_info << 1.008395589795798, -0.040950074712520, -0.063131365181801, 0.000000000000000, 0.712470499879096, + -0.071247049987910, 0.000000000000000, 0.000000000000000, 0.577350269189626; /* *INDENT-ON* */ fuse_core::Matrix3d expected_cov = cov; @@ -102,8 +92,7 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto orientation_variable = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->w() = 0.952; orientation_variable->x() = 0.038; orientation_variable->y() = -0.189; @@ -114,35 +103,25 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull) mean << 0.5, 1.0, 1.5; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - std::vector axes = - {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, - Orientation3DStamped::Euler::PITCH}; - auto constraint = AbsoluteOrientation3DStampedEulerConstraint::make_shared( - "test", - *orientation_variable, - mean, - cov, - axes); + std::vector axes = { Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, + Orientation3DStamped::Euler::PITCH }; + auto constraint = + AbsoluteOrientation3DStampedEulerConstraint::make_shared("test", *orientation_variable, mean, cov, axes); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -151,8 +130,8 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull) // Check Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); EXPECT_NEAR(expected.w(), orientation_variable->w(), 5.0e-3); EXPECT_NEAR(expected.x(), orientation_variable->x(), 5.0e-3); EXPECT_NEAR(expected.y(), orientation_variable->y(), 5.0e-3); @@ -165,8 +144,7 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto orientation_variable = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->w() = 0.952; orientation_variable->x() = 0.038; orientation_variable->y() = -0.189; @@ -177,51 +155,34 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) mean1 << 0.5, 1.5; fuse_core::Matrix2d cov1; cov1 << 1.0, 0.2, 0.2, 3.0; - std::vector axes1 = - {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::PITCH}; - auto constraint1 = AbsoluteOrientation3DStampedEulerConstraint::make_shared( - "test", - *orientation_variable, - mean1, - cov1, - axes1); + std::vector axes1 = { Orientation3DStamped::Euler::YAW, + Orientation3DStamped::Euler::PITCH }; + auto constraint1 = + AbsoluteOrientation3DStampedEulerConstraint::make_shared("test", *orientation_variable, mean1, cov1, axes1); fuse_core::Vector1d mean2; mean2 << 1.0; fuse_core::Matrix1d cov2; cov2 << 2.0; - std::vector axes2 = - {Orientation3DStamped::Euler::ROLL}; - auto constraint2 = AbsoluteOrientation3DStampedEulerConstraint::make_shared( - "test", - *orientation_variable, - mean2, - cov2, - axes2); + std::vector axes2 = { Orientation3DStamped::Euler::ROLL }; + auto constraint2 = + AbsoluteOrientation3DStampedEulerConstraint::make_shared("test", *orientation_variable, mean2, cov2, axes2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint1->costFunction(), - constraint1->lossFunction(), - parameter_blocks); - problem.AddResidualBlock( - constraint2->costFunction(), - constraint2->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint1->costFunction(), constraint1->lossFunction(), parameter_blocks); + problem.AddResidualBlock(constraint2->costFunction(), constraint2->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -230,8 +191,8 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) // Check Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); EXPECT_NEAR(expected.w(), orientation_variable->w(), 5.0e-3); EXPECT_NEAR(expected.x(), orientation_variable->x(), 5.0e-3); EXPECT_NEAR(expected.y(), orientation_variable->y(), 5.0e-3); @@ -243,17 +204,14 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) TEST(AbsoluteOrientation3DStampedEulerConstraint, Serialization) { // Construct a constraint - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector3d mean; mean << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - std::vector axes = - {Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, - Orientation3DStamped::Euler::PITCH}; - AbsoluteOrientation3DStampedEulerConstraint expected("test", orientation_variable, mean, cov, - axes); + std::vector axes = { Orientation3DStamped::Euler::YAW, Orientation3DStamped::Euler::ROLL, + Orientation3DStamped::Euler::PITCH }; + AbsoluteOrientation3DStampedEulerConstraint expected("test", orientation_variable, mean, cov, axes); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp index ef88fe0dd..c8092ce36 100644 --- a/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp @@ -48,45 +48,38 @@ #include #include +using fuse_constraints::AbsolutePose2DStampedConstraint; using fuse_variables::Orientation2DStamped; using fuse_variables::Position2DStamped; -using fuse_constraints::AbsolutePose2DStampedConstraint; - TEST(AbsolutePose2DStampedConstraint, Constructor) { // Construct a constraint just to make sure it compiles. - Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); Position2DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector3d mean; mean << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; EXPECT_NO_THROW( - AbsolutePose2DStampedConstraint constraint( - "test", position_variable, orientation_variable, - mean, cov)); + AbsolutePose2DStampedConstraint constraint("test", position_variable, orientation_variable, mean, cov)); } TEST(AbsolutePose2DStampedConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct - Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); Position2DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); fuse_core::Vector3d mean; mean << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - AbsolutePose2DStampedConstraint constraint("test", position_variable, orientation_variable, mean, - cov); + AbsolutePose2DStampedConstraint constraint("test", position_variable, orientation_variable, mean, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix3d expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 1.008395589795798, -0.040950074712520, -0.063131365181801, - 0.000000000000000, 0.712470499879096, -0.071247049987910, - 0.000000000000000, 0.000000000000000, 0.577350269189626; + expected_sqrt_info << 1.008395589795798, -0.040950074712520, -0.063131365181801, 0.000000000000000, 0.712470499879096, + -0.071247049987910, 0.000000000000000, 0.000000000000000, 0.577350269189626; /* *INDENT-ON* */ fuse_core::Matrix3d expected_cov = cov; // Compare @@ -98,11 +91,9 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto orientation_variable = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->yaw() = 0.8; - auto position_variable = Position2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position_variable = Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; position_variable->y() = -3.0; // Create an absolute pose constraint @@ -110,39 +101,28 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) mean << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - auto constraint = AbsolutePose2DStampedConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean, - cov); + auto constraint = + AbsolutePose2DStampedConstraint::make_shared("test", *position_variable, *orientation_variable, mean, cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - problem.AddParameterBlock( - position_variable->data(), - position_variable->size(), + problem.AddParameterBlock(position_variable->data(), position_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - position_variable->localParameterization()); + position_variable->localParameterization()); #else - position_variable->manifold()); + position_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -152,7 +132,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) EXPECT_NEAR(2.0, position_variable->y(), 1.0e-5); EXPECT_NEAR(3.0, orientation_variable->yaw(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); @@ -160,18 +140,11 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); std::vector covariance_vector1(position_variable->size() * position_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - position_variable->data(), covariance_vector1.data()); + covariance.GetCovarianceBlock(position_variable->data(), position_variable->data(), covariance_vector1.data()); std::vector covariance_vector2(position_variable->size() * orientation_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - orientation_variable->data(), covariance_vector2.data()); - std::vector covariance_vector3(orientation_variable->size() * - orientation_variable->size()); - covariance.GetCovarianceBlock( - orientation_variable->data(), - orientation_variable->data(), covariance_vector3.data()); + covariance.GetCovarianceBlock(position_variable->data(), orientation_variable->data(), covariance_vector2.data()); + std::vector covariance_vector3(orientation_variable->size() * orientation_variable->size()); + covariance.GetCovarianceBlock(orientation_variable->data(), orientation_variable->data(), covariance_vector3.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix3d actual_covariance; actual_covariance(0, 0) = covariance_vector1[0]; @@ -191,11 +164,9 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto orientation_variable = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->yaw() = 0.8; - auto position_variable = Position2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position_variable = Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; position_variable->y() = -3.0; @@ -204,64 +175,42 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) mean1 << 1.0, 3.0; fuse_core::Matrix2d cov1; cov1 << 1.0, 0.2, 0.2, 3.0; - std::vector axes_lin1 = {fuse_variables::Position2DStamped::X}; - std::vector axes_ang1 = {fuse_variables::Orientation2DStamped::YAW}; - auto constraint1 = AbsolutePose2DStampedConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean1, - cov1, - axes_lin1, - axes_ang1); + std::vector axes_lin1 = { fuse_variables::Position2DStamped::X }; + std::vector axes_ang1 = { fuse_variables::Orientation2DStamped::YAW }; + auto constraint1 = AbsolutePose2DStampedConstraint::make_shared("test", *position_variable, *orientation_variable, + mean1, cov1, axes_lin1, axes_ang1); // Create an absolute pose constraint fuse_core::Vector1d mean2; mean2 << 2.0; fuse_core::Matrix1d cov2; cov2 << 2.0; - std::vector axes_lin2 = {fuse_variables::Position2DStamped::Y}; + std::vector axes_lin2 = { fuse_variables::Position2DStamped::Y }; std::vector axes_ang2; - auto constraint2 = AbsolutePose2DStampedConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean2, - cov2, - axes_lin2, - axes_ang2); + auto constraint2 = AbsolutePose2DStampedConstraint::make_shared("test", *position_variable, *orientation_variable, + mean2, cov2, axes_lin2, axes_ang2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - position_variable->data(), - position_variable->size(), + problem.AddParameterBlock(position_variable->data(), position_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - position_variable->localParameterization()); + position_variable->localParameterization()); #else - position_variable->manifold()); + position_variable->manifold()); #endif - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint1->costFunction(), - constraint1->lossFunction(), - parameter_blocks); - problem.AddResidualBlock( - constraint2->costFunction(), - constraint2->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint1->costFunction(), constraint1->lossFunction(), parameter_blocks); + problem.AddResidualBlock(constraint2->costFunction(), constraint2->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -274,7 +223,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) EXPECT_NEAR(3.0, orientation_variable->yaw(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); @@ -282,18 +231,11 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); std::vector covariance_vector1(position_variable->size() * position_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - position_variable->data(), covariance_vector1.data()); + covariance.GetCovarianceBlock(position_variable->data(), position_variable->data(), covariance_vector1.data()); std::vector covariance_vector2(position_variable->size() * orientation_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - orientation_variable->data(), covariance_vector2.data()); - std::vector covariance_vector3(orientation_variable->size() * - orientation_variable->size()); - covariance.GetCovarianceBlock( - orientation_variable->data(), - orientation_variable->data(), covariance_vector3.data()); + covariance.GetCovarianceBlock(position_variable->data(), orientation_variable->data(), covariance_vector2.data()); + std::vector covariance_vector3(orientation_variable->size() * orientation_variable->size()); + covariance.GetCovarianceBlock(orientation_variable->data(), orientation_variable->data(), covariance_vector3.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix3d actual_covariance; @@ -322,15 +264,13 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) TEST(AbsolutePose2DStampedConstraint, Serialization) { // Construct a constraint - Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation2DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); Position2DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector3d mean; mean << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - AbsolutePose2DStampedConstraint expected("test", position_variable, orientation_variable, mean, - cov); + AbsolutePose2DStampedConstraint expected("test", position_variable, orientation_variable, mean, cov); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp index e13c14186..e7d93e9cc 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp @@ -48,17 +48,15 @@ #include #include +using fuse_constraints::AbsolutePose3DStampedConstraint; using fuse_variables::Orientation3DStamped; using fuse_variables::Position3DStamped; -using fuse_constraints::AbsolutePose3DStampedConstraint; - TEST(AbsolutePose3DStampedConstraint, Constructor) { // Construct a constraint just to make sure it compiles. Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector7d mean; mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0; @@ -67,26 +65,29 @@ TEST(AbsolutePose3DStampedConstraint, Constructor) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ EXPECT_NO_THROW( - AbsolutePose3DStampedConstraint constraint( - "test", position_variable, orientation_variable, - mean, cov)); + AbsolutePose3DStampedConstraint constraint("test", position_variable, orientation_variable, mean, cov)); } TEST(AbsolutePose3DStampedConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); fuse_core::Vector7d mean; mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0; @@ -95,26 +96,32 @@ TEST(AbsolutePose3DStampedConstraint, Covariance) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - AbsolutePose3DStampedConstraint constraint("test", position_variable, orientation_variable, mean, - cov); + AbsolutePose3DStampedConstraint constraint("test", position_variable, orientation_variable, mean, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix6d expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT - 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT - 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT - 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, + -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov = cov; @@ -127,14 +134,12 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto position_variable = Position3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position_variable = Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; position_variable->y() = -3.0; position_variable->z() = 10.0; - auto orientation_variable = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->w() = 0.952; orientation_variable->x() = 0.038; orientation_variable->y() = -0.189; @@ -146,50 +151,35 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, - 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, - 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, - 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, - 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, - 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, 0.3, 0.5, 0.2, 4.0, + 0.3, 0.4, 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; /* *INDENT-ON* */ - auto constraint = AbsolutePose3DStampedConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean, - cov); + auto constraint = + AbsolutePose3DStampedConstraint::make_shared("test", *position_variable, *orientation_variable, mean, cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - position_variable->data(), - position_variable->size(), + problem.AddParameterBlock(position_variable->data(), position_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - position_variable->localParameterization()); + position_variable->localParameterization()); #else - position_variable->manifold()); + position_variable->manifold()); #endif - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -206,7 +196,7 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); @@ -215,18 +205,15 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - position_variable->data(), cov_pos_pos.data()); + covariance.GetCovarianceBlock(position_variable->data(), position_variable->data(), cov_pos_pos.data()); - fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), - orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); + fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace(orientation_variable->data(), orientation_variable->data(), + cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - position_variable->data(), orientation_variable->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position_variable->data(), orientation_variable->data(), + cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -235,13 +222,8 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, - 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, - 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, - 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, - 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, - 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + expected_covariance << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, 0.3, + 0.5, 0.2, 4.0, 0.3, 0.4, 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; /* *INDENT-ON* */ EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-5); @@ -251,8 +233,7 @@ TEST(AbsolutePose3DStampedConstraint, Serialization) { // Construct a constraint Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector7d mean; mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0; @@ -261,16 +242,21 @@ TEST(AbsolutePose3DStampedConstraint, Serialization) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - AbsolutePose3DStampedConstraint expected("test", position_variable, orientation_variable, mean, - cov); + AbsolutePose3DStampedConstraint expected("test", position_variable, orientation_variable, mean, cov); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_constraints/test/test_marginal_constraint.cpp b/fuse_constraints/test/test_marginal_constraint.cpp index cf49f9ef0..58409d5f0 100644 --- a/fuse_constraints/test/test_marginal_constraint.cpp +++ b/fuse_constraints/test/test_marginal_constraint.cpp @@ -64,13 +64,8 @@ TEST(MarginalConstraint, OneVariable) fuse_core::Vector1d b; b << 3.0; - auto constraint = fuse_constraints::MarginalConstraint( - "test", - variables.begin(), - variables.end(), - A.begin(), - A.end(), - b); + auto constraint = + fuse_constraints::MarginalConstraint("test", variables.begin(), variables.end(), A.begin(), A.end(), b); auto cost_function = constraint.costFunction(); @@ -79,10 +74,10 @@ TEST(MarginalConstraint, OneVariable) x1.y() = 6.0; // Compute the actual residuals and jacobians - std::vector variable_values = {x1.data()}; + std::vector variable_values = { x1.data() }; fuse_core::Vector1d actual_residuals; fuse_core::MatrixXd actual_jacobian1(1, 2); - std::vector actual_jacobians = {actual_jacobian1.data()}; + std::vector actual_jacobians = { actual_jacobian1.data() }; cost_function->Evaluate(variable_values.data(), actual_residuals.data(), actual_jacobians.data()); // Define the expected residuals and jacobians @@ -123,13 +118,8 @@ TEST(MarginalConstraint, TwoVariables) fuse_core::Vector1d b; b << 9.0; - auto constraint = fuse_constraints::MarginalConstraint( - "test", - variables.begin(), - variables.end(), - A.begin(), - A.end(), - b); + auto constraint = + fuse_constraints::MarginalConstraint("test", variables.begin(), variables.end(), A.begin(), A.end(), b); auto cost_function = constraint.costFunction(); @@ -141,11 +131,11 @@ TEST(MarginalConstraint, TwoVariables) x2.y() = 18.0; // Compute the actual residuals and jacobians - std::vector variable_values = {x1.data(), x2.data()}; + std::vector variable_values = { x1.data(), x2.data() }; fuse_core::Vector1d actual_residuals; fuse_core::MatrixXd actual_jacobian1(1, 2); fuse_core::MatrixXd actual_jacobian2(1, 2); - std::vector actual_jacobians = {actual_jacobian1.data(), actual_jacobian2.data()}; + std::vector actual_jacobians = { actual_jacobian1.data(), actual_jacobian2.data() }; cost_function->Evaluate(variable_values.data(), actual_residuals.data(), actual_jacobians.data()); // Define the expected residuals and jacobians @@ -184,13 +174,8 @@ TEST(MarginalConstraint, LocalParameterization) fuse_core::Vector1d b; b << 8.0; - auto constraint = fuse_constraints::MarginalConstraint( - "test", - variables.begin(), - variables.end(), - A.begin(), - A.end(), - b); + auto constraint = + fuse_constraints::MarginalConstraint("test", variables.begin(), variables.end(), A.begin(), A.end(), b); auto cost_function = constraint.costFunction(); // Update the variable value @@ -201,14 +186,11 @@ TEST(MarginalConstraint, LocalParameterization) x1.z() = 0.526043; // Compute the actual residuals and jacobians - std::vector variable_values = {x1.data()}; + std::vector variable_values = { x1.data() }; fuse_core::Vector1d actual_residuals; fuse_core::MatrixXd actual_jacobian1(1, 4); - std::vector actual_jacobians = {actual_jacobian1.data()}; - cost_function->Evaluate( - variable_values.data(), - actual_residuals.data(), - actual_jacobians.data()); + std::vector actual_jacobians = { actual_jacobian1.data() }; + cost_function->Evaluate(variable_values.data(), actual_residuals.data(), actual_jacobians.data()); // Define the expected residuals and jacobians fuse_core::Vector1d expected_residuals; @@ -245,13 +227,8 @@ TEST(MarginalConstraint, Serialization) fuse_core::Vector1d b; b << 8.0; - auto expected = fuse_constraints::MarginalConstraint( - "test", - variables.begin(), - variables.end(), - A.begin(), - A.end(), - b); + auto expected = + fuse_constraints::MarginalConstraint("test", variables.begin(), variables.end(), A.begin(), A.end(), b); // Serialize the constraint into an archive std::stringstream stream; @@ -277,15 +254,16 @@ TEST(MarginalConstraint, Serialization) #if !CERES_SUPPORTS_MANIFOLDS using ExpectedLocalParam = fuse_variables::Orientation3DLocalParameterization; ASSERT_EQ(expected.localParameterizations().size(), actual.localParameterizations().size()); - for (auto i = 0u; i < actual.localParameterizations().size(); ++i) { - auto actual_derived = std::dynamic_pointer_cast( - actual.localParameterizations()[i]); + for (auto i = 0u; i < actual.localParameterizations().size(); ++i) + { + auto actual_derived = std::dynamic_pointer_cast(actual.localParameterizations()[i]); EXPECT_TRUE(static_cast(actual_derived)); } #else using ExpectedManifold = fuse_variables::Orientation3DManifold; ASSERT_EQ(expected.manifolds().size(), actual.manifolds().size()); - for (auto i = 0u; i < actual.manifolds().size(); ++i) { + for (auto i = 0u; i < actual.manifolds().size(); ++i) + { auto actual_derived = std::dynamic_pointer_cast(actual.manifolds()[i]); EXPECT_TRUE(static_cast(actual_derived)); } @@ -314,13 +292,8 @@ TEST(MarginalConstraint, LegacyDeserialization) fuse_core::Vector1d b; b << 8.0; - auto expected = fuse_constraints::MarginalConstraint( - "test", - variables.begin(), - variables.end(), - A.begin(), - A.end(), - b); + auto expected = + fuse_constraints::MarginalConstraint("test", variables.begin(), variables.end(), A.begin(), A.end(), b); // The legacy serialization file was generated using the following code: // { @@ -346,7 +319,8 @@ TEST(MarginalConstraint, LegacyDeserialization) // should get wrapped in a ManifoldAdpater using ExpectedManifold = fuse_core::ManifoldAdapter; ASSERT_EQ(expected.manifolds().size(), actual.manifolds().size()); - for (auto i = 0u; i < actual.manifolds().size(); ++i) { + for (auto i = 0u; i < actual.manifolds().size(); ++i) + { auto actual_derived = std::dynamic_pointer_cast(actual.manifolds()[i]); EXPECT_TRUE(static_cast(actual_derived)); } diff --git a/fuse_constraints/test/test_marginalize_variables.cpp b/fuse_constraints/test/test_marginalize_variables.cpp index 591fc6d65..2530a887b 100644 --- a/fuse_constraints/test/test_marginalize_variables.cpp +++ b/fuse_constraints/test/test_marginalize_variables.cpp @@ -63,22 +63,31 @@ class GenericVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(GenericVariable) - GenericVariable() - : fuse_core::Variable(fuse_core::uuid::generate()), - data_{} - {} + GenericVariable() : fuse_core::Variable(fuse_core::uuid::generate()), data_{} + { + } - explicit GenericVariable(const fuse_core::UUID & uuid) - : fuse_core::Variable(uuid), - data_{} - {} + explicit GenericVariable(const fuse_core::UUID& uuid) : fuse_core::Variable(uuid), data_{} + { + } - size_t size() const override {return 1;} + size_t size() const override + { + return 1; + } - const double * data() const override {return &data_;} - double * data() override {return &data_;} + const double* data() const override + { + return &data_; + } + double* data() override + { + return &data_; + } - void print(std::ostream & /*stream = std::cout*/) const override {} + void print(std::ostream& /*stream = std::cout*/) const override + { + } protected: double data_; @@ -94,11 +103,11 @@ class GenericVariable : public fuse_core::Variable * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; @@ -112,18 +121,27 @@ class GenericConstraint : public fuse_core::Constraint public: FUSE_CONSTRAINT_DEFINITIONS(GenericConstraint) - GenericConstraint(std::initializer_list variable_uuids) - : Constraint("test", variable_uuids) {} + GenericConstraint(std::initializer_list variable_uuids) : Constraint("test", variable_uuids) + { + } - explicit GenericConstraint(const fuse_core::UUID & variable1) - : fuse_core::Constraint("test", {variable1}) {} + explicit GenericConstraint(const fuse_core::UUID& variable1) : fuse_core::Constraint("test", { variable1 }) + { + } - GenericConstraint(const fuse_core::UUID & variable1, const fuse_core::UUID & variable2) - : fuse_core::Constraint("test", {variable1, variable2}) {} + GenericConstraint(const fuse_core::UUID& variable1, const fuse_core::UUID& variable2) + : fuse_core::Constraint("test", { variable1, variable2 }) + { + } - void print(std::ostream & /*stream = std::cout*/) const override {} + void print(std::ostream& /*stream = std::cout*/) const override + { + } - ceres::CostFunction * costFunction() const override {return nullptr;} + ceres::CostFunction* costFunction() const override + { + return nullptr; + } private: // Allow Boost Serialization access to private methods @@ -136,10 +154,10 @@ class GenericConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; @@ -150,10 +168,8 @@ class FixedOrientation3DStamped : public fuse_variables::Orientation3DStamped FixedOrientation3DStamped() = default; - explicit FixedOrientation3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL) - : Orientation3DStamped(stamp, device_id) + explicit FixedOrientation3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL) + : Orientation3DStamped(stamp, device_id) { } @@ -173,10 +189,10 @@ class FixedOrientation3DStamped : public fuse_variables::Orientation3DStamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; @@ -208,7 +224,7 @@ TEST(MarginalizeVariables, ComputeEliminationOrder) graph.addConstraint(c6); // Define the set of variables to be marginalized - auto to_be_marginalized = std::vector{x2->uuid(), x1->uuid()}; + auto to_be_marginalized = std::vector{ x2->uuid(), x1->uuid() }; // Compute the ordering auto actual = fuse_constraints::computeEliminationOrder(to_be_marginalized, graph); @@ -222,7 +238,8 @@ TEST(MarginalizeVariables, ComputeEliminationOrder) // Check ASSERT_EQ(expected.size(), actual.size()); - for (size_t i = 0; i < expected.size(); ++i) { + for (size_t i = 0; i < expected.size(); ++i) + { SCOPED_TRACE(i); EXPECT_EQ(fuse_core::uuid::to_string(expected.at(i)), fuse_core::uuid::to_string(actual.at(i))); } @@ -259,14 +276,11 @@ TEST(MarginalizeVariables, ComputeEliminationOrderWithOrphanVariables) // fail) With 1 or 2 orphan variables computeEliminationOrder throws an std::runtime_error // exception because CCOLAMD fails With 3 or more orphan variables computeEliminationOrder crashes // with `free(): invalid next size (fast)` - auto o1 = - GenericVariable::make_shared( - fuse_core::uuid::from_string( - "b726fbef-4015-4dc8-b4dd-cb57d4439c74")); + auto o1 = GenericVariable::make_shared(fuse_core::uuid::from_string("b726fbef-4015-4dc8-b4dd-cb57d4439c74")); graph.addVariable(o1); // Define the set of variables to be marginalized - auto to_be_marginalized = std::vector{x2->uuid(), x1->uuid(), o1->uuid()}; + auto to_be_marginalized = std::vector{ x2->uuid(), x1->uuid(), o1->uuid() }; // Compute the ordering fuse_constraints::UuidOrdering actual; @@ -282,18 +296,21 @@ TEST(MarginalizeVariables, ComputeEliminationOrderWithOrphanVariables) // Check ASSERT_EQ(expected.size(), actual.size()); - for (size_t i = 0; i < expected.size(); ++i) { + for (size_t i = 0; i < expected.size(); ++i) + { SCOPED_TRACE(i); EXPECT_EQ(fuse_core::uuid::to_string(expected.at(i)), fuse_core::uuid::to_string(actual.at(i))); } // Check all marginalized variables are in the elimination order // This check is equivalent to the assert in marginalizeVariables - for (const auto & variable_uuid : to_be_marginalized) { + for (const auto& variable_uuid : to_be_marginalized) + { SCOPED_TRACE(fuse_core::uuid::to_string(variable_uuid)); EXPECT_TRUE(actual.exists(variable_uuid)); - if (actual.exists(variable_uuid)) { + if (actual.exists(variable_uuid)) + { EXPECT_GT(to_be_marginalized.size(), actual.at(variable_uuid)); } } @@ -318,11 +335,7 @@ TEST(MarginalizeVariables, Linearize) delta << 0.979795897, 0.0, 0.0, 0.2; fuse_core::Matrix3d cov; cov << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto constraint = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", - *x1, *x2, - delta, - cov); + auto constraint = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x1, *x2, delta, cov); auto graph = fuse_graphs::HashGraph(); graph.addVariable(x1); @@ -344,16 +357,16 @@ TEST(MarginalizeVariables, Linearize) // Define the expected values fuse_core::MatrixXd expected_A0(3, 3); /* *INDENT-OFF* */ - expected_A0 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, - 0.27712824947756498073, -0.65053818745824854020, -2.5352112979770691226e-07, - 7.1505019336171038447e-08, 2.5546009342625186633e-07, -0.57735026918958520792; + 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); /* *INDENT-OFF* */ - 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; + 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* */ fuse_core::VectorXd expected_b(3); @@ -376,9 +389,9 @@ TEST(MarginalizeVariables, MarginalizeNext) term1.variables.push_back(1); auto A1 = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - 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; + 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); auto b1 = fuse_core::VectorXd(3); @@ -390,15 +403,15 @@ TEST(MarginalizeVariables, MarginalizeNext) term2.variables.push_back(1); auto A21 = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - 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; + 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); /* *INDENT-OFF* */ - A22 << -0.91999992754510684367, -0.39191852892782985673, -2.8735440640859089001e-07, - 0.27712824947756498073, -0.6505381874582485402, -2.5352112979770691226e-07, - 7.1505019336171038447e-08, 2.5546009342625186633e-07, -0.57735026918958520792; + 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); @@ -418,9 +431,8 @@ TEST(MarginalizeVariables, MarginalizeNext) expected.variables.push_back(3); auto A_expected = fuse_core::MatrixXd(3, 3); /* *INDENT-OFF* */ - A_expected << -0.686835139329528, 0.064384601986636, 0.000000153209328, - 0.000000000000000, -0.509885650799691, 0.000000079984512, - 0.000000000000000, 0.000000000000000, 0.408248290463911; + 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); auto b_expected = fuse_core::VectorXd(3); @@ -465,29 +477,28 @@ TEST(MarginalizeVariables, MarginalizeVariables) mean1 << 0.92736185, 0.1, 0.2, 0.3; fuse_core::Matrix3d cov1; cov1 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto prior_x1 = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( - "test", *x1, mean1, cov1); + auto prior_x1 = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared("test", *x1, mean1, cov1); fuse_core::Vector4d delta2; delta2 << 0.979795897, 0.0, 0.0, 0.2; fuse_core::Matrix3d cov2; cov2 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto relative_x1_x2 = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", *x1, *x2, delta2, cov2); + auto relative_x1_x2 = + fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x1, *x2, delta2, cov2); fuse_core::Vector4d delta3; delta3 << 0.979795897, 0.0, 0.0, 0.2; fuse_core::Matrix3d cov3; cov3 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto relative_x2_x3 = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", *x2, *x3, delta3, cov3); + auto relative_x2_x3 = + fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x2, *x3, delta3, cov3); fuse_core::Vector4d delta4; delta4 << 0.979795897, 0.2, 0.0, 0.0; fuse_core::Matrix3d cov4; cov4 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto relative_x2_l1 = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", *x2, *l1, delta4, cov4); + auto relative_x2_l1 = + fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x2, *l1, delta4, cov4); // Add to the graph auto graph = fuse_graphs::HashGraph(); @@ -508,26 +519,24 @@ TEST(MarginalizeVariables, MarginalizeVariables) auto expected_x3 = x3->array(); auto expected_l1 = l1->array(); - auto requests = std::vector> - { - {x2->uuid(), x2->uuid()}, {x3->uuid(), x3->uuid()}, {l1->uuid(), l1->uuid()} - }; + auto requests = std::vector>{ { x2->uuid(), x2->uuid() }, + { x3->uuid(), x3->uuid() }, + { l1->uuid(), l1->uuid() } }; auto expected_covariances = std::vector>(); graph.getCovariance(requests, expected_covariances); - const auto & expected_x2_cov = expected_covariances[0]; - const auto & expected_x3_cov = expected_covariances[1]; - const auto & expected_l1_cov = expected_covariances[2]; + const auto& expected_x2_cov = expected_covariances[0]; + const auto& expected_x3_cov = expected_covariances[1]; + const auto& expected_l1_cov = expected_covariances[2]; // Marginalize out X1 - auto transaction = fuse_constraints::marginalizeVariables("test", {x1->uuid()}, graph); // NOLINT + auto transaction = fuse_constraints::marginalizeVariables("test", { x1->uuid() }, graph); // NOLINT // Verify the computed transaction auto added_variables = transaction.addedVariables(); EXPECT_EQ(0u, std::distance(added_variables.begin(), added_variables.end())); auto removed_variables_range = transaction.removedVariables(); - auto removed_variables = std::set( - removed_variables_range.begin(), removed_variables_range.end()); + auto removed_variables = std::set(removed_variables_range.begin(), removed_variables_range.end()); EXPECT_EQ(1u, removed_variables.size()); EXPECT_TRUE(removed_variables.count(x1->uuid())); @@ -535,9 +544,8 @@ TEST(MarginalizeVariables, MarginalizeVariables) EXPECT_EQ(1u, std::distance(added_constraints.begin(), added_constraints.end())); auto removed_constraints_range = transaction.removedConstraints(); - auto removed_constraints = std::set( - removed_constraints_range.begin(), - removed_constraints_range.end()); + auto removed_constraints = + std::set(removed_constraints_range.begin(), removed_constraints_range.end()); EXPECT_EQ(2u, removed_constraints.size()); EXPECT_TRUE(removed_constraints.count(prior_x1->uuid())); EXPECT_TRUE(removed_constraints.count(relative_x1_x2->uuid())); @@ -555,35 +563,41 @@ TEST(MarginalizeVariables, MarginalizeVariables) auto actual_covariances = std::vector>(); graph.getCovariance(requests, actual_covariances); - const auto & actual_x2_cov = actual_covariances[0]; - const auto & actual_x3_cov = actual_covariances[1]; - const auto & actual_l1_cov = actual_covariances[2]; + const auto& actual_x2_cov = actual_covariances[0]; + const auto& actual_x3_cov = actual_covariances[1]; + const auto& actual_l1_cov = actual_covariances[2]; // Compare. The post-marginal results should be identical to the pre-marginal results ASSERT_EQ(expected_x2.size(), actual_x2.size()); - for (size_t i = 0; i < expected_x2.size(); ++i) { + for (size_t i = 0; i < expected_x2.size(); ++i) + { EXPECT_NEAR(expected_x2[i], actual_x2[i], 1.0e-5); } ASSERT_EQ(expected_x2_cov.size(), actual_x2_cov.size()); - for (size_t i = 0; i < expected_x2_cov.size(); ++i) { + for (size_t i = 0; i < expected_x2_cov.size(); ++i) + { EXPECT_NEAR(expected_x2_cov[i], actual_x2_cov[i], 1.0e-5); } ASSERT_EQ(expected_x3.size(), actual_x3.size()); - for (size_t i = 0; i < expected_x3.size(); ++i) { + for (size_t i = 0; i < expected_x3.size(); ++i) + { EXPECT_NEAR(expected_x3[i], actual_x3[i], 1.0e-5); } ASSERT_EQ(expected_x3_cov.size(), actual_x3_cov.size()); - for (size_t i = 0; i < expected_x3_cov.size(); ++i) { + for (size_t i = 0; i < expected_x3_cov.size(); ++i) + { EXPECT_NEAR(expected_x3_cov[i], actual_x3_cov[i], 1.0e-5); } ASSERT_EQ(expected_l1.size(), actual_l1.size()); - for (size_t i = 0; i < expected_l1.size(); ++i) { + for (size_t i = 0; i < expected_l1.size(); ++i) + { EXPECT_NEAR(expected_l1[i], actual_l1[i], 1.0e-5); } ASSERT_EQ(expected_l1_cov.size(), actual_l1_cov.size()); - for (size_t i = 0; i < expected_l1_cov.size(); ++i) { + for (size_t i = 0; i < expected_l1_cov.size(); ++i) + { EXPECT_NEAR(expected_l1_cov[i], actual_l1_cov[i], 1.0e-5); } } @@ -617,8 +631,7 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables) mean1 << 0.92736185, 0.1, 0.2, 0.3; fuse_core::Matrix3d cov1; cov1 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto prior_x1 = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( - "test", *x1, mean1, cov1); + auto prior_x1 = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared("test", *x1, mean1, cov1); // Note that this prior on the landmark is required. The covariance of the prior has no impact on // the solution, as the value of the landmark will be held constant. However, due to assumptions @@ -628,29 +641,28 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables) mean2 << 0.842614977, 0.2, 0.3, 0.4; fuse_core::Matrix3d cov2; cov2 << 3.0, 0.0, 0.0, 0.0, 3.1, 0.0, 0.0, 0.0, 3.2; - auto prior_l1 = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( - "test", *l1, mean2, cov2); + auto prior_l1 = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared("test", *l1, mean2, cov2); fuse_core::Vector4d delta3; delta3 << 0.979795897, 0.0, 0.0, 0.2; fuse_core::Matrix3d cov3; cov3 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto relative_x1_x2 = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", *x1, *x2, delta3, cov3); + auto relative_x1_x2 = + fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x1, *x2, delta3, cov3); fuse_core::Vector4d delta4; delta4 << 0.979795897, 0.0, 0.0, 0.2; fuse_core::Matrix3d cov4; cov4 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto relative_x2_x3 = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", *x2, *x3, delta4, cov4); + auto relative_x2_x3 = + fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x2, *x3, delta4, cov4); fuse_core::Vector4d delta5; delta5 << 0.979795897, 0.2, 0.0, 0.0; fuse_core::Matrix3d cov5; cov5 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto relative_x1_l1 = fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared( - "test", *x1, *l1, delta5, cov5); + auto relative_x1_l1 = + fuse_constraints::RelativeOrientation3DStampedConstraint::make_shared("test", *x1, *l1, delta5, cov5); // Add to the graph auto graph = fuse_graphs::HashGraph(); @@ -671,26 +683,22 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables) auto expected_x2 = x2->array(); auto expected_x3 = x3->array(); - auto requests = std::vector> - { - {x2->uuid(), x2->uuid()}, {x3->uuid(), x3->uuid()} - }; + auto requests = std::vector>{ { x2->uuid(), x2->uuid() }, + { x3->uuid(), x3->uuid() } }; auto expected_covariances = std::vector>(); graph.getCovariance(requests, expected_covariances); - const auto & expected_x2_cov = expected_covariances[0]; - const auto & expected_x3_cov = expected_covariances[1]; + const auto& expected_x2_cov = expected_covariances[0]; + const auto& expected_x3_cov = expected_covariances[1]; // Marginalize out X1 and L1 - auto transaction = - fuse_constraints::marginalizeVariables("test", {x1->uuid(), l1->uuid()}, graph); // NOLINT + auto transaction = fuse_constraints::marginalizeVariables("test", { x1->uuid(), l1->uuid() }, graph); // NOLINT // Verify the computed transaction auto added_variables = transaction.addedVariables(); EXPECT_EQ(0u, std::distance(added_variables.begin(), added_variables.end())); auto removed_variables_range = transaction.removedVariables(); - auto removed_variables = std::set( - removed_variables_range.begin(), removed_variables_range.end()); + auto removed_variables = std::set(removed_variables_range.begin(), removed_variables_range.end()); EXPECT_EQ(2u, removed_variables.size()); EXPECT_TRUE(removed_variables.count(x1->uuid())); EXPECT_TRUE(removed_variables.count(l1->uuid())); @@ -699,9 +707,8 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables) EXPECT_EQ(1u, std::distance(added_constraints.begin(), added_constraints.end())); auto removed_constraints_range = transaction.removedConstraints(); - auto removed_constraints = std::set( - removed_constraints_range.begin(), - removed_constraints_range.end()); + auto removed_constraints = + std::set(removed_constraints_range.begin(), removed_constraints_range.end()); EXPECT_EQ(4u, removed_constraints.size()); EXPECT_TRUE(removed_constraints.count(prior_x1->uuid())); @@ -721,25 +728,29 @@ TEST(MarginalizeVariables, MarginalizeFixedVariables) auto actual_covariances = std::vector>(); graph.getCovariance(requests, actual_covariances); - const auto & actual_x2_cov = actual_covariances[0]; - const auto & actual_x3_cov = actual_covariances[1]; + const auto& actual_x2_cov = actual_covariances[0]; + const auto& actual_x3_cov = actual_covariances[1]; // Compare. The post-marginal results should be identical to the pre-marginal results ASSERT_EQ(expected_x2.size(), actual_x2.size()); - for (size_t i = 0; i < expected_x2.size(); ++i) { + for (size_t i = 0; i < expected_x2.size(); ++i) + { EXPECT_NEAR(expected_x2[i], actual_x2[i], 1.0e-3); } ASSERT_EQ(expected_x2_cov.size(), actual_x2_cov.size()); - for (size_t i = 0; i < expected_x2_cov.size(); ++i) { + for (size_t i = 0; i < expected_x2_cov.size(); ++i) + { EXPECT_NEAR(expected_x2_cov[i], actual_x2_cov[i], 1.0e-3); } ASSERT_EQ(expected_x3.size(), actual_x3.size()); - for (size_t i = 0; i < expected_x3.size(); ++i) { + for (size_t i = 0; i < expected_x3.size(); ++i) + { EXPECT_NEAR(expected_x3[i], actual_x3[i], 1.0e-3); } ASSERT_EQ(expected_x3_cov.size(), actual_x3_cov.size()); - for (size_t i = 0; i < expected_x3_cov.size(); ++i) { + for (size_t i = 0; i < expected_x3_cov.size(); ++i) + { EXPECT_NEAR(expected_x3_cov[i], actual_x3_cov[i], 1.0e-3); } } diff --git a/fuse_constraints/test/test_normal_delta_pose_2d.cpp b/fuse_constraints/test/test_normal_delta_pose_2d.cpp index 71ba0641d..45f0f5732 100644 --- a/fuse_constraints/test/test_normal_delta_pose_2d.cpp +++ b/fuse_constraints/test/test_normal_delta_pose_2d.cpp @@ -51,8 +51,7 @@ class NormalDeltaPose2DTestFixture : public ::testing::Test public: //!< The automatic differentiation cost function type for the pose 2d cost functor using AutoDiffNormalDeltaPose2D = - ceres::AutoDiffCostFunction; + ceres::AutoDiffCostFunction; /** * @brief Constructor @@ -63,25 +62,24 @@ class NormalDeltaPose2DTestFixture : public ::testing::Test } const fuse_core::Matrix3d covariance = - fuse_core::Vector3d(2e-3, 1e-3, 1e-2).asDiagonal(); //!< The full pose 2d covariance for the x, - //!< y and yaw components - Eigen::Matrix3d full_sqrt_information; //!< The full pose 2d sqrt information matrix for the x, y - //!< and yaw components - const Eigen::Vector3d full_delta{1.0, 2.0, 3.0}; //!< The full pose 2d delta components: x, y and - //!< yaw + fuse_core::Vector3d(2e-3, 1e-3, 1e-2).asDiagonal(); //!< The full pose 2d covariance for the x, + //!< y and yaw components + Eigen::Matrix3d full_sqrt_information; //!< The full pose 2d sqrt information matrix for the x, y + //!< and yaw components + const Eigen::Vector3d full_delta{ 1.0, 2.0, 3.0 }; //!< The full pose 2d delta components: x, y and + //!< yaw }; TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForFullResiduals) { // Create cost function - const fuse_constraints::NormalDeltaPose2D cost_function{full_sqrt_information, full_delta}; + const fuse_constraints::NormalDeltaPose2D cost_function{ full_sqrt_information, full_delta }; // Create automatic differentiation cost function const auto num_residuals = full_sqrt_information.rows(); AutoDiffNormalDeltaPose2D autodiff_cost_function( - new fuse_constraints::NormalDeltaPose2DCostFunctor(full_sqrt_information, full_delta), - num_residuals); + new fuse_constraints::NormalDeltaPose2DCostFunctor(full_sqrt_information, full_delta), num_residuals); // Compare the expected, automatic differentiation, cost function and the actual one ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function); @@ -92,9 +90,9 @@ TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor // Create cost function for each possible pair of two residuals, the ones in each possible pair of // rows using IndicesPair = std::array; - std::array indices_pairs = {IndicesPair{0, 1}, IndicesPair{0, 2}, IndicesPair{1, 2}}; - for (const auto & indices_pair : indices_pairs) { + std::array indices_pairs = { IndicesPair{ 0, 1 }, IndicesPair{ 0, 2 }, IndicesPair{ 1, 2 } }; + for (const auto& indices_pair : indices_pairs) + { // It is a shame we need Eigen 3.4+ in order to use the slicing and indexing API documented in: // // https://eigen.tuxfamily.org/dox-devel/group__TutorialSlicingIndexing.html @@ -104,18 +102,18 @@ TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor // const fuse_core::Matrix partial_sqrt_information = // full_sqrt_information(indices_pair, Eigen::all); fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices_pair.size(); ++i) { + for (size_t i = 0; i < indices_pair.size(); ++i) + { partial_sqrt_information.row(i) = full_sqrt_information.row(indices_pair[i]); } - const fuse_constraints::NormalDeltaPose2D cost_function{partial_sqrt_information, full_delta}; + const fuse_constraints::NormalDeltaPose2D cost_function{ partial_sqrt_information, full_delta }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalDeltaPose2D autodiff_cost_function( - new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, full_delta), - num_residuals); + new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, full_delta), num_residuals); ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function); } @@ -124,19 +122,19 @@ TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor TEST_F(NormalDeltaPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForOneResidual) { // Create cost function for one residual, the one in each row - for (size_t i = 0; i < 3; ++i) { + for (size_t i = 0; i < 3; ++i) + { SCOPED_TRACE("Residual " + std::to_string(i)); const fuse_core::Matrix partial_sqrt_information = full_sqrt_information.row(i); - const fuse_constraints::NormalDeltaPose2D cost_function{partial_sqrt_information, full_delta}; + const fuse_constraints::NormalDeltaPose2D cost_function{ partial_sqrt_information, full_delta }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalDeltaPose2D autodiff_cost_function( - new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, full_delta), - num_residuals); + new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, full_delta), num_residuals); ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function); } diff --git a/fuse_constraints/test/test_normal_prior_pose_2d.cpp b/fuse_constraints/test/test_normal_prior_pose_2d.cpp index 151e3172d..605c228b5 100644 --- a/fuse_constraints/test/test_normal_prior_pose_2d.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_2d.cpp @@ -51,8 +51,7 @@ class NormalPriorPose2DTestFixture : public ::testing::Test public: //!< The automatic differentiation cost function type for the pose 2d cost functor using AutoDiffNormalPriorPose2D = - ceres::AutoDiffCostFunction; + ceres::AutoDiffCostFunction; /** * @brief Constructor @@ -63,25 +62,24 @@ class NormalPriorPose2DTestFixture : public ::testing::Test } const fuse_core::Matrix3d covariance = - fuse_core::Vector3d(2e-3, 1e-3, 1e-2).asDiagonal(); //!< The full pose 2d covariance for the x, - //!< y and yaw components - Eigen::Matrix3d full_sqrt_information; //!< The full pose 2d sqrt information matrix for the x, y - //!< and yaw components - const Eigen::Vector3d full_mean{1.0, 2.0, 3.0}; //!< The full pose 2d mean components: x, y and - //!< yaw + fuse_core::Vector3d(2e-3, 1e-3, 1e-2).asDiagonal(); //!< The full pose 2d covariance for the x, + //!< y and yaw components + Eigen::Matrix3d full_sqrt_information; //!< The full pose 2d sqrt information matrix for the x, y + //!< and yaw components + const Eigen::Vector3d full_mean{ 1.0, 2.0, 3.0 }; //!< The full pose 2d mean components: x, y and + //!< yaw }; TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForFullResiduals) { // Create cost function - const fuse_constraints::NormalPriorPose2D cost_function{full_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose2D cost_function{ full_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = full_sqrt_information.rows(); AutoDiffNormalPriorPose2D autodiff_cost_function( - new fuse_constraints::NormalPriorPose2DCostFunctor(full_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose2DCostFunctor(full_sqrt_information, full_mean), num_residuals); // Compare the expected, automatic differentiation, cost function and the actual one ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function); @@ -92,9 +90,9 @@ TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor // Create cost function for each possible pair of two residuals, the ones in each possible pair of // rows using IndicesPair = std::array; - std::array indices_pairs = {IndicesPair{0, 1}, IndicesPair{0, 2}, IndicesPair{1, 2}}; - for (const auto & indices_pair : indices_pairs) { + std::array indices_pairs = { IndicesPair{ 0, 1 }, IndicesPair{ 0, 2 }, IndicesPair{ 1, 2 } }; + for (const auto& indices_pair : indices_pairs) + { // It is a shame we need Eigen 3.4+ in order to use the slicing and indexing API documented in: // // https://eigen.tuxfamily.org/dox-devel/group__TutorialSlicingIndexing.html @@ -104,18 +102,18 @@ TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor // const fuse_core::Matrix partial_sqrt_information = // full_sqrt_information(indices_pair, Eigen::all); fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices_pair.size(); ++i) { + for (size_t i = 0; i < indices_pair.size(); ++i) + { partial_sqrt_information.row(i) = full_sqrt_information.row(indices_pair[i]); } - const fuse_constraints::NormalPriorPose2D cost_function{partial_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose2D cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose2D autodiff_cost_function( - new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, full_mean), num_residuals); ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function); } @@ -124,19 +122,19 @@ TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualFor TEST_F(NormalPriorPose2DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForOneResidual) { // Create cost function for one residual, the one in each row - for (size_t i = 0; i < 3; ++i) { + for (size_t i = 0; i < 3; ++i) + { SCOPED_TRACE("Residual " + std::to_string(i)); const fuse_core::Matrix partial_sqrt_information = full_sqrt_information.row(i); - const fuse_constraints::NormalPriorPose2D cost_function{partial_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose2D cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose2D autodiff_cost_function( - new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, full_mean), num_residuals); ExpectCostFunctionsAreEqual(autodiff_cost_function, cost_function); } diff --git a/fuse_constraints/test/test_relative_constraint.cpp b/fuse_constraints/test/test_relative_constraint.cpp index 721fba58a..40b08c4ac 100644 --- a/fuse_constraints/test/test_relative_constraint.cpp +++ b/fuse_constraints/test/test_relative_constraint.cpp @@ -58,98 +58,69 @@ TEST(RelativeConstraint, Constructor) { // Construct a constraint for every type, just to make sure they compile. { - fuse_variables::AccelerationAngular2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("robby")); - fuse_variables::AccelerationAngular2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("robby")); + fuse_variables::AccelerationAngular2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby")); + fuse_variables::AccelerationAngular2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("robby")); fuse_core::VectorXd delta(1); delta << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; EXPECT_NO_THROW( - fuse_constraints::RelativeAccelerationAngular2DStampedConstraint constraint( - "test", x1, x2, - delta, cov)); + fuse_constraints::RelativeAccelerationAngular2DStampedConstraint constraint("test", x1, x2, delta, cov)); } { - fuse_variables::AccelerationLinear2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("bender")); - fuse_variables::AccelerationLinear2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("bender")); + fuse_variables::AccelerationLinear2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("bender")); + fuse_variables::AccelerationLinear2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("bender")); fuse_core::VectorXd delta(2); delta << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; EXPECT_NO_THROW( - fuse_constraints::RelativeAccelerationLinear2DStampedConstraint constraint( - "test", x1, x2, - delta, cov)); + fuse_constraints::RelativeAccelerationLinear2DStampedConstraint constraint("test", x1, x2, delta, cov)); } { - fuse_variables::Orientation2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("johnny5")); - fuse_variables::Orientation2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("johnny5")); + fuse_variables::Orientation2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("johnny5")); + fuse_variables::Orientation2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("johnny5")); fuse_core::VectorXd delta(1); delta << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - EXPECT_NO_THROW( - fuse_constraints::RelativeOrientation2DStampedConstraint constraint( - "test", x1, x2, delta, - cov)); + EXPECT_NO_THROW(fuse_constraints::RelativeOrientation2DStampedConstraint constraint("test", x1, x2, delta, cov)); } { - fuse_variables::Position2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "rosie")); - fuse_variables::Position2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate( - "rosie")); + fuse_variables::Position2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("rosie")); + fuse_variables::Position2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("rosie")); fuse_core::VectorXd delta(2); delta << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; - EXPECT_NO_THROW( - fuse_constraints::RelativePosition2DStampedConstraint constraint("test", x1, x2, delta, cov)); + EXPECT_NO_THROW(fuse_constraints::RelativePosition2DStampedConstraint constraint("test", x1, x2, delta, cov)); } { - fuse_variables::Position3DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "clank")); - fuse_variables::Position3DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate( - "clank")); + fuse_variables::Position3DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("clank")); + fuse_variables::Position3DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("clank")); fuse_core::VectorXd delta(3); delta << 1.0, 2.0, 3.0; fuse_core::MatrixXd cov(3, 3); cov << 1.0, 0.1, 0.2, 0.3, 2.0, 0.3, 0.2, 0.3, 3.0; - EXPECT_NO_THROW( - fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov)); + EXPECT_NO_THROW(fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov)); } { - fuse_variables::VelocityAngular2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("gort")); - fuse_variables::VelocityAngular2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("gort")); + fuse_variables::VelocityAngular2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("gort")); + fuse_variables::VelocityAngular2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("gort")); fuse_core::VectorXd delta(1); delta << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - EXPECT_NO_THROW( - fuse_constraints::RelativeVelocityAngular2DStampedConstraint constraint( - "test", x1, x2, delta, - cov)); + EXPECT_NO_THROW(fuse_constraints::RelativeVelocityAngular2DStampedConstraint constraint("test", x1, x2, delta, cov)); } { - fuse_variables::VelocityLinear2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("bishop")); - fuse_variables::VelocityLinear2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("bishop")); + fuse_variables::VelocityLinear2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("bishop")); + fuse_variables::VelocityLinear2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("bishop")); fuse_core::VectorXd delta(2); delta << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; - EXPECT_NO_THROW( - fuse_constraints::RelativeVelocityLinear2DStampedConstraint constraint( - "test", x1, x2, delta, - cov)); + EXPECT_NO_THROW(fuse_constraints::RelativeVelocityLinear2DStampedConstraint constraint("test", x1, x2, delta, cov)); } } @@ -161,11 +132,8 @@ TEST(RelativeConstraint, PartialMeasurement) delta << 3.0, 1.0; fuse_core::MatrixXd cov(2, 2); cov << 3.0, 0.2, 0.2, 1.0; - auto indices = std::vector{2, 0}; - EXPECT_NO_THROW( - fuse_constraints::RelativePosition3DStampedConstraint constraint( - "test", x1, x2, delta, cov, - indices)); + auto indices = std::vector{ 2, 0 }; + EXPECT_NO_THROW(fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov, indices)); } TEST(RelativeConstraint, Covariance) @@ -173,20 +141,16 @@ TEST(RelativeConstraint, Covariance) // Test the covariance of a full measurement { // Verify the covariance <--> sqrt information conversions are correct - fuse_variables::AccelerationLinear2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("chappie")); - fuse_variables::AccelerationLinear2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("chappie")); + fuse_variables::AccelerationLinear2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("chappie")); + fuse_variables::AccelerationLinear2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("chappie")); fuse_core::VectorXd delta(2); delta << 1.0, 2.0; fuse_core::MatrixXd cov(2, 2); cov << 1.0, 0.1, 0.1, 2.0; - fuse_constraints::RelativeAccelerationLinear2DStampedConstraint constraint("test", x1, x2, - delta, cov); + fuse_constraints::RelativeAccelerationLinear2DStampedConstraint constraint("test", x1, x2, delta, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix2d expected_sqrt_info; - expected_sqrt_info << 1.002509414234171, -0.050125470711709, - 0.000000000000000, 0.707106781186547; + expected_sqrt_info << 1.002509414234171, -0.050125470711709, 0.000000000000000, 0.707106781186547; fuse_core::Matrix2d expected_cov = cov; // Compare EXPECT_TRUE(expected_cov.isApprox(constraint.covariance(), 1.0e-9)); @@ -194,17 +158,14 @@ TEST(RelativeConstraint, Covariance) } // Test the covariance of a partial measurement { - fuse_variables::Position3DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("astroboy")); - fuse_variables::Position3DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("astroboy")); + fuse_variables::Position3DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("astroboy")); + fuse_variables::Position3DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("astroboy")); fuse_core::VectorXd delta(2); delta << 3.0, 1.0; fuse_core::MatrixXd cov(2, 2); cov << 3.0, 0.2, 0.2, 1.0; - auto indices = std::vector{2, 0}; - fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov, - indices); + auto indices = std::vector{ 2, 0 }; + fuse_constraints::RelativePosition3DStampedConstraint constraint("test", x1, x2, delta, cov, indices); // Define the expected matrices fuse_core::Vector3d expected_delta; expected_delta << 1.0, 0.0, 3.0; @@ -212,8 +173,8 @@ TEST(RelativeConstraint, Covariance) expected_cov << 1.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.2, 0.0, 3.0; fuse_core::MatrixXd expected_sqrt_info(2, 3); /* *INDENT-OFF* */ - expected_sqrt_info << -0.116247638743819, 0.000000000000000, 0.581238193719096, - 1.000000000000000, 0.000000000000000, 0.000000000000000; + expected_sqrt_info << -0.116247638743819, 0.000000000000000, 0.581238193719096, 1.000000000000000, + 0.000000000000000, 0.000000000000000; /* *INDENT-ON* */ // Compare EXPECT_TRUE(expected_delta.isApprox(constraint.delta(), 1.0e-9)); @@ -229,14 +190,12 @@ TEST(RelativeConstraint, Optimization) // Optimize a two-variable system with a prior on the first variable and a relative constraint // connecting the two. Verify the expected value and covariance are generated. Create the // variables - auto x1 = fuse_variables::AccelerationLinear2DStamped::make_shared( - rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("t800")); + auto x1 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("t800")); x1->x() = 10.7; x1->y() = -3.2; - auto x2 = fuse_variables::AccelerationLinear2DStamped::make_shared( - rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("t800")); + auto x2 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(1235, 5678), + fuse_core::uuid::generate("t800")); x2->x() = -4.2; x2->y() = 1.9; // Create an absolute constraint @@ -244,55 +203,37 @@ TEST(RelativeConstraint, Optimization) mean << 1.0, 2.0; fuse_core::MatrixXd cov1(2, 2); cov1 << 1.0, 0.1, 0.1, 2.0; - auto prior = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( - "test", - *x1, - mean, - cov1); + auto prior = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared("test", *x1, mean, cov1); // Create an relative constraint fuse_core::VectorXd delta(2); delta << 0.1, 0.2; fuse_core::MatrixXd cov2(2, 2); cov2 << 1.0, 0.0, 0.0, 2.0; - auto relative = fuse_constraints::RelativeAccelerationLinear2DStampedConstraint::make_shared( - "test", - *x1, - *x2, - delta, - cov2); + auto relative = + fuse_constraints::RelativeAccelerationLinear2DStampedConstraint::make_shared("test", *x1, *x2, delta, cov2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - x1->data(), - x1->size(), + problem.AddParameterBlock(x1->data(), x1->size(), #if !CERES_SUPPORTS_MANIFOLDS - x1->localParameterization()); + x1->localParameterization()); #else - x1->manifold()); + x1->manifold()); #endif - problem.AddParameterBlock( - x2->data(), - x2->size(), + problem.AddParameterBlock(x2->data(), x2->size(), #if !CERES_SUPPORTS_MANIFOLDS - x2->localParameterization()); + x2->localParameterization()); #else - x2->manifold()); + x2->manifold()); #endif - std::vector prior_parameter_blocks; + std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(x1->data()); - problem.AddResidualBlock( - prior->costFunction(), - prior->lossFunction(), - prior_parameter_blocks); - std::vector relative_parameter_blocks; + problem.AddResidualBlock(prior->costFunction(), prior->lossFunction(), prior_parameter_blocks); + std::vector relative_parameter_blocks; relative_parameter_blocks.push_back(x1->data()); relative_parameter_blocks.push_back(x2->data()); - problem.AddResidualBlock( - relative->costFunction(), - relative->lossFunction(), - relative_parameter_blocks); + problem.AddResidualBlock(relative->costFunction(), relative->lossFunction(), relative_parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -303,7 +244,7 @@ TEST(RelativeConstraint, Optimization) EXPECT_NEAR(1.1, x2->x(), 1.0e-5); EXPECT_NEAR(2.2, x2->y(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(x1->data(), x1->data()); covariance_blocks.emplace_back(x2->data(), x2->data()); ceres::Covariance::Options cov_options; @@ -331,15 +272,13 @@ TEST(RelativeConstraint, Optimization) // Optimize a two-variable system with a prior on the first variable and a relative constraint // connecting the two. Verify the expected value and covariance are generated. Create the // variables - auto x1 = fuse_variables::Position3DStamped::make_shared( - rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("t1000")); + auto x1 = + fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("t1000")); x1->x() = 10.7; x1->y() = -3.2; x1->z() = 0.4; - auto x2 = fuse_variables::Position3DStamped::make_shared( - rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("t1000")); + auto x2 = + fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("t1000")); x2->x() = -4.2; x2->y() = 1.9; x2->z() = 19.2; @@ -348,75 +287,48 @@ TEST(RelativeConstraint, Optimization) mean1 << 1.0, 2.0, 3.0; fuse_core::MatrixXd cov1(3, 3); cov1 << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - auto c1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( - "test", - *x1, - mean1, - cov1); + auto c1 = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared("test", *x1, mean1, cov1); // Create an relative constraint fuse_core::VectorXd delta2(3); delta2 << 0.1, 0.2, 0.3; fuse_core::MatrixXd cov2(3, 3); cov2 << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; - auto c2 = fuse_constraints::RelativePosition3DStampedConstraint::make_shared( - "test", - *x1, - *x2, - delta2, - cov2); + auto c2 = fuse_constraints::RelativePosition3DStampedConstraint::make_shared("test", *x1, *x2, delta2, cov2); // Create an partial relative constraint fuse_core::VectorXd delta3(2); delta3 << 0.1, 0.2; fuse_core::MatrixXd cov3(2, 2); cov3 << 1.0, 0.0, 0.0, 3.0; - auto indices3 = std::vector{2, 0}; - auto c3 = fuse_constraints::RelativePosition3DStampedConstraint::make_shared( - "test", - *x1, - *x2, - delta3, - cov3, - indices3); + auto indices3 = std::vector{ 2, 0 }; + auto c3 = + fuse_constraints::RelativePosition3DStampedConstraint::make_shared("test", *x1, *x2, delta3, cov3, indices3); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - x1->data(), - x1->size(), + problem.AddParameterBlock(x1->data(), x1->size(), #if !CERES_SUPPORTS_MANIFOLDS - x1->localParameterization()); + x1->localParameterization()); #else - x1->manifold()); + x1->manifold()); #endif - problem.AddParameterBlock( - x2->data(), - x2->size(), + problem.AddParameterBlock(x2->data(), x2->size(), #if !CERES_SUPPORTS_MANIFOLDS - x2->localParameterization()); + x2->localParameterization()); #else - x2->manifold()); + x2->manifold()); #endif - std::vector c1_parameter_blocks; + std::vector c1_parameter_blocks; c1_parameter_blocks.push_back(x1->data()); - problem.AddResidualBlock( - c1->costFunction(), - c1->lossFunction(), - c1_parameter_blocks); - std::vector c2_parameter_blocks; + problem.AddResidualBlock(c1->costFunction(), c1->lossFunction(), c1_parameter_blocks); + std::vector c2_parameter_blocks; c2_parameter_blocks.push_back(x1->data()); c2_parameter_blocks.push_back(x2->data()); - problem.AddResidualBlock( - c2->costFunction(), - c2->lossFunction(), - c2_parameter_blocks); - std::vector c3_parameter_blocks; + problem.AddResidualBlock(c2->costFunction(), c2->lossFunction(), c2_parameter_blocks); + std::vector c3_parameter_blocks; c3_parameter_blocks.push_back(x1->data()); c3_parameter_blocks.push_back(x2->data()); - problem.AddResidualBlock( - c3->costFunction(), - c3->lossFunction(), - c3_parameter_blocks); + problem.AddResidualBlock(c3->costFunction(), c3->lossFunction(), c3_parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -429,7 +341,7 @@ TEST(RelativeConstraint, Optimization) EXPECT_NEAR(2.2, x2->y(), 1.0e-5); EXPECT_NEAR(3.15, x2->z(), 1.0e-5); // Compute the marginal covariances - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(x1->data(), x1->data()); covariance_blocks.emplace_back(x2->data(), x2->data()); ceres::Covariance::Options cov_options; @@ -457,68 +369,47 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) // Optimize a two-variable system with a prior on the first variable and a relative constraint // connecting the two. Verify the expected value and covariance are generated. Create the // variables - auto x1 = fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("t800")); + auto x1 = + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("t800")); x1->yaw() = 0.7; - auto x2 = fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("t800")); + auto x2 = + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("t800")); x2->yaw() = -2.2; // Create an absolute constraint fuse_core::VectorXd mean(1); mean << 1.0; fuse_core::MatrixXd cov1(1, 1); cov1 << 2.0; - auto prior = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared( - "test", - *x1, - mean, - cov1); + auto prior = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared("test", *x1, mean, cov1); // Create an relative constraint fuse_core::VectorXd delta(1); delta << 0.1; fuse_core::MatrixXd cov2(1, 1); cov2 << 1.0; - auto relative = fuse_constraints::RelativeOrientation2DStampedConstraint::make_shared( - "test", - *x1, - *x2, - delta, - cov2); + auto relative = fuse_constraints::RelativeOrientation2DStampedConstraint::make_shared("test", *x1, *x2, delta, cov2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - x1->data(), - x1->size(), + problem.AddParameterBlock(x1->data(), x1->size(), #if !CERES_SUPPORTS_MANIFOLDS - x1->localParameterization()); + x1->localParameterization()); #else - x1->manifold()); + x1->manifold()); #endif - problem.AddParameterBlock( - x2->data(), - x2->size(), + problem.AddParameterBlock(x2->data(), x2->size(), #if !CERES_SUPPORTS_MANIFOLDS - x2->localParameterization()); + x2->localParameterization()); #else - x2->manifold()); + x2->manifold()); #endif - std::vector prior_parameter_blocks; + std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(x1->data()); - problem.AddResidualBlock( - prior->costFunction(), - prior->lossFunction(), - prior_parameter_blocks); - std::vector relative_parameter_blocks; + problem.AddResidualBlock(prior->costFunction(), prior->lossFunction(), prior_parameter_blocks); + std::vector relative_parameter_blocks; relative_parameter_blocks.push_back(x1->data()); relative_parameter_blocks.push_back(x2->data()); - problem.AddResidualBlock( - relative->costFunction(), - relative->lossFunction(), - relative_parameter_blocks); + problem.AddResidualBlock(relative->costFunction(), relative->lossFunction(), relative_parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -527,7 +418,7 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) EXPECT_NEAR(1.0, x1->yaw(), 1.0e-5); EXPECT_NEAR(1.1, x2->yaw(), 1.0e-5); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(x1->data(), x1->data()); covariance_blocks.emplace_back(x2->data(), x2->data()); ceres::Covariance::Options cov_options; @@ -552,16 +443,13 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) TEST(RelativeConstraint, Serialization) { // Construct a constraint - fuse_variables::AccelerationAngular2DStamped x1(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("robby")); - fuse_variables::AccelerationAngular2DStamped x2(rclcpp::Time(1235, 5678), - fuse_core::uuid::generate("robby")); + fuse_variables::AccelerationAngular2DStamped x1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("robby")); + fuse_variables::AccelerationAngular2DStamped x2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("robby")); fuse_core::VectorXd delta(1); delta << 3.0; fuse_core::MatrixXd cov(1, 1); cov << 1.0; - fuse_constraints::RelativeAccelerationAngular2DStampedConstraint expected("test", x1, x2, delta, - cov); + fuse_constraints::RelativeAccelerationAngular2DStampedConstraint expected("test", x1, x2, delta, cov); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp b/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp index 014ad5387..91b2a85d2 100644 --- a/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp @@ -49,11 +49,10 @@ #include #include -using fuse_variables::Orientation2DStamped; -using fuse_variables::Position2DStamped; using fuse_constraints::AbsolutePose2DStampedConstraint; using fuse_constraints::RelativePose2DStampedConstraint; - +using fuse_variables::Orientation2DStamped; +using fuse_variables::Position2DStamped; TEST(RelativePose2DStampedConstraint, Constructor) { @@ -67,9 +66,7 @@ TEST(RelativePose2DStampedConstraint, Constructor) fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; EXPECT_NO_THROW( - RelativePose2DStampedConstraint constraint( - "test", position1, orientation1, position2, - orientation2, delta, cov)); + RelativePose2DStampedConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov)); } TEST(RelativePose2DStampedConstraint, Covariance) @@ -83,20 +80,12 @@ TEST(RelativePose2DStampedConstraint, Covariance) delta << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - RelativePose2DStampedConstraint constraint( - "test", - position1, - orientation1, - position2, - orientation2, - delta, - cov); + RelativePose2DStampedConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov); // Define the expected matrices (used Octave to compute sqrt_info) fuse_core::Matrix3d expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 1.008395589795798, -0.040950074712520, -0.063131365181801, - 0.000000000000000, 0.712470499879096, -0.071247049987910, - 0.000000000000000, 0.000000000000000, 0.577350269189626; + expected_sqrt_info << 1.008395589795798, -0.040950074712520, -0.063131365181801, 0.000000000000000, 0.712470499879096, + -0.071247049987910, 0.000000000000000, 0.000000000000000, 0.577350269189626; /* *INDENT-ON* */ fuse_core::Matrix3d expected_cov = cov; // Compare @@ -109,18 +98,14 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) // Optimize a two-pose system with a pose prior and a relative pose constraint // Verify the expected poses and covariances are generated. // Create two poses - auto orientation1 = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); + auto orientation1 = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); orientation1->yaw() = 0.8; - auto position1 = - Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); + auto position1 = Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); position1->x() = 1.5; position1->y() = -3.0; - auto orientation2 = Orientation2DStamped::make_shared( - rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); + auto orientation2 = Orientation2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); orientation2->yaw() = -2.7; - auto position2 = - Position2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); + auto position2 = Position2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); position2->x() = 3.7; position2->y() = 1.2; // Create an absolute pose constraint at the origin @@ -128,77 +113,52 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) mean1 << 0.0, 0.0, 0.0; fuse_core::Matrix3d cov1; cov1 << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - auto prior = AbsolutePose2DStampedConstraint::make_shared( - "test", - *position1, - *orientation1, - mean1, - cov1); + auto prior = AbsolutePose2DStampedConstraint::make_shared("test", *position1, *orientation1, mean1, cov1); // Create a relative pose constraint for 1m in the x direction fuse_core::Vector3d delta2; delta2 << 1.0, 0.0, 0.0; fuse_core::Matrix3d cov2; cov2 << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - auto relative = RelativePose2DStampedConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - delta2, - cov2); + auto relative = RelativePose2DStampedConstraint::make_shared("test", *position1, *orientation1, *position2, + *orientation2, delta2, cov2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation1->data(), - orientation1->size(), + problem.AddParameterBlock(orientation1->data(), orientation1->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation1->localParameterization()); + orientation1->localParameterization()); #else - orientation1->manifold()); + orientation1->manifold()); #endif - problem.AddParameterBlock( - position1->data(), - position1->size(), + problem.AddParameterBlock(position1->data(), position1->size(), #if !CERES_SUPPORTS_MANIFOLDS - position1->localParameterization()); + position1->localParameterization()); #else - position1->manifold()); + position1->manifold()); #endif - problem.AddParameterBlock( - orientation2->data(), - orientation2->size(), + problem.AddParameterBlock(orientation2->data(), orientation2->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation2->localParameterization()); + orientation2->localParameterization()); #else - orientation2->manifold()); + orientation2->manifold()); #endif - problem.AddParameterBlock( - position2->data(), - position2->size(), + problem.AddParameterBlock(position2->data(), position2->size(), #if !CERES_SUPPORTS_MANIFOLDS - position2->localParameterization()); + position2->localParameterization()); #else - position2->manifold()); + position2->manifold()); #endif - std::vector prior_parameter_blocks; + std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); - problem.AddResidualBlock( - prior->costFunction(), - prior->lossFunction(), - prior_parameter_blocks); - std::vector relative_parameter_blocks; + problem.AddResidualBlock(prior->costFunction(), prior->lossFunction(), prior_parameter_blocks); + std::vector relative_parameter_blocks; relative_parameter_blocks.push_back(position1->data()); relative_parameter_blocks.push_back(orientation1->data()); relative_parameter_blocks.push_back(position2->data()); relative_parameter_blocks.push_back(orientation2->data()); - problem.AddResidualBlock( - relative->costFunction(), - relative->lossFunction(), - relative_parameter_blocks); + problem.AddResidualBlock(relative->costFunction(), relative->lossFunction(), relative_parameter_blocks); // Run the solver ceres::Solver::Options options; ceres::Solver::Summary summary; @@ -212,7 +172,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) EXPECT_NEAR(0.0, orientation2->yaw(), 1.0e-5); // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); @@ -222,13 +182,9 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) std::vector covariance_vector1(position1->size() * position1->size()); covariance.GetCovarianceBlock(position1->data(), position1->data(), covariance_vector1.data()); std::vector covariance_vector2(position1->size() * orientation1->size()); - covariance.GetCovarianceBlock( - position1->data(), orientation1->data(), - covariance_vector2.data()); + covariance.GetCovarianceBlock(position1->data(), orientation1->data(), covariance_vector2.data()); std::vector covariance_vector3(orientation1->size() * orientation1->size()); - covariance.GetCovarianceBlock( - orientation1->data(), - orientation1->data(), covariance_vector3.data()); + covariance.GetCovarianceBlock(orientation1->data(), orientation1->data(), covariance_vector3.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix3d actual_covariance; actual_covariance(0, 0) = covariance_vector1[0]; @@ -245,7 +201,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) } // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); @@ -255,13 +211,9 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) std::vector covariance_vector1(position2->size() * position2->size()); covariance.GetCovarianceBlock(position2->data(), position2->data(), covariance_vector1.data()); std::vector covariance_vector2(position2->size() * orientation2->size()); - covariance.GetCovarianceBlock( - position2->data(), orientation2->data(), - covariance_vector2.data()); + covariance.GetCovarianceBlock(position2->data(), orientation2->data(), covariance_vector2.data()); std::vector covariance_vector3(orientation2->size() * orientation2->size()); - covariance.GetCovarianceBlock( - orientation2->data(), - orientation2->data(), covariance_vector3.data()); + covariance.GetCovarianceBlock(orientation2->data(), orientation2->data(), covariance_vector3.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix3d actual_covariance; actual_covariance(0, 0) = covariance_vector1[0]; @@ -284,19 +236,15 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) // Optimize a two-pose system with a pose prior and a relative pose constraint // Verify the expected poses and covariances are generated. // Create two poses - auto orientation1 = Orientation2DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); + auto orientation1 = Orientation2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); orientation1->yaw() = 0.8; - auto position1 = - Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); + auto position1 = Position2DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); position1->x() = 1.5; position1->y() = -3.0; - auto orientation2 = Orientation2DStamped::make_shared( - rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); + auto orientation2 = Orientation2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); orientation2->yaw() = -2.7; - auto position2 = - Position2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); + auto position2 = Position2DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); position2->x() = 3.7; position2->y() = 1.2; @@ -305,107 +253,69 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) mean1 << 0.0, 0.0, 0.0; fuse_core::Matrix3d cov1; cov1 << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; - auto prior = AbsolutePose2DStampedConstraint::make_shared( - "test", - *position1, - *orientation1, - mean1, - cov1); + auto prior = AbsolutePose2DStampedConstraint::make_shared("test", *position1, *orientation1, mean1, cov1); // Create a relative pose constraint for 1m in the x direction fuse_core::Vector2d delta1; delta1 << 1.0, 0.0; fuse_core::Matrix2d cov_rel1; cov_rel1 << 1.0, 0.0, 0.0, 1.0; - std::vector axes_lin1 = {fuse_variables::Position2DStamped::X}; - std::vector axes_ang1 = {fuse_variables::Orientation2DStamped::YAW}; - auto relative1 = RelativePose2DStampedConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - delta1, - cov_rel1, - axes_lin1, - axes_ang1); + std::vector axes_lin1 = { fuse_variables::Position2DStamped::X }; + std::vector axes_ang1 = { fuse_variables::Orientation2DStamped::YAW }; + auto relative1 = RelativePose2DStampedConstraint::make_shared("test", *position1, *orientation1, *position2, + *orientation2, delta1, cov_rel1, axes_lin1, axes_ang1); // Create a relative pose constraint for 0m in the y direction fuse_core::Vector1d delta2; delta2 << 0.0; fuse_core::Matrix1d cov_rel2; cov_rel2 << 1.0; - std::vector axes_lin2 = {fuse_variables::Position2DStamped::Y}; + std::vector axes_lin2 = { fuse_variables::Position2DStamped::Y }; std::vector axes_ang2 = {}; - auto relative2 = RelativePose2DStampedConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - delta2, - cov_rel2, - axes_lin2, - axes_ang2); + auto relative2 = RelativePose2DStampedConstraint::make_shared("test", *position1, *orientation1, *position2, + *orientation2, delta2, cov_rel2, axes_lin2, axes_ang2); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation1->data(), - orientation1->size(), + problem.AddParameterBlock(orientation1->data(), orientation1->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation1->localParameterization()); + orientation1->localParameterization()); #else - orientation1->manifold()); + orientation1->manifold()); #endif - problem.AddParameterBlock( - position1->data(), - position1->size(), + problem.AddParameterBlock(position1->data(), position1->size(), #if !CERES_SUPPORTS_MANIFOLDS - position1->localParameterization()); + position1->localParameterization()); #else - position1->manifold()); + position1->manifold()); #endif - problem.AddParameterBlock( - orientation2->data(), - orientation2->size(), + problem.AddParameterBlock(orientation2->data(), orientation2->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation2->localParameterization()); + orientation2->localParameterization()); #else - orientation2->manifold()); + orientation2->manifold()); #endif - problem.AddParameterBlock( - position2->data(), - position2->size(), + problem.AddParameterBlock(position2->data(), position2->size(), #if !CERES_SUPPORTS_MANIFOLDS - position2->localParameterization()); + position2->localParameterization()); #else - position2->manifold()); + position2->manifold()); #endif - std::vector prior_parameter_blocks; + std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); - problem.AddResidualBlock( - prior->costFunction(), - prior->lossFunction(), - prior_parameter_blocks); + problem.AddResidualBlock(prior->costFunction(), prior->lossFunction(), prior_parameter_blocks); - std::vector relative_parameter_blocks; + std::vector relative_parameter_blocks; relative_parameter_blocks.push_back(position1->data()); relative_parameter_blocks.push_back(orientation1->data()); relative_parameter_blocks.push_back(position2->data()); relative_parameter_blocks.push_back(orientation2->data()); - problem.AddResidualBlock( - relative1->costFunction(), - relative1->lossFunction(), - relative_parameter_blocks); - problem.AddResidualBlock( - relative2->costFunction(), - relative2->lossFunction(), - relative_parameter_blocks); + problem.AddResidualBlock(relative1->costFunction(), relative1->lossFunction(), relative_parameter_blocks); + problem.AddResidualBlock(relative2->costFunction(), relative2->lossFunction(), relative_parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -422,7 +332,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); @@ -434,13 +344,9 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) std::vector covariance_vector1(position1->size() * position1->size()); covariance.GetCovarianceBlock(position1->data(), position1->data(), covariance_vector1.data()); std::vector covariance_vector2(position1->size() * orientation1->size()); - covariance.GetCovarianceBlock( - position1->data(), orientation1->data(), - covariance_vector2.data()); + covariance.GetCovarianceBlock(position1->data(), orientation1->data(), covariance_vector2.data()); std::vector covariance_vector3(orientation1->size() * orientation1->size()); - covariance.GetCovarianceBlock( - orientation1->data(), - orientation1->data(), covariance_vector3.data()); + covariance.GetCovarianceBlock(orientation1->data(), orientation1->data(), covariance_vector3.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix3d actual_covariance; @@ -458,7 +364,7 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) } // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); @@ -470,13 +376,9 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) std::vector covariance_vector1(position2->size() * position2->size()); covariance.GetCovarianceBlock(position2->data(), position2->data(), covariance_vector1.data()); std::vector covariance_vector2(position2->size() * orientation2->size()); - covariance.GetCovarianceBlock( - position2->data(), orientation2->data(), - covariance_vector2.data()); + covariance.GetCovarianceBlock(position2->data(), orientation2->data(), covariance_vector2.data()); std::vector covariance_vector3(orientation2->size() * orientation2->size()); - covariance.GetCovarianceBlock( - orientation2->data(), - orientation2->data(), covariance_vector3.data()); + covariance.GetCovarianceBlock(orientation2->data(), orientation2->data(), covariance_vector3.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix3d actual_covariance; @@ -506,8 +408,7 @@ TEST(RelativePose2DStampedConstraint, Serialization) delta << 1.0, 2.0, 3.0; fuse_core::Matrix3d cov; cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; - RelativePose2DStampedConstraint expected("test", position1, orientation1, position2, orientation2, - delta, cov); + RelativePose2DStampedConstraint expected("test", position1, orientation1, position2, orientation2, delta, cov); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp index 7e16d7e56..0ddee00cc 100644 --- a/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp @@ -49,11 +49,10 @@ #include #include -using fuse_variables::Orientation3DStamped; -using fuse_variables::Position3DStamped; using fuse_constraints::AbsolutePose3DStampedConstraint; using fuse_constraints::RelativePose3DStampedConstraint; - +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; TEST(RelativePose3DStampedConstraint, Constructor) { @@ -70,18 +69,22 @@ TEST(RelativePose3DStampedConstraint, Constructor) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ EXPECT_NO_THROW( - RelativePose3DStampedConstraint constraint( - "test", position1, orientation1, position2, - orientation2, delta, cov)); + RelativePose3DStampedConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov)); } TEST(RelativePose3DStampedConstraint, Covariance) @@ -98,32 +101,32 @@ TEST(RelativePose3DStampedConstraint, Covariance) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - RelativePose3DStampedConstraint constraint( - "test", - position1, - orientation1, - position2, - orientation2, - delta, - cov); + RelativePose3DStampedConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix6d expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT - 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT - 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT - 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, + -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov = cov; @@ -137,27 +140,23 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Optimize a two-pose system with a pose prior and a relative pose constraint // Verify the expected poses and covariances are generated. // Create two poses - auto position1 = - Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position1 = Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position1->x() = 1.5; position1->y() = -3.0; position1->z() = 10.0; - auto orientation1 = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation1 = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation1->w() = 0.952; orientation1->x() = 0.038; orientation1->y() = -0.189; orientation1->z() = 0.239; - auto position2 = - Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + auto position2 = Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); position2->x() = -1.5; position2->y() = 3.0; position2->z() = -10.0; - auto orientation2 = Orientation3DStamped::make_shared( - rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + auto orientation2 = Orientation3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); orientation2->w() = 0.944; orientation2->x() = -0.128; orientation2->y() = 0.145; @@ -167,78 +166,53 @@ TEST(RelativePose3DStampedConstraint, Optimization) fuse_core::Vector7d mean_origin; mean_origin << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix6d cov_origin = fuse_core::Matrix6d::Identity(); - auto prior = AbsolutePose3DStampedConstraint::make_shared( - "test", - *position1, - *orientation1, - mean_origin, - cov_origin); + auto prior = AbsolutePose3DStampedConstraint::make_shared("test", *position1, *orientation1, mean_origin, cov_origin); // Create a relative pose constraint for 1m in the x direction fuse_core::Vector7d mean_delta; mean_delta << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix6d cov_delta = fuse_core::Matrix6d::Identity(); - auto relative = RelativePose3DStampedConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - mean_delta, - cov_delta); + auto relative = RelativePose3DStampedConstraint::make_shared("test", *position1, *orientation1, *position2, + *orientation2, mean_delta, cov_delta); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation1->data(), - orientation1->size(), + problem.AddParameterBlock(orientation1->data(), orientation1->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation1->localParameterization()); + orientation1->localParameterization()); #else - orientation1->manifold()); + orientation1->manifold()); #endif - problem.AddParameterBlock( - position1->data(), - position1->size(), + problem.AddParameterBlock(position1->data(), position1->size(), #if !CERES_SUPPORTS_MANIFOLDS - position1->localParameterization()); + position1->localParameterization()); #else - position1->manifold()); + position1->manifold()); #endif - problem.AddParameterBlock( - orientation2->data(), - orientation2->size(), + problem.AddParameterBlock(orientation2->data(), orientation2->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation2->localParameterization()); + orientation2->localParameterization()); #else - orientation2->manifold()); + orientation2->manifold()); #endif - problem.AddParameterBlock( - position2->data(), - position2->size(), + problem.AddParameterBlock(position2->data(), position2->size(), #if !CERES_SUPPORTS_MANIFOLDS - position2->localParameterization()); + position2->localParameterization()); #else - position2->manifold()); + position2->manifold()); #endif - std::vector prior_parameter_blocks; + std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); - problem.AddResidualBlock( - prior->costFunction(), - prior->lossFunction(), - prior_parameter_blocks); - std::vector relative_parameter_blocks; + problem.AddResidualBlock(prior->costFunction(), prior->lossFunction(), prior_parameter_blocks); + std::vector relative_parameter_blocks; relative_parameter_blocks.push_back(position1->data()); relative_parameter_blocks.push_back(orientation1->data()); relative_parameter_blocks.push_back(position2->data()); relative_parameter_blocks.push_back(orientation2->data()); - problem.AddResidualBlock( - relative->costFunction(), - relative->lossFunction(), - relative_parameter_blocks); + problem.AddResidualBlock(relative->costFunction(), relative->lossFunction(), relative_parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -263,7 +237,7 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); @@ -276,14 +250,10 @@ TEST(RelativePose3DStampedConstraint, Optimization) covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); fuse_core::MatrixXd cov_or_or(3, 3); - covariance.GetCovarianceBlockInTangentSpace( - orientation1->data(), - orientation1->data(), cov_or_or.data()); + covariance.GetCovarianceBlockInTangentSpace(orientation1->data(), orientation1->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position1->size(), 3); - covariance.GetCovarianceBlockInTangentSpace( - position1->data(), - orientation1->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position1->data(), orientation1->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -292,13 +262,8 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; + expected_covariance << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; /* *INDENT-ON* */ EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); @@ -306,7 +271,7 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); @@ -319,14 +284,10 @@ TEST(RelativePose3DStampedConstraint, Optimization) covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); fuse_core::MatrixXd cov_or_or(3, 3); - covariance.GetCovarianceBlockInTangentSpace( - orientation2->data(), - orientation2->data(), cov_or_or.data()); + covariance.GetCovarianceBlockInTangentSpace(orientation2->data(), orientation2->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position1->size(), 3); - covariance.GetCovarianceBlockInTangentSpace( - position2->data(), - orientation2->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position2->data(), orientation2->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -335,13 +296,8 @@ TEST(RelativePose3DStampedConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, - 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, - 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, - 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; + expected_covariance << 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, + 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; /* *INDENT-ON* */ EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); @@ -363,16 +319,21 @@ TEST(RelativePose3DStampedConstraint, Serialization) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - RelativePose3DStampedConstraint expected("test", position1, orientation1, position2, orientation2, - delta, cov); + RelativePose3DStampedConstraint expected("test", position1, orientation1, position2, orientation2, delta, cov); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_constraints/test/test_uuid_ordering.cpp b/fuse_constraints/test/test_uuid_ordering.cpp index 508731a64..22c103340 100644 --- a/fuse_constraints/test/test_uuid_ordering.cpp +++ b/fuse_constraints/test/test_uuid_ordering.cpp @@ -46,11 +46,11 @@ TEST(UuidOrdering, Constructor) EXPECT_NO_THROW(UuidOrdering()); // Iterators - std::vector uuids{fuse_core::uuid::generate(), fuse_core::uuid::generate()}; + std::vector uuids{ fuse_core::uuid::generate(), fuse_core::uuid::generate() }; EXPECT_NO_THROW(UuidOrdering(uuids.begin(), uuids.end())); // Initializer List - EXPECT_NO_THROW(UuidOrdering({fuse_core::uuid::generate(), fuse_core::uuid::generate()})); // NOLINT + EXPECT_NO_THROW(UuidOrdering({ fuse_core::uuid::generate(), fuse_core::uuid::generate() })); // NOLINT } TEST(UuidOrdering, Access) @@ -59,7 +59,7 @@ TEST(UuidOrdering, Access) auto uuid2 = fuse_core::uuid::generate(); auto uuid3 = fuse_core::uuid::generate(); auto uuid4 = fuse_core::uuid::generate(); - auto order = UuidOrdering{uuid1, uuid2, uuid3}; + auto order = UuidOrdering{ uuid1, uuid2, uuid3 }; EXPECT_EQ(0u, order.at(uuid1)); EXPECT_EQ(1u, order.at(uuid2)); @@ -87,7 +87,7 @@ TEST(UuidOrdering, PushBack) auto uuid2 = fuse_core::uuid::generate(); auto uuid3 = fuse_core::uuid::generate(); auto uuid4 = fuse_core::uuid::generate(); - auto order = UuidOrdering{uuid1, uuid2, uuid3}; + auto order = UuidOrdering{ uuid1, uuid2, uuid3 }; EXPECT_EQ(3u, order.size()); EXPECT_FALSE(order.push_back(uuid3)); @@ -118,7 +118,7 @@ TEST(UuidOrdering, Exists) auto uuid2 = fuse_core::uuid::generate(); auto uuid3 = fuse_core::uuid::generate(); auto uuid4 = fuse_core::uuid::generate(); - auto order = UuidOrdering{uuid1, uuid2, uuid3}; + auto order = UuidOrdering{ uuid1, uuid2, uuid3 }; EXPECT_TRUE(order.exists(0)); EXPECT_TRUE(order.exists(1)); diff --git a/fuse_constraints/test/test_variable_constraints.cpp b/fuse_constraints/test/test_variable_constraints.cpp index 2edb0144b..b640854a6 100644 --- a/fuse_constraints/test/test_variable_constraints.cpp +++ b/fuse_constraints/test/test_variable_constraints.cpp @@ -48,9 +48,9 @@ TEST(VariableConstraints, Size) EXPECT_TRUE(vars.empty()); EXPECT_EQ(0u, vars.size()); - vars.insert(0u, {0u, 1u}); // NOLINT - vars.insert(1u, {0u, 1u}); // NOLINT - vars.insert(2u, {0u, 1u}); // NOLINT + vars.insert(0u, { 0u, 1u }); // NOLINT + vars.insert(1u, { 0u, 1u }); // NOLINT + vars.insert(2u, { 0u, 1u }); // NOLINT EXPECT_FALSE(vars.empty()); EXPECT_EQ(6u, vars.size()); @@ -61,7 +61,7 @@ TEST(VariableConstraints, NextVariableIndex) auto vars = VariableConstraints(); EXPECT_EQ(0u, vars.nextVariableIndex()); - vars.insert(0u, {9u, 10u}); // NOLINT + vars.insert(0u, { 9u, 10u }); // NOLINT EXPECT_EQ(11u, vars.nextVariableIndex()); } @@ -70,15 +70,15 @@ TEST(VariableConstraints, GetConstraints) { auto vars = VariableConstraints(); - vars.insert(0u, {0u, 1u, 2u}); // NOLINT - vars.insert(1u, {0u, 2u}); // NOLINT - vars.insert(2u, {1u, 2u}); // NOLINT - vars.insert(3u, {2u, 3u}); // NOLINT + vars.insert(0u, { 0u, 1u, 2u }); // NOLINT + vars.insert(1u, { 0u, 2u }); // NOLINT + vars.insert(2u, { 1u, 2u }); // NOLINT + vars.insert(3u, { 2u, 3u }); // NOLINT - auto expected0 = std::vector{0u, 1u}; // NOLINT - auto expected1 = std::vector{0u, 2u}; // NOLINT - auto expected2 = std::vector{0u, 1u, 2u, 3u}; // NOLINT - auto expected3 = std::vector{3u}; // NOLINT + auto expected0 = std::vector{ 0u, 1u }; // NOLINT + auto expected1 = std::vector{ 0u, 2u }; // NOLINT + auto expected2 = std::vector{ 0u, 1u, 2u, 3u }; // NOLINT + auto expected3 = std::vector{ 3u }; // NOLINT std::vector actual0; vars.getConstraints(0u, std::back_inserter(actual0)); @@ -113,18 +113,10 @@ TEST(VariableConstraints, GetConstraints) auto actual3_iter = actual3.begin(); actual3_iter = vars.getConstraints(3u, actual3_iter); - EXPECT_EQ( - static_cast(expected0.size()), - std::distance(actual0.begin(), actual0_iter)); - EXPECT_EQ( - static_cast(expected1.size()), - std::distance(actual1.begin(), actual1_iter)); - EXPECT_EQ( - static_cast(expected2.size()), - std::distance(actual2.begin(), actual2_iter)); - EXPECT_EQ( - static_cast(expected3.size()), - std::distance(actual3.begin(), actual3_iter)); + EXPECT_EQ(static_cast(expected0.size()), std::distance(actual0.begin(), actual0_iter)); + EXPECT_EQ(static_cast(expected1.size()), std::distance(actual1.begin(), actual1_iter)); + EXPECT_EQ(static_cast(expected2.size()), std::distance(actual2.begin(), actual2_iter)); + EXPECT_EQ(static_cast(expected3.size()), std::distance(actual3.begin(), actual3_iter)); } TEST(VariableConstraints, InsertOrphanVariable) diff --git a/fuse_core/CHANGELOG.rst b/fuse_core/CHANGELOG.rst index a05d65b98..286516c90 100644 --- a/fuse_core/CHANGELOG.rst +++ b/fuse_core/CHANGELOG.rst @@ -7,7 +7,7 @@ Changelog for package fuse_core 1.1.1 (2024-05-02) ------------------ -* Required formatting changes for the lastest version of ROS 2 Rolling (`#368 `_) +* Required formatting changes for the latest version of ROS 2 Rolling (`#368 `_) * Contributors: Stephen Williams 1.1.0 (2024-04-20) diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt index 737542c0a..c396fc2d5 100644 --- a/fuse_core/CMakeLists.txt +++ b/fuse_core/CMakeLists.txt @@ -23,12 +23,13 @@ find_package(Eigen3 REQUIRED) include(boost-extras.cmake) -########### -## Build ## -########### +# ############################################################################## +# Build ## +# ############################################################################## -## fuse_core library -add_library(${PROJECT_NAME} +# fuse_core library +add_library( + ${PROJECT_NAME} src/async_motion_model.cpp src/async_publisher.cpp src/async_sensor_model.cpp @@ -45,66 +46,62 @@ add_library(${PROJECT_NAME} src/transaction.cpp src/transaction_deserializer.cpp src/uuid.cpp - src/variable.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$" -) -target_link_libraries(${PROJECT_NAME} PUBLIC - Boost::serialization - Ceres::ceres - Eigen3::Eigen - ${fuse_msgs_TARGETS} - pluginlib::pluginlib - rclcpp::rclcpp - ${rcl_interfaces_TARGETS} -) -target_link_libraries(${PROJECT_NAME} PRIVATE - rclcpp_components::component -) - -## fuse_echo executable + src/variable.cpp) +target_include_directories( + ${PROJECT_NAME} + PUBLIC "$" + "$") +target_link_libraries( + ${PROJECT_NAME} + PUBLIC Boost::serialization + Ceres::ceres + Eigen3::Eigen + ${fuse_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + ${rcl_interfaces_TARGETS}) +target_link_libraries(${PROJECT_NAME} PRIVATE rclcpp_components::component) + +# fuse_echo executable add_library(fuse_echo_component SHARED src/fuse_echo.cpp) target_link_libraries(fuse_echo_component PUBLIC ${PROJECT_NAME}) target_link_libraries(fuse_echo_component PRIVATE rclcpp_components::component) -rclcpp_components_register_node(fuse_echo_component - PLUGIN "fuse_core::FuseEcho" - EXECUTABLE fuse_echo - EXECUTOR SingleThreadedExecutor -) +rclcpp_components_register_node( + fuse_echo_component + PLUGIN + "fuse_core::FuseEcho" + EXECUTABLE + fuse_echo + EXECUTOR + SingleThreadedExecutor) -############# -## Testing ## -############# +# ############################################################################## +# Testing ## +# ############################################################################## if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - add_subdirectory(test) endif() -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## -install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-export ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) -install(TARGETS fuse_echo_component +install( + TARGETS fuse_echo_component ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET) ament_export_dependencies( @@ -115,7 +112,6 @@ ament_export_dependencies( rclcpp rclcpp_components Ceres - Eigen3 -) + Eigen3) ament_package(CONFIG_EXTRAS boost-extras.cmake) diff --git a/fuse_core/README.md b/fuse_core/README.md index 8ed9bda79..06ff00c40 100644 --- a/fuse_core/README.md +++ b/fuse_core/README.md @@ -11,7 +11,7 @@ instances, allowing measurements to involve previous states as well as the curre When defining a new variable type, there is a balance that must be struct between reusability and convenience. If you define a complex composite state, it is unlikely that any other available components will use that same state definition. If you make the state too granular, then more book-keeping and value lookups will be required to piece -together a useful concept from many smallar scalar components. +together a useful concept from many smaller scalar components. As an example, let's consider a 3D pose consisting of a 3D position (x, y, z) and a quaternion orientation (qx, qy, qx, qw). We could define a single state for the 3D pose consisting of all 7 scalar components. Alternatively, diff --git a/fuse_core/boost-extras.cmake b/fuse_core/boost-extras.cmake index 12c9f2f6c..a7331140d 100644 --- a/fuse_core/boost-extras.cmake +++ b/fuse_core/boost-extras.cmake @@ -3,27 +3,26 @@ # 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 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. +# * 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 Open Source Robotics Foundation, Inc. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# * Neither the name of the Open Source Robotics Foundation, Inc. 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. +# 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. find_package(Boost REQUIRED COMPONENTS serialization) diff --git a/fuse_core/include/fuse_core/async_motion_model.hpp b/fuse_core/include/fuse_core/async_motion_model.hpp index 0e3f6f430..cdbb091a4 100644 --- a/fuse_core/include/fuse_core/async_motion_model.hpp +++ b/fuse_core/include/fuse_core/async_motion_model.hpp @@ -80,7 +80,7 @@ namespace fuse_core * motion model needs access to the latest values of the state variables. In many cases, motion * models will simply not need that information. If the motion model does need access the to * graph, the most common implementation will simply be to move the provided pointer into a class - * memebr variable, for use in other callbacks. + * member variable, for use in other callbacks. * @code{.cpp} * void onGraphUpdate(Graph::ConstSharedPtr graph) override { this->graph_ = std::move(graph); } * @endcode @@ -112,7 +112,7 @@ class AsyncMotionModel : public MotionModel * @return True if the motion models were generated successfully, false * otherwise */ - bool apply(Transaction & transaction) override; + bool apply(Transaction& transaction) override; /** * @brief Function to be executed whenever the optimizer has completed a Graph update @@ -142,14 +142,16 @@ class AsyncMotionModel : public MotionModel * @param[in] name A unique name to give this plugin instance * @throws runtime_error if already initialized */ - void initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; /** * @brief Get the unique name of this motion model */ - const std::string & name() const override {return name_;} + const std::string& name() const override + { + return name_; + } /** * @brief Function to be executed whenever the optimizer is ready to receive transactions @@ -201,8 +203,8 @@ class AsyncMotionModel : public MotionModel //! A single/multi-threaded executor assigned to the local callback queue rclcpp::Executor::SharedPtr executor_; - size_t executor_thread_count_{1}; - std::thread spinner_; //!< Internal thread for spinning the executor + size_t executor_thread_count_{ 1 }; + std::thread spinner_; //!< Internal thread for spinning the executor std::atomic initialized_ = false; //!< True if instance has been fully initialized /** @@ -232,7 +234,7 @@ class AsyncMotionModel : public MotionModel * @return True if the motion models were generated successfully, false * otherwise */ - virtual bool applyCallback(Transaction & transaction) = 0; + virtual bool applyCallback(Transaction& transaction) = 0; /** * @brief Callback fired in the local callback queue thread(s) whenever a new Graph is received @@ -251,7 +253,9 @@ class AsyncMotionModel : public MotionModel * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed * whenever needed. */ - virtual void onGraphUpdate(Graph::ConstSharedPtr /*graph*/) {} + virtual void onGraphUpdate(Graph::ConstSharedPtr /*graph*/) + { + } /** * @brief Perform any required initialization for the motion model @@ -260,7 +264,9 @@ class AsyncMotionModel : public MotionModel * The class's node will be properly initialized before onInit() is called. Spinning * of the callback queue will not begin until after the call to onInit() completes. */ - virtual void onInit() {} + virtual void onInit() + { + } /** * @brief Perform any required operations to prepare for servicing calls to apply() @@ -268,7 +274,9 @@ class AsyncMotionModel : public MotionModel * This function will be called once after initialize() but before any calls to apply(). It * may also be called at any time after a call to stop(). */ - virtual void onStart() {} + virtual void onStart() + { + } /** * @brief Perform any required operations to clean up the internal state @@ -277,7 +285,9 @@ class AsyncMotionModel : public MotionModel * after a call to start(). No calls to apply() will occur after stop() is called, but before * start() is called. */ - virtual void onStop() {} + virtual void onStop() + { + } private: //! Stop the internal executor thread (in order to use this class again it must be re-initialized) diff --git a/fuse_core/include/fuse_core/async_publisher.hpp b/fuse_core/include/fuse_core/async_publisher.hpp index 4df5fef6d..af016b583 100644 --- a/fuse_core/include/fuse_core/async_publisher.hpp +++ b/fuse_core/include/fuse_core/async_publisher.hpp @@ -86,14 +86,16 @@ class AsyncPublisher : public Publisher * @param[in] name A unique name to give this plugin instance * @throws runtime_error if already initialized */ - void initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; /** * @brief Get the unique name of this publisher */ - const std::string & name() const override {return name_;} + const std::string& name() const override + { + return name_; + } /** * @brief Notify the publisher that an optimization cycle is complete, and about changes to the @@ -154,7 +156,7 @@ class AsyncPublisher : public Publisher //! The callback queue used for fuse internal callbacks std::shared_ptr callback_queue_; - std::string name_; //!< The unique name for this publisher instance + std::string name_; //!< The unique name for this publisher instance rclcpp::CallbackGroup::SharedPtr cb_group_; //!< Internal re-entrant callback group //! The node interfaces @@ -162,8 +164,8 @@ class AsyncPublisher : public Publisher //! A single/multi-threaded executor assigned to the local callback queue rclcpp::Executor::SharedPtr executor_; - size_t executor_thread_count_{1}; - std::thread spinner_; //!< Internal thread for spinning the executor + size_t executor_thread_count_{ 1 }; + std::thread spinner_; //!< Internal thread for spinning the executor std::atomic initialized_ = false; //!< True if instance has been fully initialized /** @@ -185,7 +187,9 @@ class AsyncPublisher : public Publisher * Derived classes should override this method to implement any additional initialization * steps needed (access the parameter server, advertise, subscribe, etc.). */ - virtual void onInit() {} + virtual void onInit() + { + } /** * @brief Callback method executed in response to the optimizer completing an optimization cycle. @@ -199,9 +203,9 @@ class AsyncPublisher : public Publisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ - virtual void notifyCallback( - Transaction::ConstSharedPtr /*transaction*/, - Graph::ConstSharedPtr /*graph*/) {} + virtual void notifyCallback(Transaction::ConstSharedPtr /*transaction*/, Graph::ConstSharedPtr /*graph*/) + { + } /** * @brief Perform any required operations to prepare for servicing calls to notify() @@ -209,7 +213,9 @@ class AsyncPublisher : public Publisher * This function will be called once after initialize() but before any calls to notify(). It * may also be called at any time after a call to stop(). */ - virtual void onStart() {} + virtual void onStart() + { + } /** * @brief Perform any required operations to clean up the internal state @@ -218,7 +224,9 @@ class AsyncPublisher : public Publisher * after a call to start(). No calls to notify() will occur after stop() is called, but * before start() is called. */ - virtual void onStop() {} + virtual void onStop() + { + } private: //! Stop the internal executor thread (in order to use this class again it must be re-initialized) diff --git a/fuse_core/include/fuse_core/async_sensor_model.hpp b/fuse_core/include/fuse_core/async_sensor_model.hpp index c82748fab..c550fe72e 100644 --- a/fuse_core/include/fuse_core/async_sensor_model.hpp +++ b/fuse_core/include/fuse_core/async_sensor_model.hpp @@ -127,10 +127,8 @@ class AsyncSensorModel : public SensorModel * @param[in] transaction_callback The function to call every time a transaction is published * @throws runtime_error if already initialized */ - void initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name, - TransactionCallback transaction_callback) override; + void initialize(node_interfaces::NodeInterfaces interfaces, const std::string& name, + TransactionCallback transaction_callback) override; /** * @brief Send a transaction to the Optimizer @@ -152,7 +150,10 @@ class AsyncSensorModel : public SensorModel /** * @brief Get the unique name of this sensor */ - const std::string & name() const override {return name_;} + const std::string& name() const override + { + return name_; + } /** * @brief Function to be executed whenever the optimizer is ready to receive transactions @@ -205,8 +206,8 @@ class AsyncSensorModel : public SensorModel //! The function to be executed every time a Transaction is "published" TransactionCallback transaction_callback_; - size_t executor_thread_count_{1}; - std::thread spinner_; //!< Internal thread for spinning the executor + size_t executor_thread_count_{ 1 }; + std::thread spinner_; //!< Internal thread for spinning the executor std::atomic initialized_ = false; //!< True if instance has been fully initialized /** @@ -235,7 +236,9 @@ class AsyncSensorModel : public SensorModel * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed * whenever needed. */ - virtual void onGraphUpdate(Graph::ConstSharedPtr /*graph*/) {} + virtual void onGraphUpdate(Graph::ConstSharedPtr /*graph*/) + { + } /** * @brief Perform any required initialization for the sensor model @@ -247,7 +250,9 @@ class AsyncSensorModel : public SensorModel * Derived sensor models classes must implement this function, because otherwise I'm not sure * how the derived sensor model would actually do anything. */ - virtual void onInit() {} + virtual void onInit() + { + } /** * @brief Perform any required operations to prepare for sending transactions to the optimizer @@ -258,7 +263,9 @@ class AsyncSensorModel : public SensorModel * The sensor model must not send any transactions to the optimizer before onStart() is * called. */ - virtual void onStart() {} + virtual void onStart() + { + } /** * @brief Perform any required operations to stop sending transactions to the optimizer @@ -268,7 +275,9 @@ class AsyncSensorModel : public SensorModel * * The sensor model must not send any transactions to the optimizer after stop() is called. */ - virtual void onStop() {} + virtual void onStop() + { + } private: //! Stop the internal executor thread (in order to use this class again it must be re-initialized) diff --git a/fuse_core/include/fuse_core/autodiff_local_parameterization.h b/fuse_core/include/fuse_core/autodiff_local_parameterization.h index 104404a1b..f3135b8c2 100644 --- a/fuse_core/include/fuse_core/autodiff_local_parameterization.h +++ b/fuse_core/include/fuse_core/autodiff_local_parameterization.h @@ -35,8 +35,7 @@ #ifndef FUSE_CORE__AUTODIFF_LOCAL_PARAMETERIZATION_H_ #define FUSE_CORE__AUTODIFF_LOCAL_PARAMETERIZATION_H_ -#warning \ - This header is obsolete, please include fuse_core/autodiff_local_parameterization.hpp instead +#warning This header is obsolete, please include fuse_core/autodiff_local_parameterization.hpp instead #include diff --git a/fuse_core/include/fuse_core/autodiff_local_parameterization.hpp b/fuse_core/include/fuse_core/autodiff_local_parameterization.hpp index 2fa781cb9..e532c8097 100644 --- a/fuse_core/include/fuse_core/autodiff_local_parameterization.hpp +++ b/fuse_core/include/fuse_core/autodiff_local_parameterization.hpp @@ -73,13 +73,11 @@ namespace fuse_core * * For more information on local parameterizations, see fuse_core::LocalParameterization */ -template +template class AutoDiffLocalParameterization : public LocalParameterization { public: - FUSE_SMART_PTR_DEFINITIONS( - AutoDiffLocalParameterization) + FUSE_SMART_PTR_DEFINITIONS(AutoDiffLocalParameterization) /** * @brief Constructs new PlusFunctor and MinusFunctor instances @@ -89,7 +87,7 @@ class AutoDiffLocalParameterization : public LocalParameterization /** * @brief Takes ownership of the provided PlusFunctor and MinusFunctor instances */ - AutoDiffLocalParameterization(PlusFunctor * plus_functor, MinusFunctor * minus_functor); + AutoDiffLocalParameterization(PlusFunctor* plus_functor, MinusFunctor* minus_functor); /** * @brief Generalization of the addition operation, implemented by the provided PlusFunctor @@ -99,10 +97,7 @@ class AutoDiffLocalParameterization : public LocalParameterization * @param[out] x_plus_delta The final variable value, of size \p GlobalSize() * @return True if successful, false otherwise */ - bool Plus( - const double * x, - const double * delta, - double * x_plus_delta) const override; + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override; /** * @brief The Jacobian of Plus(x, delta) w.r.t delta at delta = 0, computed using automatic @@ -112,9 +107,7 @@ class AutoDiffLocalParameterization : public LocalParameterization * @param[out] jacobian The Jacobian in row-major order, of size \p GlobalSize() x \p LocalSize() * @return True is successful, false otherwise */ - bool ComputeJacobian( - const double * x, - double * jacobian) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; /** * @brief Generalization of the subtraction operation, implemented by the provided MinusFunctor @@ -125,10 +118,7 @@ class AutoDiffLocalParameterization : public LocalParameterization * LocalSize() * @return True if successful, false otherwise */ - bool Minus( - const double * x1, - const double * x2, - double * delta) const override; + bool Minus(const double* x1, const double* x2, double* delta) const override; /** * @brief The Jacobian of Minus(x1, x2) w.r.t x2 evaluated at x1 = x2 = x, computed using @@ -137,99 +127,91 @@ class AutoDiffLocalParameterization : public LocalParameterization * @param[out] jacobian The Jacobian in row-major order, of size \p LocalSize() x \p GlobalSize() * @return True is successful, false otherwise */ - bool ComputeMinusJacobian( - const double * x, - double * jacobian) const override; + bool ComputeMinusJacobian(const double* x, double* jacobian) const override; /** * @brief The size of the variable parameterization in the nonlinear manifold */ - int GlobalSize() const override {return kGlobalSize;} + int GlobalSize() const override + { + return kGlobalSize; + } /** * @brief The size of a delta vector in the linear tangent space to the nonlinear manifold */ - int LocalSize() const override {return kLocalSize;} + int LocalSize() const override + { + return kLocalSize; + } private: std::unique_ptr plus_functor_; std::unique_ptr minus_functor_; }; -template -AutoDiffLocalParameterization::AutoDiffLocalParameterization() -: plus_functor_(new PlusFunctor()), - minus_functor_(new MinusFunctor()) +template +AutoDiffLocalParameterization::AutoDiffLocalParameterization() + : plus_functor_(new PlusFunctor()), minus_functor_(new MinusFunctor()) { } -template -AutoDiffLocalParameterization::AutoDiffLocalParameterization( - PlusFunctor * plus_functor, - MinusFunctor * minus_functor) -: plus_functor_(plus_functor), - minus_functor_(minus_functor) +template +AutoDiffLocalParameterization::AutoDiffLocalParameterization( + PlusFunctor* plus_functor, MinusFunctor* minus_functor) + : plus_functor_(plus_functor), minus_functor_(minus_functor) { } -template -bool AutoDiffLocalParameterization::Plus( - const double * x, - const double * delta, - double * x_plus_delta) const +template +bool AutoDiffLocalParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { return (*plus_functor_)(x, delta, x_plus_delta); } -template -bool AutoDiffLocalParameterization::ComputeJacobian( - const double * x, - double * jacobian) const +template +bool AutoDiffLocalParameterization::ComputeJacobian( + const double* x, double* jacobian) const { double zero_delta[kLocalSize] = {}; // zero-initialize double x_plus_delta[kGlobalSize]; - const double * parameter_ptrs[2] = {x, zero_delta}; - double * jacobian_ptrs[2] = {NULL, jacobian}; + const double* parameter_ptrs[2] = { x, zero_delta }; + double* jacobian_ptrs[2] = { NULL, jacobian }; #if !CERES_VERSION_AT_LEAST(2, 0, 0) - return ceres::internal::AutoDiff - ::Differentiate(*plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); + return ceres::internal::AutoDiff::Differentiate( + *plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); #else - return ceres::internal::AutoDifferentiate>( - *plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); + return ceres::internal::AutoDifferentiate >( + *plus_functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); #endif } -template -bool AutoDiffLocalParameterization::Minus( - const double * x1, - const double * x2, - double * delta) const +template +bool AutoDiffLocalParameterization::Minus(const double* x1, + const double* x2, + double* delta) const { return (*minus_functor_)(x1, x2, delta); } -template -bool AutoDiffLocalParameterization::ComputeMinusJacobian( - const double * x, - double * jacobian) const +template +bool AutoDiffLocalParameterization::ComputeMinusJacobian( + const double* x, double* jacobian) const { double delta[kLocalSize] = {}; // zero-initialize - const double * parameter_ptrs[2] = {x, x}; - double * jacobian_ptrs[2] = {NULL, jacobian}; + const double* parameter_ptrs[2] = { x, x }; + double* jacobian_ptrs[2] = { NULL, jacobian }; #if !CERES_VERSION_AT_LEAST(2, 0, 0) - return ceres::internal::AutoDiff - ::Differentiate(*minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs); + return ceres::internal::AutoDiff::Differentiate( + *minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs); #else using StaticParameters = ceres::internal::StaticParameterDims; - return ceres::internal::AutoDifferentiate( - *minus_functor_, parameter_ptrs, kLocalSize, delta, jacobian_ptrs); + return ceres::internal::AutoDifferentiate(*minus_functor_, parameter_ptrs, kLocalSize, + delta, jacobian_ptrs); #endif } diff --git a/fuse_core/include/fuse_core/callback_wrapper.hpp b/fuse_core/include/fuse_core/callback_wrapper.hpp index 0d6532823..3180a8401 100644 --- a/fuse_core/include/fuse_core/callback_wrapper.hpp +++ b/fuse_core/include/fuse_core/callback_wrapper.hpp @@ -105,19 +105,18 @@ class CallbackWrapperBase virtual void call() = 0; }; -template +template class CallbackWrapper : public CallbackWrapperBase { public: - using CallbackFunction = std::function; + using CallbackFunction = std::function; /** * @brief Constructor * * @param[in] callback The function to be called from the callback queue */ - explicit CallbackWrapper(CallbackFunction callback) - : callback_(callback) + explicit CallbackWrapper(CallbackFunction callback) : callback_(callback) { } @@ -139,19 +138,18 @@ class CallbackWrapper : public CallbackWrapperBase private: CallbackFunction callback_; //!< The function to execute within the - std::promise promise_; //!< Promise/Future used to return data after the callback is executed + std::promise promise_; //!< Promise/Future used to return data after the callback is executed }; // Specialization to handle 'void' return types // Specifically, promise_.set_value(callback_()) does not work if callback_() returns void. -template<> +template <> inline void CallbackWrapper::call() { callback_(); promise_.set_value(); } - class CallbackAdapter : public rclcpp::Waitable { public: @@ -162,12 +160,10 @@ class CallbackAdapter : public rclcpp::Waitable */ size_t get_number_of_ready_guard_conditions() override; - /** * @brief tell the CallbackGroup that this waitable is ready to execute anything */ - bool is_ready(rcl_wait_set_t const & wait_set) override; - + bool is_ready(rcl_wait_set_t const& wait_set) override; /** * @brief add_to_wait_set is called by rclcpp during NodeWaitables::add_waitable() and @@ -175,15 +171,15 @@ class CallbackAdapter : public rclcpp::Waitable waitable_ptr = std::make_shared(); node->get_node_waitables_interface()->add_waitable(waitable_ptr, (rclcpp::CallbackGroup::SharedPtr) nullptr); */ - void add_to_wait_set(rcl_wait_set_t & wait_set) override; + void add_to_wait_set(rcl_wait_set_t& wait_set) override; std::shared_ptr take_data() override; - void execute(std::shared_ptr const & data) override; + void execute(std::shared_ptr const& data) override; - void addCallback(const std::shared_ptr & callback); + void addCallback(const std::shared_ptr& callback); - void addCallback(std::shared_ptr && callback); + void addCallback(std::shared_ptr&& callback); void removeAllCallbacks(); @@ -195,7 +191,6 @@ class CallbackAdapter : public rclcpp::Waitable std::deque> callback_queue_; }; - } // namespace fuse_core #endif // FUSE_CORE__CALLBACK_WRAPPER_HPP_ diff --git a/fuse_core/include/fuse_core/ceres_macros.hpp b/fuse_core/include/fuse_core/ceres_macros.hpp index d5c648204..a742c3656 100644 --- a/fuse_core/include/fuse_core/ceres_macros.hpp +++ b/fuse_core/include/fuse_core/ceres_macros.hpp @@ -41,9 +41,9 @@ */ /* *INDENT-OFF* */ // Bypass uncrustify -#define CERES_VERSION_AT_LEAST(x, y, z) (CERES_VERSION_MAJOR > x || (CERES_VERSION_MAJOR >= x && \ - (CERES_VERSION_MINOR > y || (CERES_VERSION_MINOR >= y && \ - CERES_VERSION_REVISION >= z)))) +#define CERES_VERSION_AT_LEAST(x, y, z) \ + (CERES_VERSION_MAJOR > x || (CERES_VERSION_MAJOR >= x && \ + (CERES_VERSION_MINOR > y || (CERES_VERSION_MINOR >= y && CERES_VERSION_REVISION >= z)))) /* *INDENT-ON* */ #define CERES_SUPPORTS_MANIFOLDS CERES_VERSION_AT_LEAST(2, 1, 0) diff --git a/fuse_core/include/fuse_core/ceres_options.hpp b/fuse_core/include/fuse_core/ceres_options.hpp index e364b2118..58022f7a3 100644 --- a/fuse_core/include/fuse_core/ceres_options.hpp +++ b/fuse_core/include/fuse_core/ceres_options.hpp @@ -53,10 +53,10 @@ * * For a given Ceres Solver Option , the function ToString calls ceres::ToString */ -#define CERES_OPTION_TO_STRING_DEFINITION(Option) \ - static inline const char * ToString(ceres::Option value) \ - { \ - return ceres::Option ## ToString(value); \ +#define CERES_OPTION_TO_STRING_DEFINITION(Option) \ + static inline const char* ToString(ceres::Option value) \ + { \ + return ceres::Option##ToString(value); \ } /** @@ -64,10 +64,10 @@ * * For a given Ceres Solver Option , the function FromString calls ceres::StringTo */ -#define CERES_OPTION_FROM_STRING_DEFINITION(Option) \ - static inline bool FromString(std::string string_value, ceres::Option * value) \ - { \ - return ceres::StringTo ## Option(string_value, value); \ +#define CERES_OPTION_FROM_STRING_DEFINITION(Option) \ + static inline bool FromString(std::string string_value, ceres::Option* value) \ + { \ + return ceres::StringTo##Option(string_value, value); \ } /** @@ -75,8 +75,8 @@ * * See CERES_OPTION_TO_STRING_DEFINITION and CERES_OPTION_FROM_STRING_DEFINITION. */ -#define CERES_OPTION_STRING_DEFINITIONS(Option) \ - CERES_OPTION_TO_STRING_DEFINITION(Option) \ +#define CERES_OPTION_STRING_DEFINITIONS(Option) \ + CERES_OPTION_TO_STRING_DEFINITION(Option) \ CERES_OPTION_FROM_STRING_DEFINITION(Option) #if !CERES_VERSION_AT_LEAST(2, 0, 0) @@ -88,25 +88,33 @@ namespace ceres { -#define CASESTR(x) case x: return #x -#define STRENUM(x) if (value == #x) {*type = x; return true;} +#define CASESTR(x) \ + case x: \ + return #x +#define STRENUM(x) \ + if (value == #x) \ + { \ + *type = x; \ + return true; \ + } -static void UpperCase(std::string * input) +static void UpperCase(std::string* input) { std::transform(input->begin(), input->end(), input->begin(), ::toupper); } -inline const char * LoggingTypeToString(LoggingType type) +inline const char* LoggingTypeToString(LoggingType type) { - switch (type) { - CASESTR(SILENT); - CASESTR(PER_MINIMIZER_ITERATION); + switch (type) + { + CASESTR(SILENT); + CASESTR(PER_MINIMIZER_ITERATION); default: return "UNKNOWN"; } } -inline bool StringToLoggingType(std::string value, LoggingType * type) +inline bool StringToLoggingType(std::string value, LoggingType* type) { UpperCase(&value); STRENUM(SILENT); @@ -114,17 +122,18 @@ inline bool StringToLoggingType(std::string value, LoggingType * type) return false; } -inline const char * DumpFormatTypeToString(DumpFormatType type) +inline const char* DumpFormatTypeToString(DumpFormatType type) { - switch (type) { - CASESTR(CONSOLE); - CASESTR(TEXTFILE); + switch (type) + { + CASESTR(CONSOLE); + CASESTR(TEXTFILE); default: return "UNKNOWN"; } } -inline bool StringToDumpFormatType(std::string value, DumpFormatType * type) +inline bool StringToDumpFormatType(std::string value, DumpFormatType* type) { UpperCase(&value); STRENUM(CONSOLE); @@ -144,12 +153,12 @@ inline bool StringToDumpFormatType(std::string value, DumpFormatType * type) namespace ceres { -inline bool StringToLoggingType(std::string value, LoggingType * type) +inline bool StringToLoggingType(std::string value, LoggingType* type) { return StringtoLoggingType(value, type); } -inline bool StringToDumpFormatType(std::string value, DumpFormatType * type) +inline bool StringToDumpFormatType(std::string value, DumpFormatType* type) { return StringtoDumpFormatType(value, type); } @@ -188,30 +197,25 @@ CERES_OPTION_STRING_DEFINITIONS(VisibilityClusteringType) * @param[in] default_value - A default value to use if the provided parameter name does not exist * @return The loaded (or default) value */ -template +template T declareCeresParam( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - const std::string & parameter_name, - const T & default_value, - const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor()) + node_interfaces::NodeInterfaces + interfaces, + const std::string& parameter_name, const T& default_value, + const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor()) { - const std::string default_string_value{ToString(default_value)}; + const std::string default_string_value{ ToString(default_value) }; std::string string_value; string_value = getParam(interfaces, parameter_name, default_string_value, parameter_descriptor); T value; - if (!FromString(string_value, &value)) { - RCLCPP_WARN_STREAM( - interfaces.get_node_logging_interface()->get_logger(), - "The requested " << parameter_name << " (" << string_value - << ") is not supported. Using the default value (" << default_string_value - << ") instead."); + if (!FromString(string_value, &value)) + { + RCLCPP_WARN_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "The requested " << parameter_name << " (" << string_value + << ") is not supported. Using the default value (" << default_string_value + << ") instead."); value = default_value; } @@ -227,13 +231,9 @@ T declareCeresParam( * @param[in] namespace_string - Period delimited string to prepend to the loaded parameters' names */ void loadCovarianceOptionsFromROS( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - ceres::Covariance::Options & covariance_options, - const std::string & ns = std::string()); + node_interfaces::NodeInterfaces + interfaces, + ceres::Covariance::Options& covariance_options, const std::string& ns = std::string()); /** * @brief Populate a ceres::Problem::Options object with information from the parameter server @@ -243,12 +243,8 @@ void loadCovarianceOptionsFromROS( * @param[out] problem_options - The ceres::Problem::Options object to update * @param[in] namespace_string - Period delimited string to prepend to the loaded parameters' names */ -void loadProblemOptionsFromROS( - node_interfaces::NodeInterfaces< - node_interfaces::Parameters - > interfaces, - ceres::Problem::Options & problem_options, - const std::string & ns = std::string()); +void loadProblemOptionsFromROS(node_interfaces::NodeInterfaces interfaces, + ceres::Problem::Options& problem_options, const std::string& ns = std::string()); /** * @brief Populate a ceres::Solver::Options object with information from the parameter server @@ -259,13 +255,9 @@ void loadProblemOptionsFromROS( * @param[in] namespace_string - Period delimited string to prepend to the loaded parameters' names */ void loadSolverOptionsFromROS( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - ceres::Solver::Options & solver_options, - const std::string & ns = std::string()); + node_interfaces::NodeInterfaces + interfaces, + ceres::Solver::Options& solver_options, const std::string& ns = std::string()); } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/console.hpp b/fuse_core/include/fuse_core/console.hpp index c30e6737b..9adae01f5 100644 --- a/fuse_core/include/fuse_core/console.hpp +++ b/fuse_core/include/fuse_core/console.hpp @@ -42,11 +42,11 @@ namespace fuse_core // To replicate ROS 1 behavior, the throttle checking conditions were adapted from the logic here: // https://github.com/ros/rosconsole/blob/c9503279e932a04b3d2667cca3d28a8133cacc22/include/ros/console.h #if defined(_MSC_VER) - #define FUSE_LIKELY(x) (x) - #define FUSE_UNLIKELY(x) (x) +#define FUSE_LIKELY(x) (x) +#define FUSE_UNLIKELY(x) (x) #else - #define FUSE_LIKELY(x) __builtin_expect((x), 1) - #define FUSE_UNLIKELY(x) __builtin_expect((x), 0) +#define FUSE_LIKELY(x) __builtin_expect((x), 1) +#define FUSE_UNLIKELY(x) __builtin_expect((x), 0) #endif /** @@ -62,8 +62,7 @@ class DelayedThrottleFilter * * @param[in] The throttle period in seconds */ - explicit DelayedThrottleFilter(const double period) - : period_(std::chrono::duration>(period)) + explicit DelayedThrottleFilter(const double period) : period_(std::chrono::duration>(period)) { reset(); } @@ -82,15 +81,16 @@ class DelayedThrottleFilter */ bool isEnabled() { - const auto now = std::chrono::time_point_cast( - std::chrono::system_clock::now()); + const auto now = std::chrono::time_point_cast(std::chrono::system_clock::now()); - if (last_hit_.time_since_epoch().count() < 0.0) { + if (last_hit_.time_since_epoch().count() < 0.0) + { last_hit_ = now; return true; } - if (FUSE_UNLIKELY(last_hit_ + period_ <= now) || FUSE_UNLIKELY(now < last_hit_)) { + if (FUSE_UNLIKELY(last_hit_ + period_ <= now) || FUSE_UNLIKELY(now < last_hit_)) + { last_hit_ = now; return true; } @@ -103,8 +103,7 @@ class DelayedThrottleFilter */ void reset() { - last_hit_ = std::chrono::time_point_cast( - std::chrono::system_clock::from_time_t(-1)); + last_hit_ = std::chrono::time_point_cast(std::chrono::system_clock::from_time_t(-1)); } private: diff --git a/fuse_core/include/fuse_core/constraint.hpp b/fuse_core/include/fuse_core/constraint.hpp index 1abd6d16d..2e18ee7f0 100644 --- a/fuse_core/include/fuse_core/constraint.hpp +++ b/fuse_core/include/fuse_core/constraint.hpp @@ -66,10 +66,10 @@ * } * @endcode */ -#define FUSE_CONSTRAINT_CLONE_DEFINITION(...) \ - fuse_core::Constraint::UniquePtr clone() const override \ - { \ - return __VA_ARGS__::make_unique(*this); \ +#define FUSE_CONSTRAINT_CLONE_DEFINITION(...) \ + fuse_core::Constraint::UniquePtr clone() const override \ + { \ + return __VA_ARGS__::make_unique(*this); \ } /** @@ -85,22 +85,22 @@ * } * @endcode */ -#define FUSE_CONSTRAINT_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ +#define FUSE_CONSTRAINT_SERIALIZE_DEFINITION(...) \ + void serialize(fuse_core::BinaryOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive& archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive& archive) override \ + { \ + archive >> *this; \ } /** @@ -118,17 +118,17 @@ * } * @endcode */ -#define FUSE_CONSTRAINT_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ +#define FUSE_CONSTRAINT_TYPE_DEFINITION(...) \ + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ } /** @@ -145,10 +145,10 @@ * } * @endcode */ -#define FUSE_CONSTRAINT_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ +#define FUSE_CONSTRAINT_DEFINITIONS(...) \ + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__) /** @@ -165,13 +165,12 @@ * } * @endcode */ -#define FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(...) \ - FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ - FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ +#define FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(...) \ + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ + FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \ FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__) - namespace fuse_core { @@ -213,15 +212,15 @@ class Constraint * * @param[in] variable_uuid_list The list of involved variable UUIDs */ - Constraint(const std::string & source, std::initializer_list variable_uuid_list); + Constraint(const std::string& source, std::initializer_list variable_uuid_list); /** * @brief Constructor * * Accepts an arbitrary number of variable UUIDs stored in a container using iterators. */ - template - Constraint(const std::string & source, VariableUuidIterator first, VariableUuidIterator last); + template + Constraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last); /** * @brief Destructor @@ -241,19 +240,25 @@ class Constraint * * Each constraint will generate a unique, random UUID during construction. */ - const UUID & uuid() const {return uuid_;} + const UUID& uuid() const + { + return uuid_; + } /** * @brief Returns the name of the sensor or motion model that generated this constraint */ - const std::string & source() const {return source_;} + const std::string& source() const + { + return source_; + } /** * @brief Print a human-readable description of the constraint to the provided stream. * * @param stream The stream to write to. Defaults to stdout. */ - virtual void print(std::ostream & stream = std::cout) const = 0; + virtual void print(std::ostream& stream = std::cout) const = 0; /** * @brief Create a new Ceres cost function and return a raw pointer to it. @@ -266,7 +271,7 @@ class Constraint * * @return A base pointer to an instance of a derived ceres::CostFunction. */ - virtual ceres::CostFunction * costFunction() const = 0; + virtual ceres::CostFunction* costFunction() const = 0; /** * @brief Read-only access to the loss. @@ -296,7 +301,7 @@ class Constraint * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const + ceres::LossFunction* lossFunction() const { return loss_ ? loss_->lossFunction() : nullptr; } @@ -316,7 +321,10 @@ class Constraint /** * @brief Read-only access to the ordered list of variable UUIDs involved in this constraint */ - const std::vector & variables() const {return variables_;} + const std::vector& variables() const + { + return variables_; + } /** * @brief Serialize this Constraint into the provided binary archive @@ -328,7 +336,7 @@ class Constraint * * @param[out] archive - The archive to serialize this constraint into */ - virtual void serialize(fuse_core::BinaryOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::BinaryOutputArchive& /* archive */) const = 0; /** * @brief Serialize this Constraint into the provided text archive @@ -340,7 +348,7 @@ class Constraint * * @param[out] archive - The archive to serialize this constraint into */ - virtual void serialize(fuse_core::TextOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::TextOutputArchive& /* archive */) const = 0; /** * @brief Deserialize data from the provided binary archive into this Constraint @@ -352,7 +360,7 @@ class Constraint * * @param[in] archive - The archive holding serialized Constraint data */ - virtual void deserialize(fuse_core::BinaryInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::BinaryInputArchive& /* archive */) = 0; /** * @brief Deserialize data from the provided text archive into this Constraint @@ -364,13 +372,13 @@ class Constraint * * @param[in] archive - The archive holding serialized Constraint data */ - virtual void deserialize(fuse_core::TextInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::TextInputArchive& /* archive */) = 0; private: - std::string source_; //!< The name of the sensor or motion model that generated this constraint - UUID uuid_; //!< The unique ID associated with this constraint - std::vector variables_; //!< The ordered set of variables involved with this constraint - std::shared_ptr loss_{nullptr}; //!< The loss function + std::string source_; //!< The name of the sensor or motion model that generated this constraint + UUID uuid_; //!< The unique ID associated with this constraint + std::vector variables_; //!< The ordered set of variables involved with this constraint + std::shared_ptr loss_{ nullptr }; //!< The loss function // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -387,29 +395,24 @@ class Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & source_; - archive & uuid_; - archive & variables_; - archive & loss_; + archive& source_; + archive& uuid_; + archive& variables_; + archive& loss_; } }; /** * Stream operator implementation used for all derived Constraint classes. */ -std::ostream & operator<<(std::ostream & stream, const Constraint & constraint); - +std::ostream& operator<<(std::ostream& stream, const Constraint& constraint); -template -Constraint::Constraint( - const std::string & source, VariableUuidIterator first, - VariableUuidIterator last) -: source_(source), - uuid_(uuid::generate()), - variables_(first, last) +template +Constraint::Constraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last) + : source_(source), uuid_(uuid::generate()), variables_(first, last) { } diff --git a/fuse_core/include/fuse_core/eigen.hpp b/fuse_core/include/fuse_core/eigen.hpp index 6d9ced923..23d794559 100644 --- a/fuse_core/include/fuse_core/eigen.hpp +++ b/fuse_core/include/fuse_core/eigen.hpp @@ -66,7 +66,7 @@ using Matrix7d = Eigen::Matrix; using Matrix8d = Eigen::Matrix; using Matrix9d = Eigen::Matrix; -template +template using Matrix = Eigen::Matrix; /** @@ -80,8 +80,8 @@ using Matrix = Eigen::Matrix -std::string to_string(const Eigen::DenseBase & m, const int precision = 4) +template +std::string to_string(const Eigen::DenseBase& m, const int precision = 4) { static const Eigen::IOFormat pretty(precision, 0, ", ", "\n", "[", "]"); @@ -98,11 +98,10 @@ std::string to_string(const Eigen::DenseBase & m, const int precision = * property used to check for symmetry. * @return True if the matrix m is symmetric; False, otherwise. */ -template -bool isSymmetric( - const Eigen::DenseBase & m, - const typename Eigen::DenseBase::RealScalar precision = - Eigen::NumTraits::Scalar>::dummy_precision()) +template +bool isSymmetric(const Eigen::DenseBase& m, + const typename Eigen::DenseBase::RealScalar precision = + Eigen::NumTraits::Scalar>::dummy_precision()) { // We do not use `isApprox`: // @@ -112,7 +111,7 @@ bool isSymmetric( // // See: // https://eigen.tuxfamily.org/dox/classEigen_1_1DenseBase.html#ae8443357b808cd393be1b51974213f9c - const auto & derived = m.derived(); + const auto& derived = m.derived(); return (derived - derived.transpose()).cwiseAbs().maxCoeff() < precision; } @@ -122,8 +121,8 @@ bool isSymmetric( * @param[in] m - Square matrix to check PD-ness on. * @return True if the matrix m is PD; False, otherwise. */ -template -bool isPositiveDefinite(const Eigen::DenseBase & m) +template +bool isPositiveDefinite(const Eigen::DenseBase& m) { Eigen::SelfAdjointEigenSolver solver(m); return solver.eigenvalues().minCoeff() > 0.0; diff --git a/fuse_core/include/fuse_core/eigen_gtest.hpp b/fuse_core/include/fuse_core/eigen_gtest.hpp index 5fe3d760c..c29a3a767 100644 --- a/fuse_core/include/fuse_core/eigen_gtest.hpp +++ b/fuse_core/include/fuse_core/eigen_gtest.hpp @@ -58,21 +58,22 @@ namespace testing * @param[in] v2 Actual matrix * @return AssertionSuccess or AssertionFailure */ -template -AssertionResult AssertMatrixEqualHelper( - const char * e1, - const char * e2, - const Eigen::MatrixBase & v1, - const Eigen::MatrixBase & v2) +template +AssertionResult AssertMatrixEqualHelper(const char* e1, const char* e2, const Eigen::MatrixBase& v1, + const Eigen::MatrixBase& v2) { - if (v1 == v2) { + if (v1 == v2) + { return AssertionSuccess(); } Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); - return AssertionFailure() << e1 << " is:\n" << v1.format(clean) << "\n" - << e2 << " is:\n" << v2.format(clean) << "\n" - << "Difference is:\n" << (v1 - v2).format(clean) << "\n"; + return AssertionFailure() << e1 << " is:\n" + << v1.format(clean) << "\n" + << e2 << " is:\n" + << v2.format(clean) << "\n" + << "Difference is:\n" + << (v1 - v2).format(clean) << "\n"; } /** @@ -87,44 +88,33 @@ AssertionResult AssertMatrixEqualHelper( * @param[in] tol Tolerance * @return AssertionSuccess or AssertionFailure */ -template -AssertionResult AssertMatrixNearHelper( - const char * e1, - const char * e2, - const Eigen::MatrixBase & v1, - const Eigen::MatrixBase & v2, - double tol) +template +AssertionResult AssertMatrixNearHelper(const char* e1, const char* e2, const Eigen::MatrixBase& v1, + const Eigen::MatrixBase& v2, double tol) { - if ((v1 - v2).cwiseAbs().maxCoeff() < tol) { + if ((v1 - v2).cwiseAbs().maxCoeff() < tol) + { return AssertionSuccess(); } Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); - return AssertionFailure() << e1 << " is:\n" << v1.format(clean) << "\n" - << e2 << " is:\n" << v2.format(clean) << "\n" - << "Difference is:\n" << (v1 - v2).format(clean) << "\n"; + return AssertionFailure() << e1 << " is:\n" + << v1.format(clean) << "\n" + << e2 << " is:\n" + << v2.format(clean) << "\n" + << "Difference is:\n" + << (v1 - v2).format(clean) << "\n"; } // Internal macro for implementing {EXPECT|ASSERT}_MATRIX_EQ. // Don't use this in your code. -#define GTEST_MATRIX_EQUAL_(v1, v2, on_failure) \ - GTEST_ASSERT_( \ - ::testing::AssertMatrixEqualHelper( \ - #v1, \ - #v2, \ - v1, \ - v2), on_failure) +#define GTEST_MATRIX_EQUAL_(v1, v2, on_failure) \ + GTEST_ASSERT_(::testing::AssertMatrixEqualHelper(#v1, #v2, v1, v2), on_failure) // Internal macro for implementing {EXPECT|ASSERT}_MATRIX_NEAR. // Don't use this in your code. -#define GTEST_MATRIX_NEAR_(v1, v2, tol, on_failure) \ - GTEST_ASSERT_( \ - ::testing::AssertMatrixNearHelper( \ - #v1, \ - #v2, \ - v1, \ - v2, \ - tol), on_failure) +#define GTEST_MATRIX_NEAR_(v1, v2, tol, on_failure) \ + GTEST_ASSERT_(::testing::AssertMatrixNearHelper(#v1, #v2, v1, v2, tol), on_failure) // Define gtest macros for use with Eigen @@ -136,8 +126,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] v1 The expected matrix * @param[in] v2 The actual matrix */ -#define EXPECT_MATRIX_EQ(v1, v2) \ - GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_) +#define EXPECT_MATRIX_EQ(v1, v2) GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_) /** * @brief Fatal check for exact equality of two Eigen matrix-like objects. @@ -147,8 +136,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] v1 The expected matrix * @param[in] v2 The actual matrix */ -#define ASSERT_MATRIX_EQ(v1, v2) \ - GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_) +#define ASSERT_MATRIX_EQ(v1, v2) GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_) /** * @brief Non-fatal check for approximate equality of two Eigen matrix-like objects. @@ -159,8 +147,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] v2 The actual matrix * @param[in] tol The allowed tolerance between any entries in v1 and v2 */ -#define EXPECT_MATRIX_NEAR(v1, v2, tol) \ - GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_) +#define EXPECT_MATRIX_NEAR(v1, v2, tol) GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_) /** * @brief Fatal check for approximate equality of two Eigen matrix-like objects. @@ -171,8 +158,7 @@ AssertionResult AssertMatrixNearHelper( * @param[in] v2 The actual matrix * @param[in] tol The allowed tolerance between any entries in v1 and v2 */ -#define ASSERT_MATRIX_NEAR(v1, v2, tol) \ - GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_) +#define ASSERT_MATRIX_NEAR(v1, v2, tol) GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_) } // namespace testing diff --git a/fuse_core/include/fuse_core/fuse_macros.hpp b/fuse_core/include/fuse_core/fuse_macros.hpp index 707fdf101..6484d12f3 100644 --- a/fuse_core/include/fuse_core/fuse_macros.hpp +++ b/fuse_core/include/fuse_core/fuse_macros.hpp @@ -75,9 +75,9 @@ * definitions. */ #if __cpp_aligned_new - #define FUSE_MAKE_ALIGNED_OPERATOR_NEW() +#define FUSE_MAKE_ALIGNED_OPERATOR_NEW() #else - #define FUSE_MAKE_ALIGNED_OPERATOR_NEW() EIGEN_MAKE_ALIGNED_OPERATOR_NEW +#define FUSE_MAKE_ALIGNED_OPERATOR_NEW() EIGEN_MAKE_ALIGNED_OPERATOR_NEW #endif /** @@ -85,11 +85,11 @@ * * Use in the public section of the class. */ -#define FUSE_SMART_PTR_DEFINITIONS(...) \ - __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_SHARED_DEFINITION(__VA_ARGS__) \ - __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ +#define FUSE_SMART_PTR_DEFINITIONS(...) \ + __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_SHARED_DEFINITION(__VA_ARGS__) \ + __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) /** @@ -103,16 +103,15 @@ * Use in the public section of the class. */ #if __cpp_aligned_new - #define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) +#define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) #else - #define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ - __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ - __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) +#define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ + __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ + __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) #endif /** @@ -123,56 +122,49 @@ * * Use in the public section of the class. */ -#define FUSE_SMART_PTR_ALIASES_ONLY(...) \ - __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ - __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ +#define FUSE_SMART_PTR_ALIASES_ONLY(...) \ + __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \ + __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \ __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) -#define __FUSE_SHARED_PTR_ALIAS(...) \ - using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ +#define __FUSE_SHARED_PTR_ALIAS(...) \ + using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ using ConstSharedPtr = std::shared_ptr; -#define __FUSE_MAKE_SHARED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ +#define __FUSE_MAKE_SHARED_DEFINITION(...) \ + template \ + static std::shared_ptr<__VA_ARGS__> make_shared(Args&&... args) \ + { \ + return std::make_shared<__VA_ARGS__>(std::forward(args)...); \ } -#define __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::allocate_shared<__VA_ARGS__>( \ - Eigen::aligned_allocator<__VA_ARGS__>(), \ - std::forward(args) ...); \ +#define __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(...) \ + template \ + static std::shared_ptr<__VA_ARGS__> make_shared(Args&&... args) \ + { \ + return std::allocate_shared<__VA_ARGS__>(Eigen::aligned_allocator<__VA_ARGS__>(), std::forward(args)...); \ } -#define __FUSE_WEAK_PTR_ALIAS(...) \ - using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ +#define __FUSE_WEAK_PTR_ALIAS(...) \ + using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ using ConstWeakPtr = std::weak_ptr; -#define __FUSE_UNIQUE_PTR_ALIAS(...) \ - using UniquePtr = std::unique_ptr<__VA_ARGS__>; +#define __FUSE_UNIQUE_PTR_ALIAS(...) using UniquePtr = std::unique_ptr<__VA_ARGS__>; #if __cplusplus >= 201402L - #define __FUSE_MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ - } +#define __FUSE_MAKE_UNIQUE_DEFINITION(...) \ + template \ + static std::unique_ptr<__VA_ARGS__> make_unique(Args&&... args) \ + { \ + return std::make_unique<__VA_ARGS__>(std::forward(args)...); \ + } #else - #define __FUSE_MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ - } +#define __FUSE_MAKE_UNIQUE_DEFINITION(...) \ + template \ + static std::unique_ptr<__VA_ARGS__> make_unique(Args&&... args) \ + { \ + return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args)...)); \ + } #endif #endif // FUSE_CORE__FUSE_MACROS_HPP_ diff --git a/fuse_core/include/fuse_core/graph.hpp b/fuse_core/include/fuse_core/graph.hpp index a9f366c84..6e73ffc5d 100644 --- a/fuse_core/include/fuse_core/graph.hpp +++ b/fuse_core/include/fuse_core/graph.hpp @@ -70,22 +70,22 @@ * } * @endcode */ -#define FUSE_GRAPH_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ +#define FUSE_GRAPH_SERIALIZE_DEFINITION(...) \ + void serialize(fuse_core::BinaryOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive& archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive& archive) override \ + { \ + archive >> *this; \ } /** @@ -103,38 +103,37 @@ * } * @endcode */ -#define FUSE_GRAPH_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ +#define FUSE_GRAPH_TYPE_DEFINITION(...) \ + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ } /** -* @brief Convenience function that creates the required pointer aliases, and type() method -* -* Usage: -* @code{.cpp} -* class Derived : public Graph -* { -* public: -* FUSE_GRAPH_DEFINITIONS(Derived) -* // The rest of the derived graph implementation -* } -* @endcode -*/ -#define FUSE_GRAPH_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_GRAPH_TYPE_DEFINITION(__VA_ARGS__) \ + * @brief Convenience function that creates the required pointer aliases, and type() method + * + * Usage: + * @code{.cpp} + * class Derived : public Graph + * { + * public: + * FUSE_GRAPH_DEFINITIONS(Derived) + * // The rest of the derived graph implementation + * } + * @endcode + */ +#define FUSE_GRAPH_DEFINITIONS(...) \ + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_GRAPH_TYPE_DEFINITION(__VA_ARGS__) \ FUSE_GRAPH_SERIALIZE_DEFINITION(__VA_ARGS__) - namespace fuse_core { @@ -210,7 +209,7 @@ class Graph * @param[in] constraint_uuid The UUID of the constraint being searched for * @return True if this constraint already exists, False otherwise */ - virtual bool constraintExists(const UUID & constraint_uuid) const = 0; + virtual bool constraintExists(const UUID& constraint_uuid) const = 0; /** * @brief Add a new constraint to the graph @@ -230,7 +229,7 @@ class Graph * @param[in] constraint_uuid The UUID of the constraint to be removed * @return True if the constraint was removed, false otherwise */ - virtual bool removeConstraint(const UUID & constraint_uuid) = 0; + virtual bool removeConstraint(const UUID& constraint_uuid) = 0; /** * @brief Read-only access to a constraint from the graph by UUID @@ -240,7 +239,7 @@ class Graph * @param[in] constraint_uuid The UUID of the requested constraint * @return The constraint in the graph with the specified UUID */ - virtual const Constraint & getConstraint(const UUID & constraint_uuid) const = 0; + virtual const Constraint& getConstraint(const UUID& constraint_uuid) const = 0; /** * @brief Read-only access to all of the constraints in the graph @@ -257,7 +256,7 @@ class Graph * @return A read-only iterator range containing all constraints that involve the specified * variable */ - virtual const_constraint_range getConnectedConstraints(const UUID & variable_uuid) const = 0; + virtual const_constraint_range getConnectedConstraints(const UUID& variable_uuid) const = 0; /** * @brief Check if the variable already exists in the graph @@ -265,7 +264,7 @@ class Graph * @param[in] variable_uuid The UUID of the variable being searched for * @return True if this variable already exists, False otherwise */ - virtual bool variableExists(const UUID & variable_uuid) const = 0; + virtual bool variableExists(const UUID& variable_uuid) const = 0; /** * @brief Add a new variable to the graph @@ -284,7 +283,7 @@ class Graph * @param[in] variable_uuid The UUID of the variable to be removed * @return True if the variable was removed, false otherwise */ - virtual bool removeVariable(const UUID & variable_uuid) = 0; + virtual bool removeVariable(const UUID& variable_uuid) = 0; /** * @brief Read-only access to a variable in the graph by UUID @@ -294,7 +293,7 @@ class Graph * @param[in] variable_uuid The UUID of the requested variable * @return The variable in the graph with the specified UUID */ - virtual const Variable & getVariable(const UUID & variable_uuid) const = 0; + virtual const Variable& getVariable(const UUID& variable_uuid) const = 0; /** * @brief Read-only access to all of the variables in the graph @@ -311,7 +310,7 @@ class Graph * @return A read-only iterator range containing all variables that involve the specified * constraint */ - virtual const_variable_range getConnectedVariables(const UUID & constraint_uuid) const; + virtual const_variable_range getConnectedVariables(const UUID& constraint_uuid) const; /** * @brief Configure a variable to hold its current value constant during optimization @@ -325,7 +324,7 @@ class Graph * optimization, or if the variable's value is allowed to change during * optimization. */ - virtual void holdVariable(const UUID & variable_uuid, bool hold_constant = true) = 0; + virtual void holdVariable(const UUID& variable_uuid, bool hold_constant = true) = 0; /** * @brief Check whether a variable is on hold or not @@ -333,7 +332,7 @@ class Graph * @param[in] variable_uuid The variable to test * @return True if the variable is on hold, false otherwise */ - virtual bool isVariableOnHold(const UUID & variable_uuid) const = 0; + virtual bool isVariableOnHold(const UUID& variable_uuid) const = 0; /** * @brief Compute the marginal covariance blocks for the requested set of variable pairs. @@ -353,18 +352,17 @@ class Graph * variable's tangent space/local coordinates. Otherwise it is * computed in the variable's parameter space. */ - 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; + 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; /** * @brief Update the graph with the contents of a transaction * * @param[in] transaction A set of variable and constraints additions and deletions */ - void update(const Transaction & transaction); + void update(const Transaction& transaction); /** * @brief Optimize the values of the current set of variables, given the current set of @@ -378,8 +376,7 @@ class Graph * @return A Ceres Solver Summary structure containing information about the * optimization process */ - virtual ceres::Solver::Summary optimize( - const ceres::Solver::Options & options = ceres::Solver::Options()) = 0; + virtual ceres::Solver::Summary optimize(const ceres::Solver::Options& options = ceres::Solver::Options()) = 0; /** * @brief Optimize the values of the current set of variables, given the current set of @@ -396,14 +393,14 @@ class Graph * @return A Ceres Solver Summary structure containing information about the * optimization process */ - virtual ceres::Solver::Summary optimizeFor( - const rclcpp::Duration & max_optimization_time, - const ceres::Solver::Options & options = ceres::Solver::Options(), - rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME)) = 0; // NOTE(CH3): We need to copy - // because clock.now() is non-const + virtual ceres::Solver::Summary + optimizeFor(const rclcpp::Duration& max_optimization_time, + const ceres::Solver::Options& options = ceres::Solver::Options(), + rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME)) = 0; // NOTE(CH3): We need to copy + // because clock.now() is non-const /** - * @brief Evalute the values of the current set of variables, given the current set of + * @brief Evaluate the values of the current set of variables, given the current set of * constraints. * * The values in the graph do not change after the call. @@ -425,20 +422,18 @@ class Graph * solver.googlesource.com/ceres-solver/+/master/include/ceres/problem.h#401 * @return True if the problem evaluation was successful; False, otherwise. */ - virtual bool evaluate( - double * cost, std::vector * residuals = nullptr, - std::vector * gradient = nullptr, - const ceres::Problem::EvaluateOptions & options = ceres::Problem::EvaluateOptions()) const = 0; + virtual bool evaluate(double* cost, std::vector* residuals = nullptr, std::vector* gradient = nullptr, + const ceres::Problem::EvaluateOptions& options = ceres::Problem::EvaluateOptions()) const = 0; /** * @brief Structure containing the cost and residual information for a single constraint. */ struct ConstraintCost { - double cost {}; //!< The pre-loss-function cost of the constraint, computed as the norm of the - //!< residuals - double loss {}; //!< The final cost of the constraint after any loss functions have been - //!< applied + double cost{}; //!< The pre-loss-function cost of the constraint, computed as the norm of the + //!< residuals + double loss{}; //!< The final cost of the constraint after any loss functions have been + //!< applied std::vector residuals; //!< The individual residuals for the constraint }; @@ -451,18 +446,15 @@ class Graph * @param[in] last An iterator pointing to one passed the last UUID of the desired constraints * @param[out] output An output iterator capable of assignment to a ConstraintCost object */ - template - void getConstraintCosts( - UuidForwardIterator first, - UuidForwardIterator last, - OutputIterator output); + template + void getConstraintCosts(UuidForwardIterator first, UuidForwardIterator last, OutputIterator output); /** * @brief Print a human-readable description of the graph to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - virtual void print(std::ostream & stream = std::cout) const = 0; + virtual void print(std::ostream& stream = std::cout) const = 0; /** * @brief Serialize this graph into the provided binary archive @@ -474,7 +466,7 @@ class Graph * * @param[out] archive - The archive to serialize this graph into */ - virtual void serialize(fuse_core::BinaryOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::BinaryOutputArchive& /* archive */) const = 0; /** * @brief Serialize this graph into the provided text archive @@ -486,7 +478,7 @@ class Graph * * @param[out] archive - The archive to serialize this graph into */ - virtual void serialize(fuse_core::TextOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::TextOutputArchive& /* archive */) const = 0; /** * @brief Deserialize data from the provided binary archive into this graph @@ -498,7 +490,7 @@ class Graph * * @param[in] archive - The archive holding serialized graph data */ - virtual void deserialize(fuse_core::BinaryInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::BinaryInputArchive& /* archive */) = 0; /** * @brief Deserialize data from the provided text archive into this graph @@ -510,7 +502,7 @@ class Graph * * @param[in] archive - The archive holding serialized graph data */ - virtual void deserialize(fuse_core::TextInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::TextInputArchive& /* archive */) = 0; private: // Allow Boost Serialization access to private methods @@ -529,8 +521,8 @@ class Graph * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & /* archive */, const unsigned int /* version */) + template + void serialize(Archive& /* archive */, const unsigned int /* version */) { } }; @@ -538,27 +530,25 @@ class Graph /** * Stream operator for printing Graph objects. */ -std::ostream & operator<<(std::ostream & stream, const Graph & graph); +std::ostream& operator<<(std::ostream& stream, const Graph& graph); - -template -void Graph::getConstraintCosts( - UuidForwardIterator first, - UuidForwardIterator last, - OutputIterator output) +template +void Graph::getConstraintCosts(UuidForwardIterator first, UuidForwardIterator last, OutputIterator output) { // @todo(swilliams) When I eventually refactor the Graph class to implement more of the // requirements in the base class, it should be possible to make better use of // the Problem object and avoid creating and deleting the cost and loss // functions. - while (first != last) { + while (first != last) + { // Get the next requested constraint - const auto & constraint = getConstraint(*first); + const auto& constraint = getConstraint(*first); // Collect all of the involved variables - auto parameter_blocks = std::vector(); + auto parameter_blocks = std::vector(); parameter_blocks.reserve(constraint.variables().size()); - for (auto variable_uuid : constraint.variables()) { - const auto & variable = getVariable(variable_uuid); + for (auto variable_uuid : constraint.variables()) + { + const auto& variable = getVariable(variable_uuid); parameter_blocks.push_back(variable.data()); } // Compute the residuals for this constraint using the cost function @@ -568,18 +558,18 @@ void Graph::getConstraintCosts( cost_function->Evaluate(parameter_blocks.data(), cost.residuals.data(), nullptr); // Compute the combined cost cost.cost = - std::sqrt( - std::inner_product( - cost.residuals.begin(), cost.residuals.end(), - cost.residuals.begin(), 0.0)); + std::sqrt(std::inner_product(cost.residuals.begin(), cost.residuals.end(), cost.residuals.begin(), 0.0)); // Apply the loss function, if one is configured auto loss_function = std::unique_ptr(constraint.lossFunction()); - if (loss_function) { + if (loss_function) + { double loss_result[3]; // The Loss function returns the loss-adjusted cost plus the first and // second derivative loss_function->Evaluate(cost.cost, loss_result); cost.loss = loss_result[0]; - } else { + } + else + { cost.loss = cost.cost; } // Add the final cost to the output diff --git a/fuse_core/include/fuse_core/graph_deserializer.hpp b/fuse_core/include/fuse_core/graph_deserializer.hpp index eadd32462..44f61177d 100644 --- a/fuse_core/include/fuse_core/graph_deserializer.hpp +++ b/fuse_core/include/fuse_core/graph_deserializer.hpp @@ -46,7 +46,7 @@ namespace fuse_core /** * @brief Serialize a graph into a message */ -void serializeGraph(const fuse_core::Graph & graph, fuse_msgs::msg::SerializedGraph & msg); +void serializeGraph(const fuse_core::Graph& graph, fuse_msgs::msg::SerializedGraph& msg); /** * @brief Deserialize a graph @@ -73,8 +73,7 @@ class GraphDeserializer * @param[in] msg The SerializedGraph message to be deserialized * @return A unique_ptr to a derived Graph object */ - inline fuse_core::Graph::UniquePtr deserialize( - const fuse_msgs::msg::SerializedGraph::ConstSharedPtr msg) const + inline fuse_core::Graph::UniquePtr deserialize(const fuse_msgs::msg::SerializedGraph::ConstSharedPtr msg) const { return deserialize(*msg); } @@ -88,7 +87,7 @@ class GraphDeserializer * @param[in] msg The SerializedGraph message to be deserialized * @return A unique_ptr to a derived Graph object */ - fuse_core::Graph::UniquePtr deserialize(const fuse_msgs::msg::SerializedGraph & msg) const; + fuse_core::Graph::UniquePtr deserialize(const fuse_msgs::msg::SerializedGraph& msg) const; private: //! Pluginlib class loader for Variable types diff --git a/fuse_core/include/fuse_core/local_parameterization.hpp b/fuse_core/include/fuse_core/local_parameterization.hpp index c6cebcc03..9df7a40dc 100644 --- a/fuse_core/include/fuse_core/local_parameterization.hpp +++ b/fuse_core/include/fuse_core/local_parameterization.hpp @@ -99,11 +99,8 @@ class LocalParameterization * @param[in] x variable of size \p GlobalSize() * @param[in] delta variable of size \p LocalSize() * @param[out] x_plus_delta of size \p GlobalSize() - */ - virtual bool Plus( - const double * x, - const double * delta, - double * x_plus_delta) const = 0; + */ + virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const = 0; /** * @brief The jacobian of Plus(x, delta) w.r.t delta at delta = 0. @@ -112,7 +109,7 @@ class LocalParameterization * @param[out] jacobian a row-major GlobalSize() x LocalSize() matrix. * @return */ - virtual bool ComputeJacobian(const double * x, double * jacobian) const = 0; + virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; /** * @brief Generalization of the subtraction operation @@ -129,10 +126,7 @@ class LocalParameterization * \p LocalSize() * @return True if successful, false otherwise */ - virtual bool Minus( - const double * x, - const double * y, - double * y_minus_x) const = 0; + virtual bool Minus(const double* x, const double* y, double* y_minus_x) const = 0; /** * @brief The jacobian of Minus(x, y) w.r.t y at x == y @@ -142,9 +136,7 @@ class LocalParameterization * GlobalSize() * @return True if successful, false otherwise */ - virtual bool ComputeMinusJacobian( - const double * x, - double * jacobian) const = 0; + virtual bool ComputeMinusJacobian(const double* x, double* jacobian) const = 0; #if CERES_SUPPORTS_MANIFOLDS // If the fuse::LocalParameterization class does not inherit from the @@ -164,27 +156,25 @@ class LocalParameterization * @param[in] global_matrix is a num_rows x GlobalSize row major matrix. * @param[out] local_matrix is a num_rows x LocalSize row major matrix. */ - virtual bool MultiplyByJacobian( - const double * x, - const int num_rows, - const double * global_matrix, - double * local_matrix) const + virtual bool MultiplyByJacobian(const double* x, const int num_rows, const double* global_matrix, + double* local_matrix) const { using Matrix = Eigen::Matrix; using MatrixRef = Eigen::Map; using ConstMatrixRef = Eigen::Map; - if (LocalSize() == 0) { + if (LocalSize() == 0) + { return true; } Matrix jacobian(GlobalSize(), LocalSize()); - if (!ComputeJacobian(x, jacobian.data())) { + if (!ComputeJacobian(x, jacobian.data())) + { return false; } - MatrixRef(local_matrix, num_rows, LocalSize()) = - ConstMatrixRef(global_matrix, num_rows, GlobalSize()) * jacobian; + MatrixRef(local_matrix, num_rows, LocalSize()) = ConstMatrixRef(global_matrix, num_rows, GlobalSize()) * jacobian; return true; } #endif @@ -200,8 +190,10 @@ class LocalParameterization * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & /* archive */, const unsigned int /* version */) {} + template + void serialize(Archive& /* archive */, const unsigned int /* version */) + { + } }; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/loss.hpp b/fuse_core/include/fuse_core/loss.hpp index 8465e574f..b20cbda75 100644 --- a/fuse_core/include/fuse_core/loss.hpp +++ b/fuse_core/include/fuse_core/loss.hpp @@ -58,10 +58,10 @@ * } * @endcode */ -#define FUSE_LOSS_CLONE_DEFINITION(...) \ - fuse_core::Loss::UniquePtr clone() const override \ - { \ - return __VA_ARGS__::make_unique(*this); \ +#define FUSE_LOSS_CLONE_DEFINITION(...) \ + fuse_core::Loss::UniquePtr clone() const override \ + { \ + return __VA_ARGS__::make_unique(*this); \ } /** @@ -77,22 +77,22 @@ * } * @endcode */ -#define FUSE_LOSS_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ +#define FUSE_LOSS_SERIALIZE_DEFINITION(...) \ + void serialize(fuse_core::BinaryOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive& archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive& archive) override \ + { \ + archive >> *this; \ } /** @@ -110,17 +110,17 @@ * } * @endcode */ -#define FUSE_LOSS_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ +#define FUSE_LOSS_TYPE_DEFINITION(...) \ + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ } /** @@ -137,13 +137,12 @@ * } * @endcode */ -#define FUSE_LOSS_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_LOSS_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_LOSS_CLONE_DEFINITION(__VA_ARGS__) \ +#define FUSE_LOSS_DEFINITIONS(...) \ + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_LOSS_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_LOSS_CLONE_DEFINITION(__VA_ARGS__) \ FUSE_LOSS_SERIALIZE_DEFINITION(__VA_ARGS__) - namespace fuse_core { @@ -174,7 +173,7 @@ class Loss FUSE_SMART_PTR_ALIASES_ONLY(Loss) static constexpr ceres::Ownership Ownership = - ceres::Ownership::TAKE_OWNERSHIP; //!< The ownership of the ceres::LossFunction* returned by + ceres::Ownership::TAKE_OWNERSHIP; //!< The ownership of the ceres::LossFunction* returned by //!< lossFunction() /** @@ -197,12 +196,10 @@ class Loss * server. */ virtual void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) = 0; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) = 0; /** * @brief Returns a unique name for this loss function type. @@ -217,7 +214,7 @@ class Loss * * @param stream The stream to write to. Defaults to stdout. */ - virtual void print(std::ostream & stream = std::cout) const = 0; + virtual void print(std::ostream& stream = std::cout) const = 0; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -230,7 +227,7 @@ class Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - virtual ceres::LossFunction * lossFunction() const = 0; + virtual ceres::LossFunction* lossFunction() const = 0; /** * @brief Perform a deep copy of the Loss and return a unique pointer to the copy @@ -254,7 +251,7 @@ class Loss * * @param[out] archive - The archive to serialize this loss function into */ - virtual void serialize(fuse_core::BinaryOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::BinaryOutputArchive& /* archive */) const = 0; /** * @brief Serialize this Loss into the provided text archive @@ -266,7 +263,7 @@ class Loss * * @param[out] archive - The archive to serialize this loss function into */ - virtual void serialize(fuse_core::TextOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::TextOutputArchive& /* archive */) const = 0; /** * @brief Deserialize data from the provided binary archive into this Loss @@ -278,7 +275,7 @@ class Loss * * @param[in] archive - The archive holding serialized Loss data */ - virtual void deserialize(fuse_core::BinaryInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::BinaryInputArchive& /* archive */) = 0; /** * @brief Deserialize data from the provided text archive into this Loss @@ -290,7 +287,7 @@ class Loss * * @param[in] archive - The archive holding serialized Loss data */ - virtual void deserialize(fuse_core::TextInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::TextInputArchive& /* archive */) = 0; private: // Allow Boost Serialization access to private methods @@ -303,8 +300,8 @@ class Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & /* archive */, const unsigned int /* version */) + template + void serialize(Archive& /* archive */, const unsigned int /* version */) { } }; @@ -312,7 +309,7 @@ class Loss /** * Stream operator implementation used for all derived Loss classes. */ -std::ostream & operator<<(std::ostream & stream, const Loss & loss); +std::ostream& operator<<(std::ostream& stream, const Loss& loss); } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/loss_loader.hpp b/fuse_core/include/fuse_core/loss_loader.hpp index 0a9b86d39..5fd4242b5 100644 --- a/fuse_core/include/fuse_core/loss_loader.hpp +++ b/fuse_core/include/fuse_core/loss_loader.hpp @@ -58,17 +58,17 @@ class LossLoader * * @return Loss loader singleton instance */ - static LossLoader & getInstance() + static LossLoader& getInstance() { static LossLoader instance; return instance; } // Delete copy and move constructors and assign operators - LossLoader(const LossLoader &) = delete; - LossLoader(LossLoader &&) = delete; - LossLoader & operator=(const LossLoader &) = delete; - LossLoader & operator=(LossLoader &&) = delete; + LossLoader(const LossLoader&) = delete; + LossLoader(LossLoader&&) = delete; + LossLoader& operator=(const LossLoader&) = delete; + LossLoader& operator=(LossLoader&&) = delete; /** * @brief Create unique instance of a loss function loaded with pluginlib @@ -76,7 +76,7 @@ class LossLoader * @param[in] lookup_name Loss function lookup name * @return Loss function instance handled by an std::unique_ptr<> */ - pluginlib::UniquePtr createUniqueInstance(const std::string & lookup_name) + pluginlib::UniquePtr createUniqueInstance(const std::string& lookup_name) { return loss_loader_.createUniqueInstance(lookup_name); } @@ -85,8 +85,7 @@ class LossLoader /** * @brief Constructor */ - LossLoader() - : loss_loader_("fuse_core", "fuse_core::Loss") + LossLoader() : loss_loader_("fuse_core", "fuse_core::Loss") { } @@ -100,7 +99,7 @@ class LossLoader * @param[in] lookup_name Loss function lookup name * @return Loss function instance handled by an std::unique_ptr<> */ -inline pluginlib::UniquePtr createUniqueLoss(const std::string & lookup_name) +inline pluginlib::UniquePtr createUniqueLoss(const std::string& lookup_name) { return LossLoader::getInstance().createUniqueInstance(lookup_name); } diff --git a/fuse_core/include/fuse_core/macros.hpp b/fuse_core/include/fuse_core/macros.hpp index 3af81587a..1045af348 100644 --- a/fuse_core/include/fuse_core/macros.hpp +++ b/fuse_core/include/fuse_core/macros.hpp @@ -56,7 +56,8 @@ #define FUSE_CORE__MACROS_HPP_ /* *INDENT-OFF* */ -#pragma message("Including header and is deprecated, include instead.") /* NOLINT */ +#pragma message( \ + "Including header and is deprecated, include instead.") /* NOLINT */ /* *INDENT-ON* */ // Required by __MAKE_SHARED_ALIGNED_DEFINITION, that uses Eigen::aligned_allocator(). @@ -79,9 +80,9 @@ * definitions. */ #if __cpp_aligned_new - #define FUSE_MAKE_ALIGNED_OPERATOR_NEW() +#define FUSE_MAKE_ALIGNED_OPERATOR_NEW() #else - #define FUSE_MAKE_ALIGNED_OPERATOR_NEW() EIGEN_MAKE_ALIGNED_OPERATOR_NEW +#define FUSE_MAKE_ALIGNED_OPERATOR_NEW() EIGEN_MAKE_ALIGNED_OPERATOR_NEW #endif /** @@ -89,11 +90,11 @@ * * Use in the public section of the class. */ -#define SMART_PTR_DEFINITIONS(...) \ - __SHARED_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_SHARED_DEFINITION(__VA_ARGS__) \ - __WEAK_PTR_ALIAS(__VA_ARGS__) \ - __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ +#define SMART_PTR_DEFINITIONS(...) \ + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_SHARED_DEFINITION(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ + __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) /** @@ -107,16 +108,15 @@ * Use in the public section of the class. */ #if __cpp_aligned_new - #define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - SMART_PTR_DEFINITIONS(__VA_ARGS__) +#define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) SMART_PTR_DEFINITIONS(__VA_ARGS__) #else - #define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ - __SHARED_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ - __WEAK_PTR_ALIAS(__VA_ARGS__) \ - __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ - __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) +#define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ + __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) #endif /** @@ -127,56 +127,49 @@ * * Use in the public section of the class. */ -#define SMART_PTR_ALIASES_ONLY(...) \ - __SHARED_PTR_ALIAS(__VA_ARGS__) \ - __WEAK_PTR_ALIAS(__VA_ARGS__) \ +#define SMART_PTR_ALIASES_ONLY(...) \ + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ __UNIQUE_PTR_ALIAS(__VA_ARGS__) -#define __SHARED_PTR_ALIAS(...) \ - using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ +#define __SHARED_PTR_ALIAS(...) \ + using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ using ConstSharedPtr = std::shared_ptr; -#define __MAKE_SHARED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ +#define __MAKE_SHARED_DEFINITION(...) \ + template \ + static std::shared_ptr<__VA_ARGS__> make_shared(Args&&... args) \ + { \ + return std::make_shared<__VA_ARGS__>(std::forward(args)...); \ } -#define __MAKE_SHARED_ALIGNED_DEFINITION(...) \ - template \ - static std::shared_ptr<__VA_ARGS__> \ - make_shared(Args && ... args) \ - { \ - return std::allocate_shared<__VA_ARGS__>( \ - Eigen::aligned_allocator<__VA_ARGS__>(), \ - std::forward(args) ...); \ +#define __MAKE_SHARED_ALIGNED_DEFINITION(...) \ + template \ + static std::shared_ptr<__VA_ARGS__> make_shared(Args&&... args) \ + { \ + return std::allocate_shared<__VA_ARGS__>(Eigen::aligned_allocator<__VA_ARGS__>(), std::forward(args)...); \ } -#define __WEAK_PTR_ALIAS(...) \ - using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ +#define __WEAK_PTR_ALIAS(...) \ + using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ using ConstWeakPtr = std::weak_ptr; -#define __UNIQUE_PTR_ALIAS(...) \ - using UniquePtr = std::unique_ptr<__VA_ARGS__>; +#define __UNIQUE_PTR_ALIAS(...) using UniquePtr = std::unique_ptr<__VA_ARGS__>; #if __cplusplus >= 201402L - #define __MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ - } +#define __MAKE_UNIQUE_DEFINITION(...) \ + template \ + static std::unique_ptr<__VA_ARGS__> make_unique(Args&&... args) \ + { \ + return std::make_unique<__VA_ARGS__>(std::forward(args)...); \ + } #else - #define __MAKE_UNIQUE_DEFINITION(...) \ - template \ - static std::unique_ptr<__VA_ARGS__> \ - make_unique(Args && ... args) \ - { \ - return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ - } +#define __MAKE_UNIQUE_DEFINITION(...) \ + template \ + static std::unique_ptr<__VA_ARGS__> make_unique(Args&&... args) \ + { \ + return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args)...)); \ + } #endif #endif // FUSE_CORE__MACROS_HPP_ diff --git a/fuse_core/include/fuse_core/manifold.hpp b/fuse_core/include/fuse_core/manifold.hpp index d62b0da96..7173a6cdc 100644 --- a/fuse_core/include/fuse_core/manifold.hpp +++ b/fuse_core/include/fuse_core/manifold.hpp @@ -112,7 +112,7 @@ class Manifold : public ceres::Manifold * @param[out] x_plus_delta is a \p AmbientSize() vector. * @return Return value indicates if the operation was successful or not. */ - virtual bool Plus(const double * x, const double * delta, double * x_plus_delta) const = 0; + virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const = 0; /** * @brief Compute the derivative of Plus(x, delta) w.r.t delta at delta = 0, @@ -125,7 +125,7 @@ class Manifold : public ceres::Manifold * matrix. * @return */ - virtual bool PlusJacobian(const double * x, double * jacobian) const = 0; + virtual bool PlusJacobian(const double* x, double* jacobian) const = 0; /** * @brief Generalization of the subtraction operation @@ -141,7 +141,7 @@ class Manifold : public ceres::Manifold * @param[out] y_minus_x is a \p TangentSize() vector. * @return Return value indicates if the operation was successful or not. */ - virtual bool Minus(const double * y, const double * x, double * y_minus_x) const = 0; + virtual bool Minus(const double* y, const double* x, double* y_minus_x) const = 0; /** * @brief Compute the derivative of Minus(y, x) w.r.t y at y = x, i.e @@ -152,7 +152,7 @@ class Manifold : public ceres::Manifold * @param[out] jacobian is a row-major \p TangentSize() x \p AmbientSize() matrix. * @return Return value indicates whether the operation was successful or not. */ - virtual bool MinusJacobian(const double * x, double * jacobian) const = 0; + virtual bool MinusJacobian(const double* x, double* jacobian) const = 0; private: // Allow Boost Serialization access to private methods @@ -167,8 +167,10 @@ class Manifold : public ceres::Manifold * @param[in] version - The version of the archive being read/written. * Generally unused. */ - template - void serialize(Archive & /* archive */, const unsigned int /* version */) {} + template + void serialize(Archive& /* archive */, const unsigned int /* version */) + { + } }; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/manifold_adapter.hpp b/fuse_core/include/fuse_core/manifold_adapter.hpp index 730c02bc7..b85c87d6b 100644 --- a/fuse_core/include/fuse_core/manifold_adapter.hpp +++ b/fuse_core/include/fuse_core/manifold_adapter.hpp @@ -65,7 +65,7 @@ class ManifoldAdapter : public fuse_core::Manifold * * @param[in] local_parameterization Raw pointer to a derviced fuse::LocalParameterization object */ - explicit ManifoldAdapter(fuse_core::LocalParameterization * local_parameterization) + explicit ManifoldAdapter(fuse_core::LocalParameterization* local_parameterization) { local_parameterization_.reset(local_parameterization); } @@ -89,7 +89,10 @@ class ManifoldAdapter : public fuse_core::Manifold * * @return int Dimension of the ambient space in which the manifold is embedded. */ - int AmbientSize() const override {return local_parameterization_->GlobalSize();} + int AmbientSize() const override + { + return local_parameterization_->GlobalSize(); + } /** * @brief Dimension of the manifold/tangent space. @@ -98,7 +101,10 @@ class ManifoldAdapter : public fuse_core::Manifold * * @return int Dimension of the manifold/tangent space. */ - int TangentSize() const override {return local_parameterization_->LocalSize();} + int TangentSize() const override + { + return local_parameterization_->LocalSize(); + } /** * @brief x_plus_delta = Plus(x, delta), @@ -112,7 +118,7 @@ class ManifoldAdapter : public fuse_core::Manifold * @param[out] x_plus_delta is a \p AmbientSize() vector. * @return Return value indicates if the operation was successful or not. */ - bool Plus(const double * x, const double * delta, double * x_plus_delta) const override + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { return local_parameterization_->Plus(x, delta, x_plus_delta); } @@ -128,7 +134,7 @@ class ManifoldAdapter : public fuse_core::Manifold * matrix. * @return */ - bool PlusJacobian(const double * x, double * jacobian) const override + bool PlusJacobian(const double* x, double* jacobian) const override { return local_parameterization_->ComputeJacobian(x, jacobian); } @@ -147,7 +153,7 @@ class ManifoldAdapter : public fuse_core::Manifold * @param[out] y_minus_x is a \p TangentSize() vector. * @return Return value indicates if the operation was successful or not. */ - bool Minus(const double * y, const double * x, double * y_minus_x) const override + bool Minus(const double* y, const double* x, double* y_minus_x) const override { return local_parameterization_->Minus(x, y, y_minus_x); } @@ -161,7 +167,7 @@ class ManifoldAdapter : public fuse_core::Manifold * @param[out] jacobian is a row-major \p TangentSize() x \p AmbientSize() matrix. * @return Return value indicates whether the operation was successful or not. */ - bool MinusJacobian(const double * x, double * jacobian) const override + bool MinusJacobian(const double* x, double* jacobian) const override { return local_parameterization_->ComputeMinusJacobian(x, jacobian); } @@ -178,11 +184,11 @@ class ManifoldAdapter : public fuse_core::Manifold * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & local_parameterization_; + archive& boost::serialization::base_object(*this); + archive& local_parameterization_; } /** diff --git a/fuse_core/include/fuse_core/message_buffer.hpp b/fuse_core/include/fuse_core/message_buffer.hpp index f05fc8744..11dde62a4 100644 --- a/fuse_core/include/fuse_core/message_buffer.hpp +++ b/fuse_core/include/fuse_core/message_buffer.hpp @@ -56,7 +56,7 @@ namespace fuse_core * * It is assumed that all messages are received sequentially. */ -template +template class MessageBuffer { public: @@ -70,8 +70,7 @@ class MessageBuffer * method for directly accessing the first member. When dereferenced, an iterator returns a * std::pair&. */ - using message_range = boost::any_range, - boost::forward_traversal_tag>; + using message_range = boost::any_range, boost::forward_traversal_tag>; /** * @brief A range of timestamps @@ -90,7 +89,7 @@ class MessageBuffer * timestamps that are older than the buffer length, an exception will be * thrown. */ - explicit MessageBuffer(const rclcpp::Duration & buffer_length = rclcpp::Duration::max()); + explicit MessageBuffer(const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); /** * @brief Destructor @@ -100,7 +99,7 @@ class MessageBuffer /** * @brief Read-only access to the buffer length */ - const rclcpp::Duration & bufferLength() const + const rclcpp::Duration& bufferLength() const { return buffer_length_; } @@ -108,7 +107,7 @@ class MessageBuffer /** * @brief Write access to the buffer length */ - void bufferLength(const rclcpp::Duration & buffer_length) + void bufferLength(const rclcpp::Duration& buffer_length) { buffer_length_ = buffer_length; } @@ -122,7 +121,7 @@ class MessageBuffer * @param[in] stamp The stamp to assign to the message * @param[in] msg A message */ - void insert(const rclcpp::Time & stamp, const Message & msg); + void insert(const rclcpp::Time& stamp, const Message& msg); /** * @brief Query the buffer for the set of messages between two timestamps @@ -142,9 +141,7 @@ class MessageBuffer * @return An iterator range containing all of the messages between the * specified stamps. */ - message_range query( - const rclcpp::Time & beginning_stamp, const rclcpp::Time & ending_stamp, - bool extended_range = true); + message_range query(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, bool extended_range = true); /** * @brief Read-only access to the current set of timestamps @@ -155,7 +152,7 @@ class MessageBuffer protected: using Buffer = std::deque>; - Buffer buffer_; //!< The container of received messages, sorted by timestamp + Buffer buffer_; //!< The container of received messages, sorted by timestamp rclcpp::Duration buffer_length_; //!< The length of the motion model history. Segments older than //!< \p buffer_length_ will be removed from the motion model //!< history @@ -164,7 +161,7 @@ class MessageBuffer * @brief Helper function used with boost::transform_iterators to convert the internal Buffer * value type into a const rclcpp::Time& iterator compatible with stamp_range */ - static const rclcpp::Time & extractStamp(const typename Buffer::value_type & element) + static const rclcpp::Time& extractStamp(const typename Buffer::value_type& element) { return element.first; } diff --git a/fuse_core/include/fuse_core/message_buffer_impl.hpp b/fuse_core/include/fuse_core/message_buffer_impl.hpp index 218f89ae0..1a3cdebb3 100644 --- a/fuse_core/include/fuse_core/message_buffer_impl.hpp +++ b/fuse_core/include/fuse_core/message_buffer_impl.hpp @@ -46,106 +46,101 @@ namespace fuse_core { -template -MessageBuffer::MessageBuffer(const rclcpp::Duration & buffer_length) -: buffer_length_(buffer_length) +template +MessageBuffer::MessageBuffer(const rclcpp::Duration& buffer_length) : buffer_length_(buffer_length) { } -template -void MessageBuffer::insert(const rclcpp::Time & stamp, const Message & msg) +template +void MessageBuffer::insert(const rclcpp::Time& stamp, const Message& msg) { buffer_.emplace_back(stamp, msg); purgeHistory(); } -template -typename MessageBuffer::message_range MessageBuffer::query( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - bool extended_range) +template +typename MessageBuffer::message_range MessageBuffer::query(const rclcpp::Time& beginning_stamp, + const rclcpp::Time& ending_stamp, + bool extended_range) { // Verify the query is valid - if (ending_stamp < beginning_stamp) { + if (ending_stamp < beginning_stamp) + { std::stringstream beginning_time_ss; beginning_time_ss << beginning_stamp.seconds(); std::stringstream ending_time_ss; ending_time_ss << ending_stamp.seconds(); - throw std::invalid_argument( - "The beginning_stamp (" + beginning_time_ss.str() + ") must be less than or equal to " - "the ending_stamp (" + ending_time_ss.str() + ")."); + throw std::invalid_argument("The beginning_stamp (" + beginning_time_ss.str() + + ") must be less than or equal to " + "the ending_stamp (" + + ending_time_ss.str() + ")."); } // Verify the query is within the bounds of the buffer - if (buffer_.empty() || (beginning_stamp < buffer_.front().first) || - (ending_stamp > buffer_.back().first)) + if (buffer_.empty() || (beginning_stamp < buffer_.front().first) || (ending_stamp > buffer_.back().first)) { std::stringstream requested_time_range_ss; - requested_time_range_ss << "(" << beginning_stamp.seconds() << ", " << ending_stamp.seconds() << - ")"; + requested_time_range_ss << "(" << beginning_stamp.seconds() << ", " << ending_stamp.seconds() << ")"; std::stringstream available_time_range_ss; - if (buffer_.empty()) { + if (buffer_.empty()) + { available_time_range_ss << "(EMPTY)"; - } else { - available_time_range_ss << "(" << buffer_.front().first.seconds() << ", " - << buffer_.back().first.seconds() << ")"; } - throw std::out_of_range( - "The requested time range " + requested_time_range_ss.str() + - " is outside the available time range " + available_time_range_ss.str() + "."); + else + { + available_time_range_ss << "(" << buffer_.front().first.seconds() << ", " << buffer_.back().first.seconds() + << ")"; + } + throw std::out_of_range("The requested time range " + requested_time_range_ss.str() + + " is outside the available time range " + available_time_range_ss.str() + "."); } // Find the entry that is strictly greater than the requested beginning stamp. If the extended // range flag is true, we will then back up one entry. - auto upper_bound_comparison = [](const auto & stamp, const auto & element) -> bool - { - return element.first > stamp; - }; - auto beginning_iter = std::upper_bound( - buffer_.begin(), - buffer_.end(), beginning_stamp, upper_bound_comparison); - if (extended_range) { + auto upper_bound_comparison = [](const auto& stamp, const auto& element) -> bool { return element.first > stamp; }; + auto beginning_iter = std::upper_bound(buffer_.begin(), buffer_.end(), beginning_stamp, upper_bound_comparison); + if (extended_range) + { --beginning_iter; } // Find the entry that is greater than or equal to the ending stamp. If the extended range flag is // false, we will back up one entry. - auto lower_bound_comparison = [](const auto & element, const auto & stamp) -> bool - { - return element.first < stamp; - }; - auto ending_iter = std::lower_bound( - buffer_.begin(), - buffer_.end(), ending_stamp, lower_bound_comparison); - if (extended_range && (ending_iter != buffer_.end())) { + auto lower_bound_comparison = [](const auto& element, const auto& stamp) -> bool { return element.first < stamp; }; + auto ending_iter = std::lower_bound(buffer_.begin(), buffer_.end(), ending_stamp, lower_bound_comparison); + if (extended_range && (ending_iter != buffer_.end())) + { ++ending_iter; } // Return the beginning and ending iterators as an iterator range with the correct deference type return message_range(beginning_iter, ending_iter); } -template +template typename MessageBuffer::stamp_range MessageBuffer::stamps() const { - return stamp_range( - boost::make_transform_iterator(buffer_.begin(), extractStamp), - boost::make_transform_iterator(buffer_.end(), extractStamp)); + return stamp_range(boost::make_transform_iterator(buffer_.begin(), extractStamp), + boost::make_transform_iterator(buffer_.end(), extractStamp)); } -template +template void MessageBuffer::purgeHistory() { // Purge any messages that are more than buffer_length_ seconds older than the most recent entry // A setting of rclcpp::Duration::max() means "keep everything" // And we want to keep at least two entries in buffer at all times, regardless of the stamps. - if ((buffer_length_ == rclcpp::Duration::max()) || (buffer_.size() <= 2)) { + if ((buffer_length_ == rclcpp::Duration::max()) || (buffer_.size() <= 2)) + { return; } // Compute the expiration time carefully, as ROS can't handle negative times - const auto & ending_stamp = buffer_.back().first; + const auto& ending_stamp = buffer_.back().first; rclcpp::Time expiration_time; - if (ending_stamp.seconds() > buffer_length_.seconds()) { + if (ending_stamp.seconds() > buffer_length_.seconds()) + { expiration_time = ending_stamp - buffer_length_; - } else { + } + else + { // NOTE(CH3): Uninitialized. But okay because it's just used for comparison. expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type()); } @@ -154,14 +149,10 @@ void MessageBuffer::purgeHistory() // Be careful to ensure that: // - at least two entries remains at all times // - the buffer covers *at least* until the expiration time. Longer is acceptable. - auto is_greater = [](const auto & stamp, const auto & element) -> bool - { - return element.first > stamp; - }; - auto expiration_iter = std::upper_bound( - buffer_.begin(), - buffer_.end(), expiration_time, is_greater); - if (expiration_iter != buffer_.begin()) { + auto is_greater = [](const auto& stamp, const auto& element) -> bool { return element.first > stamp; }; + auto expiration_iter = std::upper_bound(buffer_.begin(), buffer_.end(), expiration_time, is_greater); + if (expiration_iter != buffer_.begin()) + { // expiration_iter points to the first element > expiration_time. // Back up one entry, to a point that is <= expiration_time buffer_.erase(buffer_.begin(), std::prev(expiration_iter)); diff --git a/fuse_core/include/fuse_core/motion_model.hpp b/fuse_core/include/fuse_core/motion_model.hpp index b833b0b33..7fdbc1746 100644 --- a/fuse_core/include/fuse_core/motion_model.hpp +++ b/fuse_core/include/fuse_core/motion_model.hpp @@ -71,7 +71,7 @@ class MotionModel * @return True if the motion models were generated successfully, false * otherwise */ - virtual bool apply(Transaction & transaction) = 0; + virtual bool apply(Transaction& transaction) = 0; /** * @brief Function to be executed whenever the optimizer has completed a Graph update @@ -87,7 +87,9 @@ class MotionModel * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed * whenever needed. */ - virtual void graphCallback(Graph::ConstSharedPtr /*graph*/) {} + virtual void graphCallback(Graph::ConstSharedPtr /*graph*/) + { + } /** * @brief Perform any required post-construction initialization, such as subscribing to topics or @@ -100,14 +102,13 @@ class MotionModel * * @param[in] name A unique name to give this plugin instance */ - virtual void initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name) = 0; + virtual void initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name) = 0; /** * @brief Get the unique name of this motion model */ - virtual const std::string & name() const = 0; + virtual const std::string& name() const = 0; /** * @brief Function to be executed whenever the optimizer is ready to receive transactions @@ -118,7 +119,9 @@ class MotionModel * the motion model to reset any internal state before the optimizer begins processing after * a reset. No calls to apply() will happen before the optimizer calls start(). */ - virtual void start() {} + virtual void start() + { + } /** * @brief Function to be executed whenever the optimizer is no longer ready to receive @@ -130,7 +133,9 @@ class MotionModel * internal state before the optimizer begins processing after a reset. No calls to apply() * will happen until start() has been called again. */ - virtual void stop() {} + virtual void stop() + { + } protected: /** diff --git a/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp index 897e568ed..17fe3626b 100644 --- a/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp +++ b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp @@ -29,18 +29,11 @@ #include #include - -#define ALL_FUSE_CORE_NODE_INTERFACES \ - fuse_core::node_interfaces::Base, \ - fuse_core::node_interfaces::Clock, \ - fuse_core::node_interfaces::Graph, \ - fuse_core::node_interfaces::Logging, \ - fuse_core::node_interfaces::Parameters, \ - fuse_core::node_interfaces::Services, \ - fuse_core::node_interfaces::TimeSource, \ - fuse_core::node_interfaces::Timers, \ - fuse_core::node_interfaces::Topics, \ - fuse_core::node_interfaces::Waitables +#define ALL_FUSE_CORE_NODE_INTERFACES \ + fuse_core::node_interfaces::Base, fuse_core::node_interfaces::Clock, fuse_core::node_interfaces::Graph, \ + fuse_core::node_interfaces::Logging, fuse_core::node_interfaces::Parameters, \ + fuse_core::node_interfaces::Services, fuse_core::node_interfaces::TimeSource, \ + fuse_core::node_interfaces::Timers, fuse_core::node_interfaces::Topics, fuse_core::node_interfaces::Waitables namespace fuse_core { @@ -57,7 +50,7 @@ using Timers = rclcpp::node_interfaces::NodeTimersInterface; using Topics = rclcpp::node_interfaces::NodeTopicsInterface; using Waitables = rclcpp::node_interfaces::NodeWaitablesInterface; -template +template using NodeInterfaces = ::rclcpp::node_interfaces::NodeInterfaces; } // namespace node_interfaces } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/parameter.hpp b/fuse_core/include/fuse_core/parameter.hpp index bab6bcecf..b4376e9f1 100644 --- a/fuse_core/include/fuse_core/parameter.hpp +++ b/fuse_core/include/fuse_core/parameter.hpp @@ -50,7 +50,7 @@ namespace fuse_core { // Helper function to get a namespace string with a '.' suffix, but only if not empty -std::string joinParameterName(const std::string & left, const std::string & right); +std::string joinParameterName(const std::string& left, const std::string& right); // NOTE(CH3): Some of these basically mimic the behavior from rclcpp's node.hpp, but for interfaces @@ -70,30 +70,34 @@ std::string joinParameterName(const std::string & left, const std::string & righ * @return The value of the parameter. * @throws rclcpp::exceptions::InvalidParameterTypeException if the parameter type does not match */ -template +template T getParam( - node_interfaces::NodeInterfaces interfaces, - const std::string & parameter_name, - const T & default_value, - const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor(), - bool ignore_override = false) + node_interfaces::NodeInterfaces interfaces, const std::string& parameter_name, + const T& default_value, + const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false) { auto params_interface = interfaces.get_node_parameters_interface(); - if (params_interface->has_parameter(parameter_name)) { + if (params_interface->has_parameter(parameter_name)) + { return params_interface->get_parameter(parameter_name).get_parameter_value().get(); - } else { - try { - return params_interface->declare_parameter( - parameter_name, rclcpp::ParameterValue(default_value), parameter_descriptor, ignore_override - ).get(); - } catch (const rclcpp::ParameterTypeException & ex) { + } + else + { + try + { + return params_interface + ->declare_parameter(parameter_name, rclcpp::ParameterValue(default_value), parameter_descriptor, + ignore_override) + .get(); + } + catch (const rclcpp::ParameterTypeException& ex) + { throw rclcpp::exceptions::InvalidParameterTypeException(parameter_name, ex.what()); } } } - /** * @brief Compatibility wrapper for ros2 params in ros1 syntax * @@ -109,22 +113,23 @@ T getParam( * @return The value of the parameter. * @throws rclcpp::exceptions::InvalidParameterTypeException if the parameter type does not match */ -template +template T getParam( - node_interfaces::NodeInterfaces interfaces, - const std::string & parameter_name, - const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = - rcl_interfaces::msg::ParameterDescriptor(), - bool ignore_override = false) + node_interfaces::NodeInterfaces interfaces, const std::string& parameter_name, + const rcl_interfaces::msg::ParameterDescriptor& parameter_descriptor = rcl_interfaces::msg::ParameterDescriptor(), + bool ignore_override = false) { // get advantage of parameter value template magic to get the correct rclcpp::ParameterType from T // NOTE(CH3): For the same reason we can't defer to the overload of getParam - rclcpp::ParameterValue value{T{}}; - try { - return interfaces.get_node_parameters_interface()->declare_parameter( - parameter_name, value.get_type(), parameter_descriptor, ignore_override - ).get(); - } catch (const rclcpp::ParameterTypeException & ex) { + rclcpp::ParameterValue value{ T{} }; + try + { + return interfaces.get_node_parameters_interface() + ->declare_parameter(parameter_name, value.get_type(), parameter_descriptor, ignore_override) + .get(); + } + catch (const rclcpp::ParameterTypeException& ex) + { throw rclcpp::exceptions::InvalidParameterTypeException(parameter_name, ex.what()); } } @@ -133,11 +138,9 @@ namespace detail { /** @brief Internal function for unit testing. * @internal -*/ + */ std::unordered_set -list_parameter_override_prefixes( - const std::map & overrides, - std::string prefix); +list_parameter_override_prefixes(const std::map& overrides, std::string prefix); } // namespace detail /** @@ -161,11 +164,10 @@ list_parameter_override_prefixes( * @param[in] prefix - the parameter prefix * @param[in] max_depth - how deep to return parameter override names, or 0 for * unlimited depth. -*/ + */ std::unordered_set -list_parameter_override_prefixes( - node_interfaces::NodeInterfaces interfaces, - std::string prefix); +list_parameter_override_prefixes(node_interfaces::NodeInterfaces interfaces, + std::string prefix); /** * @brief Utility method for handling required ROS params @@ -175,24 +177,18 @@ list_parameter_override_prefixes( * @param[out] value - The ROS parameter value for the \p key * @throws std::runtime_error if the parameter does not exist */ -inline -void getParamRequired( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - const std::string & key, - std::string & value -) +inline void getParamRequired( + node_interfaces::NodeInterfaces + interfaces, + const std::string& key, std::string& value) { std::string default_value = ""; value = getParam(interfaces, key, default_value); - if (value == default_value) { - const std::string error = - "Could not find required parameter " + key + " in namespace " + - interfaces.get_node_base_interface()->get_namespace(); + if (value == default_value) + { + const std::string error = "Could not find required parameter " + key + " in namespace " + + interfaces.get_node_base_interface()->get_namespace(); RCLCPP_FATAL_STREAM(interfaces.get_node_logging_interface()->get_logger(), error); throw std::runtime_error(error); @@ -210,25 +206,19 @@ void getParamRequired( * @param[in] strict - Whether to check the loaded value is strictly positive or not, i.e. whether 0 * is accepted or not */ -template::value || std::is_floating_point::value>> -void getPositiveParam( - node_interfaces::NodeInterfaces< - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - const std::string & parameter_name, - T & default_value, - const bool strict = true -) +template ::value || std::is_floating_point::value>> +void getPositiveParam(node_interfaces::NodeInterfaces interfaces, + const std::string& parameter_name, T& default_value, const bool strict = true) { T value = getParam(interfaces, parameter_name, default_value); - if (value < 0 || (strict && value == 0)) { - RCLCPP_WARN_STREAM( - interfaces.get_node_logging_interface()->get_logger(), - "The requested " << parameter_name.c_str() << " is <" << (strict ? "=" : "") - << " 0. Using the default value (" << default_value << ") instead."); - } else { + if (value < 0 || (strict && value == 0)) + { + RCLCPP_WARN_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "The requested " << parameter_name.c_str() << " is <" << (strict ? "=" : "") + << " 0. Using the default value (" << default_value << ") instead."); + } + else + { default_value = value; } } @@ -243,13 +233,9 @@ void getPositiveParam( * @param[in] strict - Whether to check the loaded value is strictly positive or not, i.e. whether 0 * is accepted or not */ -inline void getPositiveParam( - node_interfaces::NodeInterfaces< - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - const std::string & parameter_name, - rclcpp::Duration & default_value, const bool strict = true) +inline void +getPositiveParam(node_interfaces::NodeInterfaces interfaces, + const std::string& parameter_name, rclcpp::Duration& default_value, const bool strict = true) { double default_value_sec = default_value.seconds(); getPositiveParam(interfaces, parameter_name, default_value_sec, strict); @@ -270,15 +256,10 @@ inline void getPositiveParam( * parameter name does not exist * @return The loaded (or default) covariance matrix, generated from the diagonal vector */ -template +template fuse_core::Matrix getCovarianceDiagonalParam( - node_interfaces::NodeInterfaces< - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - const std::string & parameter_name, - Scalar default_value -) + node_interfaces::NodeInterfaces interfaces, + const std::string& parameter_name, Scalar default_value) { using Vector = typename Eigen::Matrix; @@ -286,19 +267,16 @@ fuse_core::Matrix getCovarianceDiagonalParam( diagonal = getParam(interfaces, parameter_name, diagonal); const auto diagonal_size = diagonal.size(); - if (diagonal_size != Size) { - throw std::invalid_argument( - "Invalid size of " + std::to_string(diagonal_size) + ", expected " + - std::to_string(Size)); + if (diagonal_size != Size) + { + throw std::invalid_argument("Invalid size of " + std::to_string(diagonal_size) + ", expected " + + std::to_string(Size)); } - if (std::any_of( - diagonal.begin(), diagonal.end(), - [](const auto & value) {return value < Scalar(0);})) // NOLINT(whitespace/braces) + if (std::any_of(diagonal.begin(), diagonal.end(), + [](const auto& value) { return value < Scalar(0); })) // NOLINT(whitespace/braces) { - throw std::invalid_argument( - "Invalid negative diagonal values in " + - fuse_core::to_string(Vector(diagonal.data()))); + throw std::invalid_argument("Invalid negative diagonal values in " + fuse_core::to_string(Vector(diagonal.data()))); } return Vector(diagonal.data()).asDiagonal(); @@ -312,16 +290,11 @@ fuse_core::Matrix getCovarianceDiagonalParam( * @return Loss function or nullptr if the parameter does not exist */ inline fuse_core::Loss::SharedPtr loadLossConfig( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - const std::string & name -) + node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { - if (!interfaces.get_node_parameters_interface()->has_parameter( - name + ".type")) + if (!interfaces.get_node_parameters_interface()->has_parameter(name + ".type")) { return {}; } diff --git a/fuse_core/include/fuse_core/publisher.hpp b/fuse_core/include/fuse_core/publisher.hpp index 113b94605..6690b4fd0 100644 --- a/fuse_core/include/fuse_core/publisher.hpp +++ b/fuse_core/include/fuse_core/publisher.hpp @@ -84,14 +84,13 @@ class Publisher * * @param[in] name A unique name to give this plugin instance */ - virtual void initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name) = 0; + virtual void initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name) = 0; /** * @brief Get the unique name of this publisher */ - virtual const std::string & name() const = 0; + virtual const std::string& name() const = 0; /** * @brief Notify the publisher that an optimization cycle is complete, and about changes to the @@ -120,7 +119,9 @@ class Publisher * the publisher to reset any internal state before the optimizer begins processing after a * reset. No calls to notify() will happen before the optimizer calls start(). */ - virtual void start() {} + virtual void start() + { + } /** * @brief Function to be executed whenever the optimizer is no longer ready to receive @@ -132,7 +133,9 @@ class Publisher * internal state before the optimizer begins processing after a reset. No calls to notify() * will happen until start() has been called again. */ - virtual void stop() {} + virtual void stop() + { + } }; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/sensor_model.hpp b/fuse_core/include/fuse_core/sensor_model.hpp index 11f5a9639..935f0ea24 100644 --- a/fuse_core/include/fuse_core/sensor_model.hpp +++ b/fuse_core/include/fuse_core/sensor_model.hpp @@ -48,7 +48,7 @@ namespace fuse_core * @brief The signature of the callback function that will be executed for every generated * transaction object. */ -using TransactionCallback = std::function; +using TransactionCallback = std::function; /** * @brief The interface definition for sensor model plugins in the fuse ecosystem. @@ -91,29 +91,29 @@ class SensorModel * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed * whenever needed. */ - virtual void graphCallback(Graph::ConstSharedPtr /*graph*/) {} + virtual void graphCallback(Graph::ConstSharedPtr /*graph*/) + { + } /** - * @brief Perform any required post-construction initialization, such as subscribing to topics or - * reading from the parameter server. - * - * This will be called on each plugin after construction, and after the ROS node has been - * initialized. Plugins are encouraged to subnamespace any of their parameters to prevent - * conflicts and allow the same plugin to be used multiple times with different settings and - * topics. - * - * @param[in] name A unique name to give this plugin instance - * @param[in] transaction_callback The function to call every time a transaction is published - */ - virtual void initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name, - TransactionCallback transaction_callback) = 0; + * @brief Perform any required post-construction initialization, such as subscribing to topics or + * reading from the parameter server. + * + * This will be called on each plugin after construction, and after the ROS node has been + * initialized. Plugins are encouraged to subnamespace any of their parameters to prevent + * conflicts and allow the same plugin to be used multiple times with different settings and + * topics. + * + * @param[in] name A unique name to give this plugin instance + * @param[in] transaction_callback The function to call every time a transaction is published + */ + virtual void initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name, TransactionCallback transaction_callback) = 0; /** * @brief Get the unique name of this sensor */ - virtual const std::string & name() const = 0; + virtual const std::string& name() const = 0; /** * @brief Function to be executed whenever the optimizer is ready to receive transactions @@ -126,7 +126,9 @@ class SensorModel * * The sensor model must not send any transactions to the optimizer before start() is called. */ - virtual void start() {} + virtual void start() + { + } /** * @brief Function to be executed whenever the optimizer is no longer ready to receive @@ -139,7 +141,9 @@ class SensorModel * * The sensor model must not send any transactions to the optimizer after stop() is called. */ - virtual void stop() {} + virtual void stop() + { + } protected: /** diff --git a/fuse_core/include/fuse_core/serialization.hpp b/fuse_core/include/fuse_core/serialization.hpp index a6e1b21a4..9a73f804f 100644 --- a/fuse_core/include/fuse_core/serialization.hpp +++ b/fuse_core/include/fuse_core/serialization.hpp @@ -77,12 +77,12 @@ class MessageBufferStreamSource * * @param[in] data A byte vector from a ROS message */ - explicit MessageBufferStreamSource(const std::vector & data); + explicit MessageBufferStreamSource(const std::vector& data); /** * @brief The stream source is non-copyable */ - MessageBufferStreamSource operator=(const MessageBufferStreamSource &) = delete; + MessageBufferStreamSource operator=(const MessageBufferStreamSource&) = delete; /** * @brief Read up to n characters from the data vector @@ -93,11 +93,11 @@ class MessageBufferStreamSource * @param[in] n The number of bytes to read from the stream * @return The number of bytes read, or -1 to indicate EOF */ - std::streamsize read(char_type * s, std::streamsize n); + std::streamsize read(char_type* s, std::streamsize n); private: - const std::vector & data_; //!< Reference to the source container - size_t index_; //!< The next vector index to read + const std::vector& data_; //!< Reference to the source container + size_t index_; //!< The next vector index to read }; /** @@ -117,12 +117,12 @@ class MessageBufferStreamSink * * @param[in] data A byte vector from a ROS message */ - explicit MessageBufferStreamSink(std::vector & data); + explicit MessageBufferStreamSink(std::vector& data); /** * @brief The stream sink is non-copyable */ - MessageBufferStreamSink operator=(const MessageBufferStreamSink &) = delete; + MessageBufferStreamSink operator=(const MessageBufferStreamSink&) = delete; /** * @brief Write n characters to the data vector @@ -133,10 +133,10 @@ class MessageBufferStreamSink * @param[in] n The number of bytes to write to the stream * @return The number of bytes written */ - std::streamsize write(const char_type * s, std::streamsize n); + std::streamsize write(const char_type* s, std::streamsize n); private: - std::vector & data_; //!< Reference to the destination container + std::vector& data_; //!< Reference to the destination container }; } // namespace fuse_core @@ -149,13 +149,13 @@ namespace serialization /** * @brief Serialize a rclcpp::Time variable using Boost Serialization */ -template -void serialize(Archive & archive, rclcpp::Time & stamp, const unsigned int /* version */) +template +void serialize(Archive& archive, rclcpp::Time& stamp, const unsigned int /* version */) { auto nanoseconds = stamp.nanoseconds(); auto clock_type = stamp.get_clock_type(); - archive & nanoseconds; - archive & clock_type; + archive& nanoseconds; + archive& clock_type; stamp = rclcpp::Time(nanoseconds, clock_type); } @@ -165,21 +165,21 @@ void serialize(Archive & archive, rclcpp::Time & stamp, const unsigned int /* ve * https://stackoverflow.com/questions/54534047/eigen-matrix- * boostserialization-c17/54535484#54535484 */ -template -inline void serialize( - Archive & archive, - Eigen::Matrix & matrix, - const unsigned int /* version */) +template +inline void serialize(Archive& archive, Eigen::Matrix& matrix, + const unsigned int /* version */) { Eigen::Index rows = matrix.rows(); Eigen::Index cols = matrix.cols(); - archive & rows; - archive & cols; - if (rows != matrix.rows() || cols != matrix.cols()) { + archive& rows; + archive& cols; + if (rows != matrix.rows() || cols != matrix.cols()) + { matrix.resize(rows, cols); } - if (matrix.size() != 0) { - archive & boost::serialization::make_array(matrix.data(), rows * cols); + if (matrix.size() != 0) + { + archive& boost::serialization::make_array(matrix.data(), rows * cols); } } diff --git a/fuse_core/include/fuse_core/throttled_callback.hpp b/fuse_core/include/fuse_core/throttled_callback.hpp index 5afa1bbea..958229794 100644 --- a/fuse_core/include/fuse_core/throttled_callback.hpp +++ b/fuse_core/include/fuse_core/throttled_callback.hpp @@ -52,7 +52,7 @@ namespace fuse_core * * @tparam Callback The std::function callback */ -template +template class ThrottledCallback { public: @@ -67,23 +67,20 @@ class ThrottledCallback * throttling * @param[in] clock The clock to throttle against. Defaults to using RCL_SYSTEM_TIME */ - ThrottledCallback( - Callback && keep_callback = nullptr, // NOLINT(whitespace/operators) - Callback && drop_callback = nullptr, // NOLINT(whitespace/operators) - const rclcpp::Duration & throttle_period = rclcpp::Duration(0, 0), - rclcpp::Clock::SharedPtr clock = std::make_shared()) - : keep_callback_(keep_callback) - , drop_callback_(drop_callback) - , throttle_period_(throttle_period) - , clock_(clock) - {} + ThrottledCallback(Callback&& keep_callback = nullptr, // NOLINT(whitespace/operators) + Callback&& drop_callback = nullptr, // NOLINT(whitespace/operators) + const rclcpp::Duration& throttle_period = rclcpp::Duration(0, 0), + rclcpp::Clock::SharedPtr clock = std::make_shared()) + : keep_callback_(keep_callback), drop_callback_(drop_callback), throttle_period_(throttle_period), clock_(clock) + { + } /** * @brief Throttle period getter * * @return The current throttle period duration in seconds being used */ - const rclcpp::Duration & getThrottlePeriod() const + const rclcpp::Duration& getThrottlePeriod() const { return throttle_period_; } @@ -106,7 +103,7 @@ class ThrottledCallback * * @param[in] throttle_period The new throttle period duration in seconds to use */ - void setThrottlePeriod(const rclcpp::Duration & throttle_period) + void setThrottlePeriod(const rclcpp::Duration& throttle_period) { throttle_period_ = throttle_period; } @@ -116,7 +113,7 @@ class ThrottledCallback * * @param[in] keep_callback The new keep callback to use */ - void setKeepCallback(const Callback & keep_callback) + void setKeepCallback(const Callback& keep_callback) { keep_callback_ = keep_callback; } @@ -126,7 +123,7 @@ class ThrottledCallback * * @param[in] drop_callback The new drop callback to use */ - void setDropCallback(const Callback & drop_callback) + void setDropCallback(const Callback& drop_callback) { drop_callback_ = drop_callback; } @@ -136,7 +133,7 @@ class ThrottledCallback * * @return The last time the keep callback was called */ - const rclcpp::Time & getLastCalledTime() const + const rclcpp::Time& getLastCalledTime() const { return last_called_time_; } @@ -147,8 +144,8 @@ class ThrottledCallback * * @param[in] args The input arguments */ - template - void callback(Args &&... args) + template + void callback(Args&&... args) { // Keep the callback if: // @@ -158,20 +155,25 @@ class ThrottledCallback // (c) The elpased time between now and the last called time is greater than the throttle period rclcpp::Time now = clock_->now(); - if ((last_called_time_.nanoseconds() == 0) || - (throttle_period_.nanoseconds() == 0) || - now - last_called_time_ > throttle_period_) + if ((last_called_time_.nanoseconds() == 0) || (throttle_period_.nanoseconds() == 0) || + now - last_called_time_ > throttle_period_) { - if (keep_callback_) { + if (keep_callback_) + { keep_callback_(std::forward(args)...); } - if (last_called_time_.nanoseconds() == 0) { + if (last_called_time_.nanoseconds() == 0) + { last_called_time_ = now; - } else { + } + else + { last_called_time_ += throttle_period_; } - } else if (drop_callback_) { + } + else if (drop_callback_) + { drop_callback_(std::forward(args)...); } } @@ -181,18 +183,18 @@ class ThrottledCallback * * @param[in] args The input arguments */ - template - void operator()(Args &&... args) + template + void operator()(Args&&... args) { callback(std::forward(args)...); } private: - Callback keep_callback_; //!< The callback to call when kept, i.e. not dropped - Callback drop_callback_; //!< The callback to call when dropped because of throttling + Callback keep_callback_; //!< The callback to call when kept, i.e. not dropped + Callback drop_callback_; //!< The callback to call when dropped because of throttling rclcpp::Duration throttle_period_; //!< The throttling period duration in seconds - rclcpp::Clock::SharedPtr clock_; //!< The clock to throttle against - rclcpp::Time last_called_time_; //!< The last time the keep callback was called + rclcpp::Clock::SharedPtr clock_; //!< The clock to throttle against + rclcpp::Time last_called_time_; //!< The last time the keep callback was called }; /** @@ -200,9 +202,8 @@ class ThrottledCallback * * @tparam M The ROS message type */ -template -using ThrottledMessageCallback = - ThrottledCallback>; +template +using ThrottledMessageCallback = ThrottledCallback>; } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/timestamp_manager.hpp b/fuse_core/include/fuse_core/timestamp_manager.hpp index 413ed6b9f..758395270 100644 --- a/fuse_core/include/fuse_core/timestamp_manager.hpp +++ b/fuse_core/include/fuse_core/timestamp_manager.hpp @@ -85,10 +85,9 @@ class TimestampManager * ending_stamp. The variables should include initial values for the * optimizer. */ - using MotionModelFunction = std::function & constraints, - std::vector & variables)>; + using MotionModelFunction = + std::function& constraints, std::vector& variables)>; /** * @brief A range of timestamps @@ -110,9 +109,8 @@ class TimestampManager * timestamps that are older than the buffer length, an exception will be * thrown. */ - explicit TimestampManager( - MotionModelFunction generator, - const rclcpp::Duration & buffer_length = rclcpp::Duration::max()); + explicit TimestampManager(MotionModelFunction generator, + const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); /** * @brief Constructor that accepts the motion model generator as a member function pointer and @@ -128,15 +126,11 @@ class TimestampManager * timestamps that are older than the buffer length, an exception will be * thrown. */ - template - TimestampManager( - void(T::* fp)( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables), - T * obj, - const rclcpp::Duration & buffer_length = rclcpp::Duration::max()); + template + TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables), + T* obj, const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); /** * @brief Constructor that accepts the motion model generator as a const member function pointer @@ -152,15 +146,11 @@ class TimestampManager * timestamps that are older than the buffer length, an exception will be * thrown. */ - template - TimestampManager( - void(T::* fp)( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables) const, - T * obj, - const rclcpp::Duration & buffer_length = rclcpp::Duration::max()); + template + TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables) const, + T* obj, const rclcpp::Duration& buffer_length = rclcpp::Duration::max()); /** * @brief Destructor @@ -170,7 +160,7 @@ class TimestampManager /** * @brief Read-only access to the buffer length */ - const rclcpp::Duration & bufferLength() const + const rclcpp::Duration& bufferLength() const { return buffer_length_; } @@ -178,7 +168,7 @@ class TimestampManager /** * @brief Write access to the buffer length */ - void bufferLength(const rclcpp::Duration & buffer_length) + void bufferLength(const rclcpp::Duration& buffer_length) { buffer_length_ = buffer_length; } @@ -211,7 +201,7 @@ class TimestampManager * @throws std::invalid_argument If timestamps are not within the defined buffer length of the * motion model */ - void query(Transaction & transaction, bool update_variables = false); + void query(Transaction& transaction, bool update_variables = false); /** * @brief Read-only access to the current set of timestamps @@ -232,20 +222,14 @@ class TimestampManager std::vector variables; explicit MotionModelSegment(rcl_clock_type_t clock_t) - : beginning_stamp{0, 0, clock_t}, - ending_stamp{0, 0, clock_t} + : beginning_stamp{ 0, 0, clock_t }, ending_stamp{ 0, 0, clock_t } { } - MotionModelSegment( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - const std::vector & constraints, - const std::vector & variables) - : beginning_stamp(beginning_stamp), - ending_stamp(ending_stamp), - constraints(constraints), - variables(variables) + MotionModelSegment(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + const std::vector& constraints, + const std::vector& variables) + : beginning_stamp(beginning_stamp), ending_stamp(ending_stamp), constraints(constraints), variables(variables) { } }; @@ -259,11 +243,11 @@ class TimestampManager */ using MotionModelHistory = std::map; - MotionModelFunction generator_; //!< Users upplied function that generates motion model - //!< constraints - rclcpp::Duration buffer_length_; //!< The length of the motion model history. Segments older than - //!< \p buffer_length_ will be removed from the motion model - //!< history + MotionModelFunction generator_; //!< Users upplied function that generates motion model + //!< constraints + rclcpp::Duration buffer_length_; //!< The length of the motion model history. Segments older than + //!< \p buffer_length_ will be removed from the motion model + //!< history MotionModelHistory motion_model_history_; //!< Container that stores all previously generated //!< motion models @@ -277,10 +261,7 @@ class TimestampManager * @param[out] transaction A transaction object to be updated with the changes caused by * addSegment */ - void addSegment( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - Transaction & transaction); + void addSegment(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, Transaction& transaction); /** * @brief Remove an existing MotionModelSegment, updating the provided transaction. @@ -292,9 +273,7 @@ class TimestampManager * @param[out] transaction A transaction object to be updated with the changes caused by * removeSegment */ - void removeSegment( - MotionModelHistory::iterator & iter, - Transaction & transaction); + void removeSegment(MotionModelHistory::iterator& iter, Transaction& transaction); /** * @brief Split an existing MotionModelSegment into two pieces at the provided timestamp, updating @@ -308,10 +287,7 @@ class TimestampManager * @param[out] transaction A transaction object to be updated with the changes caused by * splitSegment */ - void splitSegment( - MotionModelHistory::iterator & iter, - const rclcpp::Time & stamp, - Transaction & transaction); + void splitSegment(MotionModelHistory::iterator& iter, const rclcpp::Time& stamp, Transaction& transaction); /** * @brief Remove any motion model segments that are older than \p buffer_length_ @@ -319,41 +295,25 @@ class TimestampManager void purgeHistory(); }; -template -TimestampManager::TimestampManager( - void(T::* fp)( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables), - T * obj, - const rclcpp::Duration & buffer_length) -: TimestampManager(std::bind(fp, - obj, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4), - buffer_length) +template +TimestampManager::TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables), + T* obj, const rclcpp::Duration& buffer_length) + : TimestampManager(std::bind(fp, obj, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4), + buffer_length) { } -template -TimestampManager::TimestampManager( - void(T::* fp)( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables) const, - T * obj, - const rclcpp::Duration & buffer_length) -: TimestampManager(std::bind(fp, - obj, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4), - buffer_length) +template +TimestampManager::TimestampManager(void (T::*fp)(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables) const, + T* obj, const rclcpp::Duration& buffer_length) + : TimestampManager(std::bind(fp, obj, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4), + buffer_length) { } diff --git a/fuse_core/include/fuse_core/transaction.hpp b/fuse_core/include/fuse_core/transaction.hpp index 514536f40..4449c439d 100644 --- a/fuse_core/include/fuse_core/transaction.hpp +++ b/fuse_core/include/fuse_core/transaction.hpp @@ -111,19 +111,28 @@ class Transaction /** * @brief Read-only access to this transaction's timestamp */ - const rclcpp::Time & stamp() const {return stamp_;} + const rclcpp::Time& stamp() const + { + return stamp_; + } /** * @brief Write access to this transaction's timestamp */ - void stamp(const rclcpp::Time & stamp) {stamp_ = stamp;} + void stamp(const rclcpp::Time& stamp) + { + stamp_ = stamp; + } /** * @brief Read-only access to the set of timestamps involved in this transaction * * @return An iterator range containing all involved timestamps, ordered oldest to newest */ - const_stamp_range involvedStamps() const {return involved_stamps_;} + const_stamp_range involvedStamps() const + { + return involved_stamps_; + } /** * @brief Read-only access to the minimum (oldest), timestamp among the transaction's stamp and @@ -131,7 +140,7 @@ class Transaction * * @return The minimum (oldest) timestamp. */ - const rclcpp::Time & minStamp() const; + const rclcpp::Time& minStamp() const; /** * @brief Read-only access to the maximum (newest) timestamp among the transaction's stamp and all @@ -139,7 +148,7 @@ class Transaction * * @return The maximum (newest) timestamp. */ - const rclcpp::Time & maxStamp() const; + const rclcpp::Time& maxStamp() const; /** * @brief Read-only access to the added constraints @@ -153,7 +162,10 @@ class Transaction * * @return An iterator range containing all removed constraint UUIDs */ - const_uuid_range removedConstraints() const {return removed_constraints_;} + const_uuid_range removedConstraints() const + { + return removed_constraints_; + } /** * @brief Read-only access to the added variables @@ -167,7 +179,10 @@ class Transaction * * @return An iterator range containing all removed variable UUIDs */ - const_uuid_range removedVariables() const {return removed_variables_;} + const_uuid_range removedVariables() const + { + return removed_variables_; + } /** * @brief Check if the transaction is empty, i.e. it has no added or removed constraints or @@ -185,7 +200,7 @@ class Transaction * * @param[in] stamp The timestamp to be added */ - void addInvolvedStamp(const rclcpp::Time & stamp); + void addInvolvedStamp(const rclcpp::Time& stamp); /** * @brief Add a constraint to this transaction @@ -210,7 +225,7 @@ class Transaction * * @param[in] constraint_uuid The UUID of the constraint to remove */ - void removeConstraint(const UUID & constraint_uuid); + void removeConstraint(const UUID& constraint_uuid); /** * @brief Add a variable to this transaction @@ -235,7 +250,7 @@ class Transaction * * @param[in] variable_uuid The UUID of the variable to remove */ - void removeVariable(const UUID & variable_uuid); + void removeVariable(const UUID& variable_uuid); /** * @brief Merge the contents of another transaction into this one. @@ -244,14 +259,14 @@ class Transaction * @param[in] overwrite Flag indicating that variables and constraints in \p other should * overwrite existing variables and constraints with the UUIDs. */ - void merge(const Transaction & other, bool overwrite = false); + void merge(const Transaction& other, bool overwrite = false); /** * @brief Print a human-readable description of the transaction to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const; + void print(std::ostream& stream = std::cout) const; /** * @brief Perform a deep copy of the Transaction and return a unique pointer to the copy @@ -267,36 +282,36 @@ class Transaction * * @param[out] archive - The archive to serialize this constraint into */ - void serialize(fuse_core::BinaryOutputArchive & /* archive */) const; + void serialize(fuse_core::BinaryOutputArchive& /* archive */) const; /** * @brief Serialize this Constraint into the provided text archive * * @param[out] archive - The archive to serialize this constraint into */ - void serialize(fuse_core::TextOutputArchive & /* archive */) const; + void serialize(fuse_core::TextOutputArchive& /* archive */) const; /** * @brief Deserialize data from the provided binary archive into this Constraint * * @param[in] archive - The archive holding serialized Constraint data */ - void deserialize(fuse_core::BinaryInputArchive & /* archive */); + void deserialize(fuse_core::BinaryInputArchive& /* archive */); /** * @brief Deserialize data from the provided text archive into this Constraint * * @param[in] archive - The archive holding serialized Constraint data */ - void deserialize(fuse_core::TextInputArchive & /* archive */); + void deserialize(fuse_core::TextInputArchive& /* archive */); private: - rclcpp::Time stamp_{0, 0, RCL_ROS_TIME}; //!< The transaction message timestamp + rclcpp::Time stamp_{ 0, 0, RCL_ROS_TIME }; //!< The transaction message timestamp std::vector added_constraints_; //!< The constraints to be added - std::vector added_variables_; //!< The variables to be added - std::set involved_stamps_; //!< The set of timestamps involved in this transaction - std::vector removed_constraints_; //!< The constraint UUIDs to be removed - std::vector removed_variables_; //!< The variable UUIDs to be removed + std::vector added_variables_; //!< The variables to be added + std::set involved_stamps_; //!< The set of timestamps involved in this transaction + std::vector removed_constraints_; //!< The constraint UUIDs to be removed + std::vector removed_variables_; //!< The variable UUIDs to be removed // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -308,22 +323,22 @@ class Transaction * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & stamp_; - archive & added_constraints_; - archive & added_variables_; - archive & involved_stamps_; - archive & removed_constraints_; - archive & removed_variables_; + archive& stamp_; + archive& added_constraints_; + archive& added_variables_; + archive& involved_stamps_; + archive& removed_constraints_; + archive& removed_variables_; } }; /** * Stream operator for printing Transaction objects. */ -std::ostream & operator<<(std::ostream & stream, const Transaction & transaction); +std::ostream& operator<<(std::ostream& stream, const Transaction& transaction); } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/transaction_deserializer.hpp b/fuse_core/include/fuse_core/transaction_deserializer.hpp index 634129b21..7d5f44675 100644 --- a/fuse_core/include/fuse_core/transaction_deserializer.hpp +++ b/fuse_core/include/fuse_core/transaction_deserializer.hpp @@ -46,9 +46,7 @@ namespace fuse_core /** * @brief Serialize a transaction into a message */ -void serializeTransaction( - const fuse_core::Transaction & transaction, - fuse_msgs::msg::SerializedTransaction & msg); +void serializeTransaction(const fuse_core::Transaction& transaction, fuse_msgs::msg::SerializedTransaction& msg); /** * @brief Deserialize a Transaction @@ -75,8 +73,8 @@ class TransactionDeserializer * @param[IN] msg The SerializedTransaction message to be deserialized * @return A fuse Transaction object */ - inline fuse_core::Transaction::UniquePtr deserialize( - const fuse_msgs::msg::SerializedTransaction::ConstSharedPtr msg) const + inline fuse_core::Transaction::UniquePtr + deserialize(const fuse_msgs::msg::SerializedTransaction::ConstSharedPtr msg) const { return deserialize(*msg); } @@ -90,8 +88,7 @@ class TransactionDeserializer * @param[IN] msg The SerializedTransaction message to be deserialized * @return A fuse Transaction object */ - fuse_core::Transaction::UniquePtr deserialize( - const fuse_msgs::msg::SerializedTransaction & msg) const; + fuse_core::Transaction::UniquePtr deserialize(const fuse_msgs::msg::SerializedTransaction& msg) const; private: //! Pluginlib class loader for Variable types diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index b3bdc4a55..444f7e574 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -54,15 +54,18 @@ namespace fuse_core * @param[in] z The quaternion x-axis component * @return The quaternion's Euler pitch angle component */ -template +template static inline T getPitch(const T w, const T x, const T y, const T z) { // Adapted from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles const T sin_pitch = T(2.0) * (w * y - z * x); - if (ceres::abs(sin_pitch) >= T(1.0)) { + if (ceres::abs(sin_pitch) >= T(1.0)) + { return (sin_pitch >= T(0.0) ? T(1.0) : T(-1.0)) * T(M_PI / 2.0); - } else { + } + else + { return ceres::asin(sin_pitch); } } @@ -76,7 +79,7 @@ static inline T getPitch(const T w, const T x, const T y, const T z) * @param[in] z The quaternion x-axis component * @return The quaternion's Euler roll angle component */ -template +template static inline T getRoll(const T w, const T x, const T y, const T z) { // Adapted from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles @@ -94,7 +97,7 @@ static inline T getRoll(const T w, const T x, const T y, const T z) * @param[in] z The quaternion x-axis component * @return The quaternion's Euler yaw angle component */ -template +template static inline T getYaw(const T w, const T x, const T y, const T z) { // Adapted from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles @@ -109,8 +112,8 @@ static inline T getYaw(const T w, const T x, const T y, const T z) * @param[in/out] angle Input angle to be wrapped to the [-Pi, +Pi) range. Angle is updated by this * function. */ -template -void wrapAngle2D(T & angle) +template +void wrapAngle2D(T& angle) { // Define some necessary variations of PI with the correct type (double or Jet) static const T PI = T(M_PI); @@ -126,8 +129,8 @@ void wrapAngle2D(T & angle) * @param[in] angle Input angle to be wrapped to the (-Pi, +Pi] range. * @return The equivalent wrapped angle */ -template -T wrapAngle2D(const T & angle) +template +T wrapAngle2D(const T& angle) { T wrapped = angle; wrapAngle2D(wrapped); @@ -140,7 +143,7 @@ T wrapAngle2D(const T & angle) * @param[in] angle The rotation angle, in radians * @return The equivalent 2x2 rotation matrix */ -template +template Eigen::Matrix rotationMatrix2D(const T angle) { const T cos_angle = ceres::cos(angle); @@ -157,22 +160,24 @@ Eigen::Matrix rotationMatrix2D(const T angle) * @param[in] b The second element * @return The joined topic name */ -inline -std::string joinTopicName(std::string a, std::string b) +inline std::string joinTopicName(std::string a, std::string b) { - if (a.empty()) { + if (a.empty()) + { return b; } - if (b.empty()) { + if (b.empty()) + { return a; } - if (b.front() == '/' || b.front() == '~') { - RCLCPP_WARN( - rclcpp::get_logger("fuse"), "Second argument to joinTopicName is absolute! Returning it."); + if (b.front() == '/' || b.front() == '~') + { + RCLCPP_WARN(rclcpp::get_logger("fuse"), "Second argument to joinTopicName is absolute! Returning it."); return b; } - if (a.back() == '/') { + if (a.back() == '/') + { a.pop_back(); } diff --git a/fuse_core/include/fuse_core/uuid.hpp b/fuse_core/include/fuse_core/uuid.hpp index 35aa98575..fd9833577 100644 --- a/fuse_core/include/fuse_core/uuid.hpp +++ b/fuse_core/include/fuse_core/uuid.hpp @@ -52,131 +52,131 @@ namespace uuid { using boost::uuids::to_string; using hash = boost::hash; -constexpr UUID NIL = {{0}}; +constexpr UUID NIL = { { 0 } }; /** - * @brief Convert a string representation of the UUID into a UUID variable - * - * There are a few supported formats: - * - "01234567-89AB-CDEF-0123-456789ABCDEF" - * - "01234567-89ab-cdef-0123-456789abcdef" - * - "0123456789abcdef0123456789abcdef" - * - "{01234567-89ab-cdef-0123-456789abcdef}" - */ -inline UUID from_string(const std::string & uuid_string) + * @brief Convert a string representation of the UUID into a UUID variable + * + * There are a few supported formats: + * - "01234567-89AB-CDEF-0123-456789ABCDEF" + * - "01234567-89ab-cdef-0123-456789abcdef" + * - "0123456789abcdef0123456789abcdef" + * - "{01234567-89ab-cdef-0123-456789abcdef}" + */ +inline UUID from_string(const std::string& uuid_string) { return boost::uuids::string_generator()(uuid_string); } /** - * @brief Generate a random UUID - */ + * @brief Generate a random UUID + */ UUID generate(); /** - * @brief Generate a UUID from a raw data buffer - * - * @param[in] data A data buffer containing information that makes this item unique - * @param[in] byte_count The number of bytes in the data buffer - * @return A repeatable UUID specific to the provided data - */ -inline UUID generate(const void * data, size_t byte_count) + * @brief Generate a UUID from a raw data buffer + * + * @param[in] data A data buffer containing information that makes this item unique + * @param[in] byte_count The number of bytes in the data buffer + * @return A repeatable UUID specific to the provided data + */ +inline UUID generate(const void* data, size_t byte_count) { return boost::uuids::name_generator(NIL)(data, byte_count); } /** - * @brief Generate a UUID from a C-style string - * - * @param[in] data A data buffer held in a C-style string - * @return A repeatable UUID specific to the provided data - */ -inline UUID generate(const char * data) + * @brief Generate a UUID from a C-style string + * + * @param[in] data A data buffer held in a C-style string + * @return A repeatable UUID specific to the provided data + */ +inline UUID generate(const char* data) { return generate(data, std::strlen(data)); } /** - * @brief Generate a UUID from a C++ string - * - * @param[in] data A data buffer held in a C++-style string - * @return A repeatable UUID specific to the provided namespace and data - */ -inline UUID generate(const std::string & data) + * @brief Generate a UUID from a C++ string + * + * @param[in] data A data buffer held in a C++-style string + * @return A repeatable UUID specific to the provided namespace and data + */ +inline UUID generate(const std::string& data) { return generate(data.c_str(), data.length()); } /** - * @brief Generate a UUID from a namespace string and a raw data buffer - * - * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs - * @param[in] data A data buffer containing information that makes this item unique - * @param[in] byte_count The number of bytes in the data buffer - * @return A repeatable UUID specific to the provided namespace and data - */ -inline UUID generate(const std::string & namespace_string, const void * data, size_t byte_count) + * @brief Generate a UUID from a namespace string and a raw data buffer + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] data A data buffer containing information that makes this item unique + * @param[in] byte_count The number of bytes in the data buffer + * @return A repeatable UUID specific to the provided namespace and data + */ +inline UUID generate(const std::string& namespace_string, const void* data, size_t byte_count) { return boost::uuids::name_generator(generate(namespace_string))(data, byte_count); } /** - * @brief Generate a UUID from a namespace string and C-style string - * - * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs - * @param[in] data A data buffer held in a C-style string - * @return A repeatable UUID specific to the provided namespace and data - */ -inline UUID generate(const std::string & namespace_string, const char * data) + * @brief Generate a UUID from a namespace string and C-style string + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] data A data buffer held in a C-style string + * @return A repeatable UUID specific to the provided namespace and data + */ +inline UUID generate(const std::string& namespace_string, const char* data) { return generate(namespace_string, data, std::strlen(data)); } /** - * @brief Generate a UUID from a namespace string and a C++ string - * - * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs - * @param[in] data A data buffer held in a C++-style string - * @return A repeatable UUID specific to the provided namespace and data - */ -inline UUID generate(const std::string & namespace_string, const std::string & data) + * @brief Generate a UUID from a namespace string and a C++ string + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] data A data buffer held in a C++-style string + * @return A repeatable UUID specific to the provided namespace and data + */ +inline UUID generate(const std::string& namespace_string, const std::string& data) { return generate(namespace_string, data.c_str(), data.length()); } /** - * @brief Generate a UUID from a namespace string and a ros timestamp - * - * Every unique timestamp will generate a unique UUID - * - * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs - * @param[in] stamp An rclcpp::Time timestamp - * @return A repeatable UUID specific to the provided namespace and timestamp - */ -UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp); + * @brief Generate a UUID from a namespace string and a ros timestamp + * + * Every unique timestamp will generate a unique UUID + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] stamp An rclcpp::Time timestamp + * @return A repeatable UUID specific to the provided namespace and timestamp + */ +UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp); /** - * @brief Generate a UUID from a namespace string, a ros timestamp, and an additional id - * - * Every unique timestamp and id pair will generate a unique UUID - * - * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs - * @param[in] stamp A rclcpp::Time timestamp - * @param[in] id A UUID - * @return A repeatable UUID specific to the provided namespace and timestamp - */ -UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp, const UUID & id); + * @brief Generate a UUID from a namespace string, a ros timestamp, and an additional id + * + * Every unique timestamp and id pair will generate a unique UUID + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] stamp A rclcpp::Time timestamp + * @param[in] id A UUID + * @return A repeatable UUID specific to the provided namespace and timestamp + */ +UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp, const UUID& id); /** - * @brief Generate a UUID from a namespace string and a user provided id - * - * Every unique user id will generate a unique UUID - * - * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs - * @param[in] user_id A uint64_t user generated id - * @return A repeatable UUID specific to the provided namespace and user id - */ -UUID generate(const std::string & namespace_string, const uint64_t & user_id); + * @brief Generate a UUID from a namespace string and a user provided id + * + * Every unique user id will generate a unique UUID + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] user_id A uint64_t user generated id + * @return A repeatable UUID specific to the provided namespace and user id + */ +UUID generate(const std::string& namespace_string, const uint64_t& user_id); } // namespace uuid } // namespace fuse_core @@ -188,10 +188,10 @@ namespace std * @brief Define a hash specialization for the UUID to make it easier to use in unordered_maps and * unordered_sets */ -template<> +template <> struct hash { - size_t operator()(const fuse_core::UUID & id) const + size_t operator()(const fuse_core::UUID& id) const { return boost::uuids::hash_value(id); } diff --git a/fuse_core/include/fuse_core/variable.hpp b/fuse_core/include/fuse_core/variable.hpp index 91570d966..550021977 100644 --- a/fuse_core/include/fuse_core/variable.hpp +++ b/fuse_core/include/fuse_core/variable.hpp @@ -61,10 +61,10 @@ * } * @endcode */ -#define FUSE_VARIABLE_CLONE_DEFINITION(...) \ - fuse_core::Variable::UniquePtr clone() const override \ - { \ - return __VA_ARGS__::make_unique(*this); \ +#define FUSE_VARIABLE_CLONE_DEFINITION(...) \ + fuse_core::Variable::UniquePtr clone() const override \ + { \ + return __VA_ARGS__::make_unique(*this); \ } /** @@ -80,22 +80,22 @@ * } * @endcode */ -#define FUSE_VARIABLE_SERIALIZE_DEFINITION(...) \ - void serialize(fuse_core::BinaryOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void serialize(fuse_core::TextOutputArchive & archive) const override \ - { \ - archive << *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::BinaryInputArchive & archive) override \ - { \ - archive >> *this; \ - } /* NOLINT */ \ - void deserialize(fuse_core::TextInputArchive & archive) override \ - { \ - archive >> *this; \ +#define FUSE_VARIABLE_SERIALIZE_DEFINITION(...) \ + void serialize(fuse_core::BinaryOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void serialize(fuse_core::TextOutputArchive& archive) const override \ + { \ + archive << *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::BinaryInputArchive& archive) override \ + { \ + archive >> *this; \ + } /* NOLINT */ \ + void deserialize(fuse_core::TextInputArchive& archive) override \ + { \ + archive >> *this; \ } /** @@ -113,17 +113,17 @@ * } * @endcode */ -#define FUSE_VARIABLE_TYPE_DEFINITION(...) \ - struct detail \ - { \ - static std::string type() \ - { \ - return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ - std::string type() const override \ - { \ - return detail::type(); \ +#define FUSE_VARIABLE_TYPE_DEFINITION(...) \ + struct detail \ + { \ + static std::string type() \ + { \ + return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ + } /* NOLINT */ \ + }; /* NOLINT */ \ + std::string type() const override \ + { \ + return detail::type(); \ } /** @@ -140,10 +140,10 @@ * } * @endcode */ -#define FUSE_VARIABLE_DEFINITIONS(...) \ - FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ - FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ +#define FUSE_VARIABLE_DEFINITIONS(...) \ + FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \ + FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__) /** @@ -160,13 +160,12 @@ * } * @endcode */ -#define FUSE_VARIABLE_DEFINITIONS_WITH_EIGEN(...) \ - FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ - FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ - FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ +#define FUSE_VARIABLE_DEFINITIONS_WITH_EIGEN(...) \ + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \ + FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \ + FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \ FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__) - namespace fuse_core { @@ -219,7 +218,7 @@ class Variable * * @param[in] uuid The unique ID number for this variable */ - explicit Variable(const UUID & uuid); + explicit Variable(const UUID& uuid); /** * @brief Destructor @@ -229,7 +228,10 @@ class Variable /** * @brief Returns a UUID for this variable. */ - const UUID & uuid() const {return uuid_;} + const UUID& uuid() const + { + return uuid_; + } /** * @brief Returns a unique name for this variable type. @@ -264,7 +266,10 @@ 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 {return size();} + virtual size_t localSize() const + { + return size(); + } /** * @brief Read-only access to the variable data @@ -274,7 +279,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; + virtual const double* data() const = 0; /** * @brief Read-write access to the variable data @@ -284,14 +289,14 @@ class Variable * Variable::size() elements will be accessed externally. This interface is provided for * integration with Ceres, which uses raw pointers. */ - virtual double * data() = 0; + virtual double* data() = 0; /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - virtual void print(std::ostream & stream = std::cout) const = 0; + virtual void print(std::ostream& stream = std::cout) const = 0; /** * @brief Perform a deep copy of the Variable and return a unique pointer to the copy @@ -326,7 +331,7 @@ class Variable * * @return A base pointer to an instance of a derived LocalParameterization */ - virtual fuse_core::LocalParameterization * localParameterization() const + virtual fuse_core::LocalParameterization* localParameterization() const { return nullptr; } @@ -349,16 +354,19 @@ class Variable * * @return A base pointer to an instance of a derived Manifold */ - virtual fuse_core::Manifold * manifold() const + 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) { + if (!local_parameterization) + { return nullptr; - } else { + } + else + { return new fuse_core::ManifoldAdapter(local_parameterization); } } @@ -408,7 +416,7 @@ class Variable * * @param[out] archive - The archive to serialize this variable into */ - virtual void serialize(fuse_core::BinaryOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::BinaryOutputArchive& /* archive */) const = 0; /** * @brief Serialize this Variable into the provided text archive @@ -420,7 +428,7 @@ class Variable * * @param[out] archive - The archive to serialize this variable into */ - virtual void serialize(fuse_core::TextOutputArchive & /* archive */) const = 0; + virtual void serialize(fuse_core::TextOutputArchive& /* archive */) const = 0; /** * @brief Deserialize data from the provided binary archive into this Variable @@ -432,7 +440,7 @@ class Variable * * @param[in] archive - The archive holding serialized Variable data */ - virtual void deserialize(fuse_core::BinaryInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::BinaryInputArchive& /* archive */) = 0; /** * @brief Deserialize data from the provided text archive into this Variable @@ -444,7 +452,7 @@ class Variable * * @param[in] archive - The archive holding serialized Variable data */ - virtual void deserialize(fuse_core::TextInputArchive & /* archive */) = 0; + virtual void deserialize(fuse_core::TextInputArchive& /* archive */) = 0; private: fuse_core::UUID uuid_; //!< The unique ID number for this variable @@ -464,17 +472,17 @@ class Variable * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & uuid_; + archive& uuid_; } }; /** * Stream operator implementation used for all derived Variable classes. */ -std::ostream & operator<<(std::ostream & stream, const Variable & variable); +std::ostream& operator<<(std::ostream& stream, const Variable& variable); } // namespace fuse_core diff --git a/fuse_core/package.xml b/fuse_core/package.xml index 34301913a..a66be7175 100644 --- a/fuse_core/package.xml +++ b/fuse_core/package.xml @@ -33,8 +33,6 @@ ament_cmake_gtest ament_cmake_pytest - ament_lint_auto - ament_lint_common geometry_msgs launch launch_pytest diff --git a/fuse_core/src/async_motion_model.cpp b/fuse_core/src/async_motion_model.cpp index c2be0ad85..b3fd5da11 100644 --- a/fuse_core/src/async_motion_model.cpp +++ b/fuse_core/src/async_motion_model.cpp @@ -46,9 +46,7 @@ namespace fuse_core { -AsyncMotionModel::AsyncMotionModel(size_t thread_count) -: name_("uninitialized"), - executor_thread_count_(thread_count) +AsyncMotionModel::AsyncMotionModel(size_t thread_count) : name_("uninitialized"), executor_thread_count_(thread_count) { } @@ -57,7 +55,7 @@ AsyncMotionModel::~AsyncMotionModel() internal_stop(); } -bool AsyncMotionModel::apply(Transaction & transaction) +bool AsyncMotionModel::apply(Transaction& transaction) { // Insert a call to the motion model's queryCallback() function into the motion model's callback // queue. While this makes this particular function more difficult to write, it does simplify the @@ -66,8 +64,8 @@ bool AsyncMotionModel::apply(Transaction & transaction) // service callback, and should be a familiar pattern for ROS developers. This function blocks // until the queryCallback() call completes, thus enforcing that motion models are generated in // order. - auto callback = std::make_shared>( - std::bind(&AsyncMotionModel::applyCallback, this, std::ref(transaction))); + auto callback = + std::make_shared>(std::bind(&AsyncMotionModel::applyCallback, this, std::ref(transaction))); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -75,11 +73,11 @@ bool AsyncMotionModel::apply(Transaction & transaction) return result.get(); } -void AsyncMotionModel::initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void AsyncMotionModel::initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name) { - if (initialized_) { + if (initialized_) + { throw std::runtime_error("Calling initialize on an already initialized AsyncMotionModel!"); } @@ -91,18 +89,19 @@ void AsyncMotionModel::initialize( auto executor_options = rclcpp::ExecutorOptions(); executor_options.context = context; - if (executor_thread_count_ == 1) { + if (executor_thread_count_ == 1) + { executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(executor_options); - } else { - executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared( - executor_options, executor_thread_count_); + } + else + { + executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(executor_options, executor_thread_count_); } callback_queue_ = std::make_shared(context); // This callback group MUST be re-entrant in order to support parallelization - cb_group_ = interfaces_.get_node_base_interface()->create_callback_group( - rclcpp::CallbackGroupType::Reentrant, false); + cb_group_ = interfaces_.get_node_base_interface()->create_callback_group(rclcpp::CallbackGroupType::Reentrant, false); interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, cb_group_); // Call the derived onInit() function to perform implementation-specific initialization @@ -114,19 +113,13 @@ void AsyncMotionModel::initialize( executor_->add_callback_group(cb_group_, interfaces_.get_node_base_interface()); // Start the executor - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); // Wait for the executor to start spinning. // This avoids a race where the destructor blocks waiting for the spinner_ // thread to be joined when the class is destroyed before the thread is ever // scheduled. - auto callback = std::make_shared>( - [&]() { - initialized_ = true; - }); + auto callback = std::make_shared>([&]() { initialized_ = true; }); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -134,17 +127,14 @@ void AsyncMotionModel::initialize( void AsyncMotionModel::graphCallback(Graph::ConstSharedPtr graph) { - auto callback = std::make_shared>( - std::bind(&AsyncMotionModel::onGraphUpdate, this, std::move(graph)) - ); + auto callback = + std::make_shared>(std::bind(&AsyncMotionModel::onGraphUpdate, this, std::move(graph))); callback_queue_->addCallback(callback); } void AsyncMotionModel::start() { - auto callback = std::make_shared>( - std::bind(&AsyncMotionModel::onStart, this) - ); + auto callback = std::make_shared>(std::bind(&AsyncMotionModel::onStart, this)); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -154,14 +144,15 @@ void AsyncMotionModel::stop() { // Prefer to call onStop in executor's thread so downstream users don't have // to worry about threads in ROS callbacks when there's only 1 thread. - if (interfaces_.get_node_base_interface()->get_context()->is_valid()) { - auto callback = std::make_shared>( - std::bind(&AsyncMotionModel::onStop, this) - ); + if (interfaces_.get_node_base_interface()->get_context()->is_valid()) + { + auto callback = std::make_shared>(std::bind(&AsyncMotionModel::onStop, this)); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); - } else { + } + else + { // 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. @@ -172,7 +163,8 @@ void AsyncMotionModel::stop() void AsyncMotionModel::internal_stop() { - if (spinner_.joinable()) { + if (spinner_.joinable()) + { executor_->cancel(); spinner_.join(); } diff --git a/fuse_core/src/async_publisher.cpp b/fuse_core/src/async_publisher.cpp index 3caeadb53..cf174247a 100644 --- a/fuse_core/src/async_publisher.cpp +++ b/fuse_core/src/async_publisher.cpp @@ -37,9 +37,7 @@ namespace fuse_core { -AsyncPublisher::AsyncPublisher(size_t thread_count) -: name_("uninitialized"), - executor_thread_count_(thread_count) +AsyncPublisher::AsyncPublisher(size_t thread_count) : name_("uninitialized"), executor_thread_count_(thread_count) { } @@ -48,11 +46,11 @@ AsyncPublisher::~AsyncPublisher() internal_stop(); } -void AsyncPublisher::initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void AsyncPublisher::initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name) { - if (initialized_) { + if (initialized_) + { throw std::runtime_error("Calling initialize on an already initialized AsyncPublisher!"); } @@ -64,18 +62,19 @@ void AsyncPublisher::initialize( auto executor_options = rclcpp::ExecutorOptions(); executor_options.context = context; - if (executor_thread_count_ == 1) { + if (executor_thread_count_ == 1) + { executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(executor_options); - } else { - executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared( - executor_options, executor_thread_count_); + } + else + { + executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(executor_options, executor_thread_count_); } callback_queue_ = std::make_shared(context); // This callback group MUST be re-entrant in order to support parallelization - cb_group_ = interfaces_.get_node_base_interface()->create_callback_group( - rclcpp::CallbackGroupType::Reentrant, false); + cb_group_ = interfaces_.get_node_base_interface()->create_callback_group(rclcpp::CallbackGroupType::Reentrant, false); interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, cb_group_); // Call the derived onInit() function to perform implementation-specific initialization @@ -87,19 +86,13 @@ void AsyncPublisher::initialize( executor_->add_callback_group(cb_group_, interfaces_.get_node_base_interface()); // Start the executor - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); // Wait for the executor to start spinning. // This avoids a race where the destructor blocks waiting for the spinner_ // thread to be joined when the class is destroyed before the thread is ever // scheduled. - auto callback = std::make_shared>( - [&]() { - initialized_ = true; - }); + auto callback = std::make_shared>([&]() { initialized_ = true; }); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -110,14 +103,13 @@ void AsyncPublisher::notify(Transaction::ConstSharedPtr transaction, Graph::Cons // Insert a call to the `notifyCallback` method into the internal callback queue. // This minimizes the time spent by the optimizer's thread calling this function. auto callback = std::make_shared>( - std::bind(&AsyncPublisher::notifyCallback, this, std::move(transaction), std::move(graph))); + std::bind(&AsyncPublisher::notifyCallback, this, std::move(transaction), std::move(graph))); callback_queue_->addCallback(callback); } void AsyncPublisher::start() { - auto callback = - std::make_shared>(std::bind(&AsyncPublisher::onStart, this)); + auto callback = std::make_shared>(std::bind(&AsyncPublisher::onStart, this)); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -127,14 +119,15 @@ void AsyncPublisher::stop() { // Prefer to call onStop in executor's thread so downstream users don't have // to worry about threads in ROS callbacks when there's only 1 thread. - if (interfaces_.get_node_base_interface()->get_context()->is_valid()) { - auto callback = std::make_shared>( - std::bind(&AsyncPublisher::onStop, this) - ); + if (interfaces_.get_node_base_interface()->get_context()->is_valid()) + { + auto callback = std::make_shared>(std::bind(&AsyncPublisher::onStop, this)); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); - } else { + } + else + { // 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. @@ -145,7 +138,8 @@ void AsyncPublisher::stop() void AsyncPublisher::internal_stop() { - if (spinner_.joinable()) { + if (spinner_.joinable()) + { executor_->cancel(); spinner_.join(); } diff --git a/fuse_core/src/async_sensor_model.cpp b/fuse_core/src/async_sensor_model.cpp index 80f926448..9f9903737 100644 --- a/fuse_core/src/async_sensor_model.cpp +++ b/fuse_core/src/async_sensor_model.cpp @@ -45,9 +45,7 @@ namespace fuse_core { -AsyncSensorModel::AsyncSensorModel(size_t thread_count) -: name_("uninitialized"), - executor_thread_count_(thread_count) +AsyncSensorModel::AsyncSensorModel(size_t thread_count) : name_("uninitialized"), executor_thread_count_(thread_count) { } @@ -56,12 +54,11 @@ AsyncSensorModel::~AsyncSensorModel() internal_stop(); } -void AsyncSensorModel::initialize( - node_interfaces::NodeInterfaces interfaces, - const std::string & name, - TransactionCallback transaction_callback) +void AsyncSensorModel::initialize(node_interfaces::NodeInterfaces interfaces, + const std::string& name, TransactionCallback transaction_callback) { - if (initialized_) { + if (initialized_) + { throw std::runtime_error("Calling initialize on an already initialized AsyncSensorModel!"); } @@ -73,18 +70,19 @@ void AsyncSensorModel::initialize( auto executor_options = rclcpp::ExecutorOptions(); executor_options.context = context; - if (executor_thread_count_ == 1) { + if (executor_thread_count_ == 1) + { executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(executor_options); - } else { - executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared( - executor_options, executor_thread_count_); + } + else + { + executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared(executor_options, executor_thread_count_); } callback_queue_ = std::make_shared(context); // This callback group MUST be re-entrant in order to support parallelization - cb_group_ = interfaces_.get_node_base_interface()->create_callback_group( - rclcpp::CallbackGroupType::Reentrant, false); + cb_group_ = interfaces_.get_node_base_interface()->create_callback_group(rclcpp::CallbackGroupType::Reentrant, false); interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, cb_group_); transaction_callback_ = transaction_callback; @@ -98,19 +96,13 @@ void AsyncSensorModel::initialize( executor_->add_callback_group(cb_group_, interfaces_.get_node_base_interface()); // Start the executor - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); // Wait for the executor to start spinning. // This avoids a race where the destructor blocks waiting for the spinner_ // thread to be joined when the class is destroyed before the thread is ever // scheduled. - auto callback = std::make_shared>( - [&]() { - initialized_ = true; - }); + auto callback = std::make_shared>([&]() { initialized_ = true; }); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -118,9 +110,8 @@ void AsyncSensorModel::initialize( void AsyncSensorModel::graphCallback(Graph::ConstSharedPtr graph) { - auto callback = std::make_shared>( - std::bind(&AsyncSensorModel::onGraphUpdate, this, std::move(graph)) - ); + auto callback = + std::make_shared>(std::bind(&AsyncSensorModel::onGraphUpdate, this, std::move(graph))); callback_queue_->addCallback(callback); } @@ -131,9 +122,7 @@ void AsyncSensorModel::sendTransaction(Transaction::SharedPtr transaction) void AsyncSensorModel::start() { - auto callback = std::make_shared>( - std::bind(&AsyncSensorModel::onStart, this) - ); + auto callback = std::make_shared>(std::bind(&AsyncSensorModel::onStart, this)); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); @@ -143,14 +132,15 @@ void AsyncSensorModel::stop() { // Prefer to call onStop in executor's thread so downstream users don't have // to worry about threads in ROS callbacks when there's only 1 thread. - if (interfaces_.get_node_base_interface()->get_context()->is_valid()) { - auto callback = std::make_shared>( - std::bind(&AsyncSensorModel::onStop, this) - ); + if (interfaces_.get_node_base_interface()->get_context()->is_valid()) + { + auto callback = std::make_shared>(std::bind(&AsyncSensorModel::onStop, this)); auto result = callback->getFuture(); callback_queue_->addCallback(callback); result.wait(); - } else { + } + else + { // 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. @@ -161,7 +151,8 @@ void AsyncSensorModel::stop() void AsyncSensorModel::internal_stop() { - if (spinner_.joinable()) { + if (spinner_.joinable()) + { executor_->cancel(); spinner_.join(); } diff --git a/fuse_core/src/callback_wrapper.cpp b/fuse_core/src/callback_wrapper.cpp index 454cf6302..84dc2fba3 100644 --- a/fuse_core/src/callback_wrapper.cpp +++ b/fuse_core/src/callback_wrapper.cpp @@ -39,29 +39,30 @@ namespace fuse_core CallbackAdapter::CallbackAdapter(std::shared_ptr context_ptr) { - rcl_guard_condition_options_t guard_condition_options = - rcl_guard_condition_get_default_options(); + rcl_guard_condition_options_t guard_condition_options = rcl_guard_condition_get_default_options(); // Guard condition is used by the wait set to handle execute-or-not logic gc_ = rcl_get_zero_initialized_guard_condition(); - if (RCL_RET_OK != rcl_guard_condition_init( - &gc_, context_ptr->get_rcl_context().get(), guard_condition_options)) + if (RCL_RET_OK != rcl_guard_condition_init(&gc_, context_ptr->get_rcl_context().get(), guard_condition_options)) { throw std::runtime_error("Could not init guard condition for callback waitable."); } } /** - * @brief tell the CallbackGroup how many guard conditions are ready in this waitable - */ -size_t CallbackAdapter::get_number_of_ready_guard_conditions() {return 1;} + * @brief tell the CallbackGroup how many guard conditions are ready in this waitable + */ +size_t CallbackAdapter::get_number_of_ready_guard_conditions() +{ + return 1; +} /** - * @brief tell the CallbackGroup that this waitable is ready to execute anything - */ -bool CallbackAdapter::is_ready(rcl_wait_set_t const & wait_set) + * @brief tell the CallbackGroup that this waitable is ready to execute anything + */ +bool CallbackAdapter::is_ready(rcl_wait_set_t const& wait_set) { - (void) wait_set; + (void)wait_set; return !callback_queue_.empty(); } @@ -71,32 +72,35 @@ bool CallbackAdapter::is_ready(rcl_wait_set_t const & wait_set) waitable_ptr = std::make_shared(); node->get_node_waitables_interface()->add_waitable(waitable_ptr, (rclcpp::CallbackGroup::SharedPtr) nullptr); */ -void CallbackAdapter::add_to_wait_set(rcl_wait_set_t & wait_set) +void CallbackAdapter::add_to_wait_set(rcl_wait_set_t& wait_set) { - if (RCL_RET_OK != rcl_wait_set_add_guard_condition(&wait_set, &gc_, NULL)) { + if (RCL_RET_OK != rcl_wait_set_add_guard_condition(&wait_set, &gc_, NULL)) + { RCLCPP_WARN(rclcpp::get_logger("fuse"), "Could not add callback waitable to wait set."); } } /** - * @brief check the callback queue and return the next callback to run - * - */ + * @brief check the callback queue and return the next callback to run + * + */ std::shared_ptr CallbackAdapter::take_data() { std::shared_ptr cb_wrapper = nullptr; // fetch the callback ptr and release the lock without spending time in the callback { std::lock_guard lock(queue_mutex_); - if (!callback_queue_.empty()) { + if (!callback_queue_.empty()) + { cb_wrapper = callback_queue_.front(); callback_queue_.pop_front(); } - if (!callback_queue_.empty()) { + if (!callback_queue_.empty()) + { // Trigger so executor wakes again - if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) { - RCLCPP_WARN( - rclcpp::get_logger("fuse"), "Could not trigger guard condition for callback"); + if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) + { + RCLCPP_WARN(rclcpp::get_logger("fuse"), "Could not trigger guard condition for callback"); } } } @@ -104,38 +108,39 @@ std::shared_ptr CallbackAdapter::take_data() } /** - * @brief hook that allows the rclcpp::waitables interface to run the next callback - * - */ -void CallbackAdapter::execute(std::shared_ptr const & data) + * @brief hook that allows the rclcpp::waitables interface to run the next callback + * + */ +void CallbackAdapter::execute(std::shared_ptr const& data) { - if (!data) { + if (!data) + { throw std::runtime_error("'data' is empty"); } std::static_pointer_cast(data)->call(); } -void CallbackAdapter::addCallback(const std::shared_ptr & callback) +void CallbackAdapter::addCallback(const std::shared_ptr& callback) { std::lock_guard lock(queue_mutex_); callback_queue_.push_back(callback); - if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) { - RCLCPP_WARN( - rclcpp::get_logger("fuse"), - "Could not trigger guard condition for callback, pulling callback off the queue."); - callback_queue_.pop_back(); // Undo + if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) + { + RCLCPP_WARN(rclcpp::get_logger("fuse"), + "Could not trigger guard condition for callback, pulling callback off the queue."); + callback_queue_.pop_back(); // Undo } } -void CallbackAdapter::addCallback(std::shared_ptr && callback) +void CallbackAdapter::addCallback(std::shared_ptr&& callback) { std::lock_guard lock(queue_mutex_); callback_queue_.push_back(std::move(callback)); - if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) { - RCLCPP_WARN( - rclcpp::get_logger("fuse"), - "Could not trigger guard condition for callback, pulling callback off the queue."); - callback_queue_.pop_back(); // Undo + if (RCL_RET_OK != rcl_trigger_guard_condition(&gc_)) + { + RCLCPP_WARN(rclcpp::get_logger("fuse"), + "Could not trigger guard condition for callback, pulling callback off the queue."); + callback_queue_.pop_back(); // Undo } } @@ -145,5 +150,4 @@ void CallbackAdapter::removeAllCallbacks() callback_queue_.clear(); } - } // namespace fuse_core diff --git a/fuse_core/src/ceres_options.cpp b/fuse_core/src/ceres_options.cpp index 1919e7f82..960fd3c00 100644 --- a/fuse_core/src/ceres_options.cpp +++ b/fuse_core/src/ceres_options.cpp @@ -54,699 +54,471 @@ namespace fuse_core { void loadCovarianceOptionsFromROS( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - ceres::Covariance::Options & covariance_options, - const std::string & ns) + node_interfaces::NodeInterfaces + interfaces, + ceres::Covariance::Options& covariance_options, const std::string& ns) { rcl_interfaces::msg::ParameterDescriptor tmp_descr; // The sparse_linear_algebra_library_type field was added to ceres::Covariance::Options in version // 1.13.0, see https://github.com/ceres-solver/ceres- // solver/commit/14d8297cf968e421c5db4e3fb0543b3b111155d7 - covariance_options.sparse_linear_algebra_library_type = fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "sparse_linear_algebra_library_type"), - covariance_options.sparse_linear_algebra_library_type); - covariance_options.algorithm_type = - fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "algorithm_type"), - covariance_options.algorithm_type); - - tmp_descr.description = ( - "If DENSE_SVD is used, this parameter sets the threshold for determining if a Jacobian matrix " - "is rank deficient following the condition: " - "\n" - "min_sigma / max_sigma < sqrt(min_reciprocal_condition_number)" - "\n" - "Where min_sigma and max_sigma are the minimum and maximum singular values of J respectively."); - covariance_options.min_reciprocal_condition_number = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_reciprocal_condition_number"), - covariance_options.min_reciprocal_condition_number, - tmp_descr - ); + covariance_options.sparse_linear_algebra_library_type = + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "sparse_linear_algebra_library_type"), + covariance_options.sparse_linear_algebra_library_type); + covariance_options.algorithm_type = fuse_core::declareCeresParam( + interfaces, fuse_core::joinParameterName(ns, "algorithm_type"), covariance_options.algorithm_type); tmp_descr.description = - "The number of singular dimensions to tolerate (-1 unbounded) no effect on `SPARSE_QR`"; + ("If DENSE_SVD is used, this parameter sets the threshold for determining if a Jacobian matrix " + "is rank deficient following the condition: " + "\n" + "min_sigma / max_sigma < sqrt(min_reciprocal_condition_number)" + "\n" + "Where min_sigma and max_sigma are the minimum and maximum singular values of J respectively."); + covariance_options.min_reciprocal_condition_number = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_reciprocal_condition_number"), + covariance_options.min_reciprocal_condition_number, tmp_descr); + + tmp_descr.description = "The number of singular dimensions to tolerate (-1 unbounded) no effect on `SPARSE_QR`"; covariance_options.null_space_rank = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "null_space_rank"), - covariance_options.null_space_rank, tmp_descr - ); + interfaces, fuse_core::joinParameterName(ns, "null_space_rank"), covariance_options.null_space_rank, tmp_descr); + + tmp_descr.description = "Number of threads to be used for evaluating the Jacobian and estimation of covariance"; + covariance_options.num_threads = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "num_threads"), + covariance_options.num_threads, tmp_descr); tmp_descr.description = - "Number of threads to be used for evaluating the Jacobian and estimation of covariance"; - covariance_options.num_threads = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "num_threads"), covariance_options.num_threads, tmp_descr - ); - - tmp_descr.description = ( - "false will turn off the application of the loss function to the output of the cost function " - "and in turn its effect on the covariance (does not affect residual blocks with built-in loss " - "functions)"); - covariance_options.apply_loss_function = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "apply_loss_function"), - covariance_options.apply_loss_function, - tmp_descr - ); + ("false will turn off the application of the loss function to the output of the cost function " + "and in turn its effect on the covariance (does not affect residual blocks with built-in loss " + "functions)"); + covariance_options.apply_loss_function = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "apply_loss_function"), + covariance_options.apply_loss_function, tmp_descr); } -void loadProblemOptionsFromROS( - node_interfaces::NodeInterfaces interfaces, - ceres::Problem::Options & problem_options, - const std::string & ns) +void loadProblemOptionsFromROS(node_interfaces::NodeInterfaces interfaces, + ceres::Problem::Options& problem_options, const std::string& ns) { rcl_interfaces::msg::ParameterDescriptor tmp_descr; tmp_descr.description = "trades memory for faster Problem::RemoveResidualBlock()"; - problem_options.enable_fast_removal = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "enable_fast_removal"), - problem_options.enable_fast_removal, - tmp_descr - ); - - tmp_descr.description = ( - "By default, Ceres performs a variety of safety checks when constructing " - "the problem. There is a small but measurable performance penalty to these " - "checks, typically around 5% of construction time. If you are sure your " - "problem construction is correct, and 5% of the problem construction time " - "is truly an overhead you want to avoid, then you can set " - "disable_all_safety_checks to true." - "\n" - "WARNING: Do not set this to true, unless you are absolutely sure of what " - "you are doing"); - problem_options.disable_all_safety_checks = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "disable_all_safety_checks"), - problem_options.disable_all_safety_checks, - tmp_descr - ); + problem_options.enable_fast_removal = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "enable_fast_removal"), + problem_options.enable_fast_removal, tmp_descr); + + tmp_descr.description = ("By default, Ceres performs a variety of safety checks when constructing " + "the problem. There is a small but measurable performance penalty to these " + "checks, typically around 5% of construction time. If you are sure your " + "problem construction is correct, and 5% of the problem construction time " + "is truly an overhead you want to avoid, then you can set " + "disable_all_safety_checks to true." + "\n" + "WARNING: Do not set this to true, unless you are absolutely sure of what " + "you are doing"); + problem_options.disable_all_safety_checks = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_all_safety_checks"), + problem_options.disable_all_safety_checks, tmp_descr); } void loadSolverOptionsFromROS( - node_interfaces::NodeInterfaces< - node_interfaces::Base, - node_interfaces::Logging, - node_interfaces::Parameters - > interfaces, - ceres::Solver::Options & solver_options, - const std::string & ns) + node_interfaces::NodeInterfaces + interfaces, + ceres::Solver::Options& solver_options, const std::string& ns) { rcl_interfaces::msg::ParameterDescriptor tmp_descr; // Minimizer options solver_options.minimizer_type = fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "minimizer_type"), solver_options.minimizer_type); - solver_options.line_search_direction_type = fuse_core::declareCeresParam( - interfaces, - fuse_core::joinParameterName(ns, "line_search_direction_type"), - solver_options.line_search_direction_type - ); + interfaces, fuse_core::joinParameterName(ns, "minimizer_type"), solver_options.minimizer_type); + solver_options.line_search_direction_type = + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "line_search_direction_type"), + solver_options.line_search_direction_type); solver_options.line_search_type = fuse_core::declareCeresParam( - interfaces, - fuse_core::joinParameterName(ns, "line_search_type"), - solver_options.line_search_type - ); - solver_options.nonlinear_conjugate_gradient_type = fuse_core::declareCeresParam( - interfaces, - fuse_core::joinParameterName(ns, "nonlinear_conjugate_gradient_type"), - solver_options.nonlinear_conjugate_gradient_type - ); - - tmp_descr.description = ( - "The rank of the LBFGS hessian approximation. See: Nocedal, J. (1980). 'Updating Quasi-Newton " - "Matrices with Limited Storage'. Mathematics of Computation 35 (151): 773-782."); - solver_options.max_lbfgs_rank = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_lbfgs_rank"), - solver_options.max_lbfgs_rank, - tmp_descr - ); - - tmp_descr.description = ( - "As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS), " - "the initial inverse Hessian approximation is taken to be the Identity. " - "However, Oren showed that using instead I * \\gamma, where \\gamma is " - "chosen to approximate an eigenvalue of the true inverse Hessian can " - "result in improved convergence in a wide variety of cases. Setting " - "use_approximate_eigenvalue_bfgs_scaling to true enables this scaling."); - solver_options.use_approximate_eigenvalue_bfgs_scaling = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_approximate_eigenvalue_bfgs_scaling"), - solver_options.use_approximate_eigenvalue_bfgs_scaling, - tmp_descr - ); - - tmp_descr.description = ( - "Degree of the polynomial used to approximate the objective function. Valid values are " - "BISECTION, QUADRATIC and CUBIC."); - solver_options.line_search_interpolation_type = fuse_core::declareCeresParam( - interfaces, - fuse_core::joinParameterName(ns, "line_search_interpolation_type"), - solver_options.line_search_interpolation_type); + interfaces, fuse_core::joinParameterName(ns, "line_search_type"), solver_options.line_search_type); + solver_options.nonlinear_conjugate_gradient_type = + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "nonlinear_conjugate_gradient_type"), + solver_options.nonlinear_conjugate_gradient_type); tmp_descr.description = - "If during the line search, the step_size falls below this value, it is truncated to zero."; - solver_options.min_line_search_step_size = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_line_search_step_size"), - solver_options.min_line_search_step_size, - tmp_descr - ); + ("The rank of the LBFGS hessian approximation. See: Nocedal, J. (1980). 'Updating Quasi-Newton " + "Matrices with Limited Storage'. Mathematics of Computation 35 (151): 773-782."); + solver_options.max_lbfgs_rank = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_lbfgs_rank"), + solver_options.max_lbfgs_rank, tmp_descr); + + tmp_descr.description = ("As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS), " + "the initial inverse Hessian approximation is taken to be the Identity. " + "However, Oren showed that using instead I * \\gamma, where \\gamma is " + "chosen to approximate an eigenvalue of the true inverse Hessian can " + "result in improved convergence in a wide variety of cases. Setting " + "use_approximate_eigenvalue_bfgs_scaling to true enables this scaling."); + solver_options.use_approximate_eigenvalue_bfgs_scaling = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_approximate_eigenvalue_bfgs_scaling"), + solver_options.use_approximate_eigenvalue_bfgs_scaling, tmp_descr); + + tmp_descr.description = ("Degree of the polynomial used to approximate the objective function. Valid values are " + "BISECTION, QUADRATIC and CUBIC."); + solver_options.line_search_interpolation_type = + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "line_search_interpolation_type"), + solver_options.line_search_interpolation_type); + + tmp_descr.description = "If during the line search, the step_size falls below this value, it is truncated to zero."; + solver_options.min_line_search_step_size = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_line_search_step_size"), + solver_options.min_line_search_step_size, tmp_descr); // Line search parameters - tmp_descr.description = ( - "Solving the line search problem exactly is computationally " - "prohibitive. Fortunately, line search based optimization " - "algorithms can still guarantee convergence if instead of an " - "exact solution, the line search algorithm returns a solution " - "which decreases the value of the objective function " - "sufficiently. More precisely, we are looking for a step_size: " - "s.t. " - "f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size"); - solver_options.line_search_sufficient_function_decrease = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "line_search_sufficient_function_decrease"), - solver_options.line_search_sufficient_function_decrease, - tmp_descr - ); - - tmp_descr.description = ( - "In each iteration of the line search, " - "new_step_size >= max_line_search_step_contraction * step_size" - "\n" - "Note that by definition, for contraction: " - "0 < max_step_contraction < min_step_contraction < 1"); - solver_options.max_line_search_step_contraction = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_line_search_step_contraction"), - solver_options.max_line_search_step_contraction, - tmp_descr - ); - - tmp_descr.description = ( - "In each iteration of the line search, " - "new_step_size <= min_line_search_step_contraction * step_size" - "\n" - "Note that by definition, for contraction: " - "0 < max_step_contraction < min_step_contraction < 1"); - solver_options.min_line_search_step_contraction = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_line_search_step_contraction"), - solver_options.min_line_search_step_contraction, - tmp_descr - ); - - tmp_descr.description = ( - "Maximum number of trial step size iterations during each line " - "search, if a step size satisfying the search conditions cannot " - "be found within this number of trials, the line search will " - "terminate. " - - "The minimum allowed value is 0 for trust region minimizer and 1 " - "otherwise. If 0 is specified for the trust region minimizer, " - "then line search will not be used when solving constrained " - "optimization problems. "); - solver_options.max_num_line_search_step_size_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_num_line_search_step_size_iterations"), - solver_options.max_num_line_search_step_size_iterations, - tmp_descr - ); - - tmp_descr.description = ( - "Maximum number of restarts of the line search direction algorithm before " - "terminating the optimization. Restarts of the line search direction " - "algorithm occur when the current algorithm fails to produce a new descent " - "direction. This typically indicates a numerical failure, or a breakdown " - "in the validity of the approximations used. "); - solver_options.max_num_line_search_direction_restarts = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_num_line_search_direction_restarts"), - solver_options.max_num_line_search_direction_restarts, - tmp_descr - ); - - tmp_descr.description = ( - "The strong Wolfe conditions consist of the Armijo sufficient " - "decrease condition, and an additional requirement that the " - "step-size be chosen s.t. the _magnitude_ ('strong' Wolfe " - "conditions) of the gradient along the search direction " - "decreases sufficiently. Precisely, this second condition " - "is that we seek a step_size s.t. " - "\n" - " |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)|" - "\n" - "Where f() is the line search objective and f'() is the derivative of f " - "w.r.t step_size (d f / d step_size)."); - solver_options.line_search_sufficient_curvature_decrease = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "line_search_sufficient_curvature_decrease"), - solver_options.line_search_sufficient_curvature_decrease, - tmp_descr - ); - - tmp_descr.description = ( - "During the bracketing phase of the Wolfe search, the step size is " - "increased until either a point satisfying the Wolfe conditions is " - "found, or an upper bound for a bracket containing a point satisfying " - "the conditions is found. Precisely, at each iteration of the expansion:" - "\n" - " new_step_size <= max_step_expansion * step_size." - "\n" - "By definition for expansion, max_step_expansion > 1.0."); - solver_options.max_line_search_step_expansion = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_line_search_step_expansion"), - solver_options.max_line_search_step_expansion, - tmp_descr - ); - - solver_options.trust_region_strategy_type = fuse_core::declareCeresParam( - interfaces, - fuse_core::joinParameterName(ns, "trust_region_strategy_type"), - solver_options.trust_region_strategy_type - ); - solver_options.dogleg_type = fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "dogleg_type"), solver_options.dogleg_type); - - tmp_descr.description = ( - "Enables the non-monotonic trust region algorithm as described by Conn, " - "Gould & Toint in 'Trust Region Methods', Section 10.1"); - solver_options.use_nonmonotonic_steps = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_nonmonotonic_steps"), - solver_options.use_nonmonotonic_steps, - tmp_descr - ); - - tmp_descr.description = - "The window size used by the step selection algorithm to accept non-monotonic steps"; - solver_options.max_consecutive_nonmonotonic_steps = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_consecutive_nonmonotonic_steps"), - solver_options.max_consecutive_nonmonotonic_steps, - tmp_descr - ); + tmp_descr.description = ("Solving the line search problem exactly is computationally " + "prohibitive. Fortunately, line search based optimization " + "algorithms can still guarantee convergence if instead of an " + "exact solution, the line search algorithm returns a solution " + "which decreases the value of the objective function " + "sufficiently. More precisely, we are looking for a step_size: " + "s.t. " + "f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size"); + solver_options.line_search_sufficient_function_decrease = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "line_search_sufficient_function_decrease"), + solver_options.line_search_sufficient_function_decrease, tmp_descr); + + tmp_descr.description = ("In each iteration of the line search, " + "new_step_size >= max_line_search_step_contraction * step_size" + "\n" + "Note that by definition, for contraction: " + "0 < max_step_contraction < min_step_contraction < 1"); + solver_options.max_line_search_step_contraction = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_line_search_step_contraction"), + solver_options.max_line_search_step_contraction, tmp_descr); + + tmp_descr.description = ("In each iteration of the line search, " + "new_step_size <= min_line_search_step_contraction * step_size" + "\n" + "Note that by definition, for contraction: " + "0 < max_step_contraction < min_step_contraction < 1"); + solver_options.min_line_search_step_contraction = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_line_search_step_contraction"), + solver_options.min_line_search_step_contraction, tmp_descr); + + tmp_descr.description = ("Maximum number of trial step size iterations during each line " + "search, if a step size satisfying the search conditions cannot " + "be found within this number of trials, the line search will " + "terminate. " + + "The minimum allowed value is 0 for trust region minimizer and 1 " + "otherwise. If 0 is specified for the trust region minimizer, " + "then line search will not be used when solving constrained " + "optimization problems. "); + solver_options.max_num_line_search_step_size_iterations = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_num_line_search_step_size_iterations"), + solver_options.max_num_line_search_step_size_iterations, tmp_descr); + + tmp_descr.description = ("Maximum number of restarts of the line search direction algorithm before " + "terminating the optimization. Restarts of the line search direction " + "algorithm occur when the current algorithm fails to produce a new descent " + "direction. This typically indicates a numerical failure, or a breakdown " + "in the validity of the approximations used. "); + solver_options.max_num_line_search_direction_restarts = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_num_line_search_direction_restarts"), + solver_options.max_num_line_search_direction_restarts, tmp_descr); + + tmp_descr.description = ("The strong Wolfe conditions consist of the Armijo sufficient " + "decrease condition, and an additional requirement that the " + "step-size be chosen s.t. the _magnitude_ ('strong' Wolfe " + "conditions) of the gradient along the search direction " + "decreases sufficiently. Precisely, this second condition " + "is that we seek a step_size s.t. " + "\n" + " |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)|" + "\n" + "Where f() is the line search objective and f'() is the derivative of f " + "w.r.t step_size (d f / d step_size)."); + solver_options.line_search_sufficient_curvature_decrease = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "line_search_sufficient_curvature_decrease"), + solver_options.line_search_sufficient_curvature_decrease, tmp_descr); + + tmp_descr.description = ("During the bracketing phase of the Wolfe search, the step size is " + "increased until either a point satisfying the Wolfe conditions is " + "found, or an upper bound for a bracket containing a point satisfying " + "the conditions is found. Precisely, at each iteration of the expansion:" + "\n" + " new_step_size <= max_step_expansion * step_size." + "\n" + "By definition for expansion, max_step_expansion > 1.0."); + solver_options.max_line_search_step_expansion = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_line_search_step_expansion"), + solver_options.max_line_search_step_expansion, tmp_descr); + + solver_options.trust_region_strategy_type = + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "trust_region_strategy_type"), + solver_options.trust_region_strategy_type); + solver_options.dogleg_type = fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "dogleg_type"), + solver_options.dogleg_type); + + tmp_descr.description = ("Enables the non-monotonic trust region algorithm as described by Conn, " + "Gould & Toint in 'Trust Region Methods', Section 10.1"); + solver_options.use_nonmonotonic_steps = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_nonmonotonic_steps"), + solver_options.use_nonmonotonic_steps, tmp_descr); + + tmp_descr.description = "The window size used by the step selection algorithm to accept non-monotonic steps"; + solver_options.max_consecutive_nonmonotonic_steps = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_consecutive_nonmonotonic_steps"), + solver_options.max_consecutive_nonmonotonic_steps, tmp_descr); tmp_descr.description = "Maximum number of iterations for which the solver should run"; solver_options.max_num_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_num_iterations"), - solver_options.max_num_iterations, - tmp_descr - ); + interfaces, fuse_core::joinParameterName(ns, "max_num_iterations"), solver_options.max_num_iterations, tmp_descr); tmp_descr.description = "Maximum amount of time for which the solver should run"; - solver_options.max_solver_time_in_seconds = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_solver_time_in_seconds"), - solver_options.max_solver_time_in_seconds, - tmp_descr - ); + solver_options.max_solver_time_in_seconds = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_solver_time_in_seconds"), + solver_options.max_solver_time_in_seconds, tmp_descr); tmp_descr.description = "Number of threads used by Ceres for evaluating the cost and jacobians"; - solver_options.num_threads = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "num_threads"), - solver_options.num_threads, - tmp_descr - ); - - tmp_descr.description = ( - "The size of the initial trust region. When the LEVENBERG_MARQUARDT strategy is used, the " - "reciprocal of this number is the initial regularization parameter"); - solver_options.initial_trust_region_radius = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "initial_trust_region_radius"), - solver_options.initial_trust_region_radius, - tmp_descr - ); + solver_options.num_threads = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "num_threads"), + solver_options.num_threads, tmp_descr); + + tmp_descr.description = ("The size of the initial trust region. When the LEVENBERG_MARQUARDT strategy is used, the " + "reciprocal of this number is the initial regularization parameter"); + solver_options.initial_trust_region_radius = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "initial_trust_region_radius"), + solver_options.initial_trust_region_radius, tmp_descr); tmp_descr.description = "The trust region radius is not allowed to grow beyond this value"; - solver_options.max_trust_region_radius = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_trust_region_radius"), - solver_options.max_trust_region_radius, - tmp_descr - ); + solver_options.max_trust_region_radius = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_trust_region_radius"), + solver_options.max_trust_region_radius, tmp_descr); + + tmp_descr.description = "The solver terminates when the trust region becomes smaller than this value"; + solver_options.min_trust_region_radius = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_trust_region_radius"), + solver_options.min_trust_region_radius, tmp_descr); + + tmp_descr.description = "Lower threshold for relative decrease before a trust-region step is accepted"; + solver_options.min_relative_decrease = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_relative_decrease"), + solver_options.min_relative_decrease, tmp_descr); tmp_descr.description = - "The solver terminates when the trust region becomes smaller than this value"; - solver_options.min_trust_region_radius = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_trust_region_radius"), - solver_options.min_trust_region_radius, - tmp_descr - ); + ("The LEVENBERG_MARQUARDT strategy, uses a diagonal matrix to regularize the trust region step. " + "This is the lower bound on the values of this diagonal matrix"); + solver_options.min_lm_diagonal = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_lm_diagonal"), + solver_options.min_lm_diagonal, tmp_descr); tmp_descr.description = - "Lower threshold for relative decrease before a trust-region step is accepted"; - solver_options.min_relative_decrease = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_relative_decrease"), - solver_options.min_relative_decrease, - tmp_descr - ); - - tmp_descr.description = ( - "The LEVENBERG_MARQUARDT strategy, uses a diagonal matrix to regularize the trust region step. " - "This is the lower bound on the values of this diagonal matrix"); - solver_options.min_lm_diagonal = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_lm_diagonal"), - solver_options.min_lm_diagonal, - tmp_descr - ); - - tmp_descr.description = ( - "The LEVENBERG_MARQUARDT strategy, uses a diagonal matrix to regularize the trust region step. " - "This is the upper bound on the values of this diagonal matrix"); - solver_options.max_lm_diagonal = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_lm_diagonal"), - solver_options.max_lm_diagonal, - tmp_descr - ); - - tmp_descr.description = ( - "The step returned by a trust region strategy can sometimes be numerically invalid, usually " - "because of conditioning issues. Instead of crashing or stopping the optimization, the " - "optimizer can go ahead and try solving with a smaller trust region/better conditioned problem." - " This parameter sets the number of consecutive retries before the minimizer gives up"); - solver_options.max_num_consecutive_invalid_steps = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_num_consecutive_invalid_steps"), - solver_options.max_num_consecutive_invalid_steps, - tmp_descr - ); + ("The LEVENBERG_MARQUARDT strategy, uses a diagonal matrix to regularize the trust region step. " + "This is the upper bound on the values of this diagonal matrix"); + solver_options.max_lm_diagonal = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_lm_diagonal"), + solver_options.max_lm_diagonal, tmp_descr); tmp_descr.description = - "Minimizer terminates when: (new_cost - old_cost) < function_tolerance * old_cost;"; + ("The step returned by a trust region strategy can sometimes be numerically invalid, usually " + "because of conditioning issues. Instead of crashing or stopping the optimization, the " + "optimizer can go ahead and try solving with a smaller trust region/better conditioned problem." + " This parameter sets the number of consecutive retries before the minimizer gives up"); + solver_options.max_num_consecutive_invalid_steps = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_num_consecutive_invalid_steps"), + solver_options.max_num_consecutive_invalid_steps, tmp_descr); + + tmp_descr.description = "Minimizer terminates when: (new_cost - old_cost) < function_tolerance * old_cost;"; solver_options.function_tolerance = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "function_tolerance"), - solver_options.function_tolerance, - tmp_descr - ); - - tmp_descr.description = ( - "Minimizer terminates when: max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance" - "\n" - "This value should typically be 1e-4 * function_tolerance"); + interfaces, fuse_core::joinParameterName(ns, "function_tolerance"), solver_options.function_tolerance, tmp_descr); + + tmp_descr.description = ("Minimizer terminates when: max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance" + "\n" + "This value should typically be 1e-4 * function_tolerance"); solver_options.gradient_tolerance = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "gradient_tolerance"), - solver_options.gradient_tolerance, - tmp_descr - ); + interfaces, fuse_core::joinParameterName(ns, "gradient_tolerance"), solver_options.gradient_tolerance, tmp_descr); tmp_descr.description = - "Minimizer terminates when: |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance)"; - solver_options.parameter_tolerance = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "parameter_tolerance"), - solver_options.parameter_tolerance, - tmp_descr - ); - - solver_options.linear_solver_type = - fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "linear_solver_type"), - solver_options.linear_solver_type); - solver_options.preconditioner_type = - fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "preconditioner_type"), - solver_options.preconditioner_type); + "Minimizer terminates when: |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance)"; + solver_options.parameter_tolerance = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "parameter_tolerance"), + solver_options.parameter_tolerance, tmp_descr); + + solver_options.linear_solver_type = fuse_core::declareCeresParam( + interfaces, fuse_core::joinParameterName(ns, "linear_solver_type"), solver_options.linear_solver_type); + solver_options.preconditioner_type = fuse_core::declareCeresParam( + interfaces, fuse_core::joinParameterName(ns, "preconditioner_type"), solver_options.preconditioner_type); solver_options.visibility_clustering_type = - fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "visibility_clustering_type"), - solver_options.visibility_clustering_type); + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "visibility_clustering_type"), + solver_options.visibility_clustering_type); solver_options.dense_linear_algebra_library_type = - fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "dense_linear_algebra_library_type"), - solver_options.dense_linear_algebra_library_type); - solver_options.sparse_linear_algebra_library_type = fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "sparse_linear_algebra_library_type"), - solver_options.sparse_linear_algebra_library_type); + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "dense_linear_algebra_library_type"), + solver_options.dense_linear_algebra_library_type); + solver_options.sparse_linear_algebra_library_type = + fuse_core::declareCeresParam(interfaces, fuse_core::joinParameterName(ns, "sparse_linear_algebra_library_type"), + solver_options.sparse_linear_algebra_library_type); // No parameter is loaded for: std::shared_ptr linear_solver_ordering; - tmp_descr.description = - "Enabling this option tells ITERATIVE_SCHUR to use an explicitly computed Schur complement."; - solver_options.use_explicit_schur_complement = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_explicit_schur_complement"), - solver_options.use_explicit_schur_complement, - tmp_descr - ); + tmp_descr.description = "Enabling this option tells ITERATIVE_SCHUR to use an explicitly computed Schur complement."; + solver_options.use_explicit_schur_complement = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_explicit_schur_complement"), + solver_options.use_explicit_schur_complement, tmp_descr); #if !CERES_VERSION_AT_LEAST(2, 2, 0) - tmp_descr.description = ( - "In some rare cases, it is worth using a more complicated " - "reordering algorithm which has slightly better runtime " - "performance at the expense of an extra copy of the Jacobian " - "matrix. Setting use_postordering to true enables this tradeoff."); + tmp_descr.description = ("In some rare cases, it is worth using a more complicated " + "reordering algorithm which has slightly better runtime " + "performance at the expense of an extra copy of the Jacobian " + "matrix. Setting use_postordering to true enables this tradeoff."); solver_options.use_postordering = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_postordering"), - solver_options.use_postordering, - tmp_descr - ); + interfaces, fuse_core::joinParameterName(ns, "use_postordering"), solver_options.use_postordering, tmp_descr); #endif tmp_descr.description = "This settings only affects the SPARSE_NORMAL_CHOLESKY solver."; solver_options.dynamic_sparsity = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "dynamic_sparsity"), - solver_options.dynamic_sparsity, - tmp_descr - ); + interfaces, fuse_core::joinParameterName(ns, "dynamic_sparsity"), solver_options.dynamic_sparsity, tmp_descr); #if CERES_VERSION_AT_LEAST(2, 0, 0) - tmp_descr.description = ( - "NOTE1: EXPERIMENTAL FEATURE, UNDER DEVELOPMENT, USE AT YOUR OWN RISK. " - "\n" - "If use_mixed_precision_solves is true, the Gauss-Newton matrix " - "is computed in double precision, but its factorization is " - "computed in single precision. This can result in significant " - "time and memory savings at the cost of some accuracy in the " - "Gauss-Newton step. Iterative refinement is used to recover some " - "of this accuracy back."); - solver_options.use_mixed_precision_solves = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_mixed_precision_solves"), - solver_options.use_mixed_precision_solves, - tmp_descr - ); + tmp_descr.description = ("NOTE1: EXPERIMENTAL FEATURE, UNDER DEVELOPMENT, USE AT YOUR OWN RISK. " + "\n" + "If use_mixed_precision_solves is true, the Gauss-Newton matrix " + "is computed in double precision, but its factorization is " + "computed in single precision. This can result in significant " + "time and memory savings at the cost of some accuracy in the " + "Gauss-Newton step. Iterative refinement is used to recover some " + "of this accuracy back."); + solver_options.use_mixed_precision_solves = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_mixed_precision_solves"), + solver_options.use_mixed_precision_solves, tmp_descr); tmp_descr.description = - "Number steps of the iterative refinement process to run when computing the Gauss-Newton step."; - solver_options.max_num_refinement_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_num_refinement_iterations"), - solver_options.max_num_refinement_iterations, - tmp_descr - ); + "Number steps of the iterative refinement process to run when computing the Gauss-Newton step."; + solver_options.max_num_refinement_iterations = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_num_refinement_iterations"), + solver_options.max_num_refinement_iterations, tmp_descr); #endif #if CERES_VERSION_AT_LEAST(2, 2, 0) - tmp_descr.description = ( - "Maximum number of iterations performed by SCHUR_POWER_SERIES_EXPANSION. " - "Each iteration corresponds to one more term in the power series expansion " - "of the inverse of the Schur complement. This value controls the maximum " - "number of iterations whether it is used as a preconditioner or just to " - "initialize the solution for ITERATIVE_SCHUR."); - solver_options.max_num_spse_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_num_spse_iterations"), - solver_options.max_num_spse_iterations, - tmp_descr - ); - - tmp_descr.description = ( - "Use SCHUR_POWER_SERIES_EXPANSION to initialize the solution for " - "ITERATIVE_SCHUR. This option can be set true regardless of what " - "preconditioner is being used."); - solver_options.use_spse_initialization = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_spse_initialization"), - solver_options.use_spse_initialization, - tmp_descr - ); - - tmp_descr.description = ( - "When use_spse_initialization is true, this parameter along with " - "max_num_spse_iterations controls the number of " - "SCHUR_POWER_SERIES_EXPANSION iterations performed for initialization. It " - "is not used to control the preconditioner."); - solver_options.spse_tolerance = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "spse_tolerance"), - solver_options.spse_tolerance, - tmp_descr - ); + tmp_descr.description = ("Maximum number of iterations performed by SCHUR_POWER_SERIES_EXPANSION. " + "Each iteration corresponds to one more term in the power series expansion " + "of the inverse of the Schur complement. This value controls the maximum " + "number of iterations whether it is used as a preconditioner or just to " + "initialize the solution for ITERATIVE_SCHUR."); + solver_options.max_num_spse_iterations = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_num_spse_iterations"), + solver_options.max_num_spse_iterations, tmp_descr); + + tmp_descr.description = ("Use SCHUR_POWER_SERIES_EXPANSION to initialize the solution for " + "ITERATIVE_SCHUR. This option can be set true regardless of what " + "preconditioner is being used."); + solver_options.use_spse_initialization = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_spse_initialization"), + solver_options.use_spse_initialization, tmp_descr); + + tmp_descr.description = ("When use_spse_initialization is true, this parameter along with " + "max_num_spse_iterations controls the number of " + "SCHUR_POWER_SERIES_EXPANSION iterations performed for initialization. It " + "is not used to control the preconditioner."); + solver_options.spse_tolerance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "spse_tolerance"), + solver_options.spse_tolerance, tmp_descr); #endif - tmp_descr.description = - "Enable the use of the non-linear generalization of Ruhe & Wedin's Algorithm II"; - solver_options.use_inner_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "use_inner_iterations"), - solver_options.use_inner_iterations, - tmp_descr - ); + tmp_descr.description = "Enable the use of the non-linear generalization of Ruhe & Wedin's Algorithm II"; + solver_options.use_inner_iterations = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_inner_iterations"), + solver_options.use_inner_iterations, tmp_descr); // No parameter is loaded for: std::shared_ptr inner_iteration_ordering; - tmp_descr.description = ( - "Once the relative decrease in the objective function due to " - "inner iterations drops below inner_iteration_tolerance, the use " - "of inner iterations in subsequent trust region minimizer " - "iterations is disabled."); - solver_options.inner_iteration_tolerance = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "inner_iteration_tolerance"), - solver_options.inner_iteration_tolerance, - tmp_descr - ); + tmp_descr.description = ("Once the relative decrease in the objective function due to " + "inner iterations drops below inner_iteration_tolerance, the use " + "of inner iterations in subsequent trust region minimizer " + "iterations is disabled."); + solver_options.inner_iteration_tolerance = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "inner_iteration_tolerance"), + solver_options.inner_iteration_tolerance, tmp_descr); tmp_descr.description = "Minimum number of iterations used by the linear iterative solver"; - solver_options.min_linear_solver_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "min_linear_solver_iterations"), - solver_options.min_linear_solver_iterations, - tmp_descr - ); + solver_options.min_linear_solver_iterations = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "min_linear_solver_iterations"), + solver_options.min_linear_solver_iterations, tmp_descr); tmp_descr.description = "Maximum number of iterations used by the linear iterative solver"; - solver_options.max_linear_solver_iterations = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "max_linear_solver_iterations"), - solver_options.max_linear_solver_iterations, - tmp_descr - ); - - tmp_descr.description = ( - "Forcing sequence parameter. The truncated Newton solver uses this number to control the " - "relative accuracy with which the Newton step is computed"); - solver_options.eta = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "eta"), - solver_options.eta, - tmp_descr - ); - - tmp_descr.description = ( - "True means that the Jacobian is scaled by the norm of its columns before being passed to the " - "linear solver. This improves the numerical conditioning of the normal equations"); - solver_options.jacobi_scaling = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "jacobi_scaling"), - solver_options.jacobi_scaling, - tmp_descr - ); + solver_options.max_linear_solver_iterations = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "max_linear_solver_iterations"), + solver_options.max_linear_solver_iterations, tmp_descr); + + tmp_descr.description = ("Forcing sequence parameter. The truncated Newton solver uses this number to control the " + "relative accuracy with which the Newton step is computed"); + solver_options.eta = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "eta"), solver_options.eta, tmp_descr); + + tmp_descr.description = + ("True means that the Jacobian is scaled by the norm of its columns before being passed to the " + "linear solver. This improves the numerical conditioning of the normal equations"); + solver_options.jacobi_scaling = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "jacobi_scaling"), + solver_options.jacobi_scaling, tmp_descr); // Logging options solver_options.logging_type = fuse_core::declareCeresParam( - interfaces, fuse_core::joinParameterName(ns, "logging_type"), solver_options.logging_type); + interfaces, fuse_core::joinParameterName(ns, "logging_type"), solver_options.logging_type); tmp_descr.description = "If logging_type is not SILENT, sends the logging output to STDOUT"; - solver_options.minimizer_progress_to_stdout = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "minimizer_progress_to_stdout"), - solver_options.minimizer_progress_to_stdout, - tmp_descr - ); + solver_options.minimizer_progress_to_stdout = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "minimizer_progress_to_stdout"), + solver_options.minimizer_progress_to_stdout, tmp_descr); fuse_core::getParam>( - interfaces, - fuse_core::joinParameterName(ns, "trust_region_minimizer_iterations_to_dump"), - std::vector() - ); - std::vector iterations_to_dump_tmp = interfaces.get_node_parameters_interface() - ->get_parameter(fuse_core::joinParameterName(ns, "trust_region_minimizer_iterations_to_dump")) - .get_value>(); - if (!iterations_to_dump_tmp.empty()) { + interfaces, fuse_core::joinParameterName(ns, "trust_region_minimizer_iterations_to_dump"), std::vector()); + std::vector iterations_to_dump_tmp = + interfaces.get_node_parameters_interface() + ->get_parameter(fuse_core::joinParameterName(ns, "trust_region_minimizer_iterations_to_dump")) + .get_value>(); + if (!iterations_to_dump_tmp.empty()) + { solver_options.trust_region_minimizer_iterations_to_dump.reserve(iterations_to_dump_tmp.size()); - std::transform( - iterations_to_dump_tmp.begin(), - iterations_to_dump_tmp.end(), - std::back_inserter(solver_options.trust_region_minimizer_iterations_to_dump), - [](int64_t val) {return val;}); + std::transform(iterations_to_dump_tmp.begin(), iterations_to_dump_tmp.end(), + std::back_inserter(solver_options.trust_region_minimizer_iterations_to_dump), + [](int64_t val) { return val; }); } - tmp_descr.description = ( - "Directory to which the problems should be written to. Should be " - "non-empty if trust_region_minimizer_iterations_to_dump is " - "non-empty and trust_region_problem_dump_format_type is not " - "CONSOLE."); - solver_options.trust_region_problem_dump_directory = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "trust_region_problem_dump_directory"), - solver_options.trust_region_problem_dump_directory, - tmp_descr - ); + tmp_descr.description = ("Directory to which the problems should be written to. Should be " + "non-empty if trust_region_minimizer_iterations_to_dump is " + "non-empty and trust_region_problem_dump_format_type is not " + "CONSOLE."); + solver_options.trust_region_problem_dump_directory = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "trust_region_problem_dump_directory"), + solver_options.trust_region_problem_dump_directory, tmp_descr); solver_options.trust_region_problem_dump_format_type = - fuse_core::declareCeresParam( - interfaces, - fuse_core::joinParameterName(ns, "trust_region_problem_dump_format_type"), - solver_options.trust_region_problem_dump_format_type - ); + fuse_core::declareCeresParam(interfaces, + fuse_core::joinParameterName(ns, "trust_region_problem_dump_format_type"), + solver_options.trust_region_problem_dump_format_type); // Finite differences options - tmp_descr.description = ( - "Check all Jacobians computed by each residual block with finite differences, abort if numeric " - "and analytic gradients differ substantially)"); - solver_options.check_gradients = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "check_gradients"), - solver_options.check_gradients, - tmp_descr - ); - tmp_descr.description = ( - "Precision to check for in the gradient checker. If the relative " - "difference between an element in a Jacobian exceeds this number, then the Jacobian for that " - "cost term is dumped"); - solver_options.gradient_check_relative_precision = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "gradient_check_relative_precision"), - solver_options.gradient_check_relative_precision, - tmp_descr - ); - tmp_descr.description = ( - "Relative shift used for taking numeric derivatives when " - "Solver::Options::check_gradients is true."); - solver_options.gradient_check_numeric_derivative_relative_step_size = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "gradient_check_numeric_derivative_relative_step_size"), - solver_options.gradient_check_numeric_derivative_relative_step_size, - tmp_descr - ); - tmp_descr.description = ( - "If update_state_every_iteration is true, then Ceres Solver will guarantee that at the end of " - "every iteration and before any user IterationCallback is called, the parameter blocks are " - "updated to the current best solution found by the solver. Thus the IterationCallback can " - "inspect the values of the parameter blocks for purposes of computation, visualization or " - "termination"); - solver_options.update_state_every_iteration = fuse_core::getParam( - interfaces, - fuse_core::joinParameterName(ns, "update_state_every_iteration"), - solver_options.update_state_every_iteration, - tmp_descr - ); + tmp_descr.description = + ("Check all Jacobians computed by each residual block with finite differences, abort if numeric " + "and analytic gradients differ substantially)"); + solver_options.check_gradients = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "check_gradients"), + solver_options.check_gradients, tmp_descr); + tmp_descr.description = + ("Precision to check for in the gradient checker. If the relative " + "difference between an element in a Jacobian exceeds this number, then the Jacobian for that " + "cost term is dumped"); + solver_options.gradient_check_relative_precision = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "gradient_check_relative_precision"), + solver_options.gradient_check_relative_precision, tmp_descr); + tmp_descr.description = ("Relative shift used for taking numeric derivatives when " + "Solver::Options::check_gradients is true."); + solver_options.gradient_check_numeric_derivative_relative_step_size = + fuse_core::getParam(interfaces, + fuse_core::joinParameterName(ns, "gradient_check_numeric_derivative_relative_step_size"), + solver_options.gradient_check_numeric_derivative_relative_step_size, tmp_descr); + tmp_descr.description = + ("If update_state_every_iteration is true, then Ceres Solver will guarantee that at the end of " + "every iteration and before any user IterationCallback is called, the parameter blocks are " + "updated to the current best solution found by the solver. Thus the IterationCallback can " + "inspect the values of the parameter blocks for purposes of computation, visualization or " + "termination"); + solver_options.update_state_every_iteration = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "update_state_every_iteration"), + solver_options.update_state_every_iteration, tmp_descr); std::string error; - if (!solver_options.IsValid(&error)) { - throw std::invalid_argument( - "Invalid solver options in parameter " + - std::string(interfaces.get_node_base_interface()->get_namespace()) + - ". Error: " + error); + if (!solver_options.IsValid(&error)) + { + throw std::invalid_argument("Invalid solver options in parameter " + + std::string(interfaces.get_node_base_interface()->get_namespace()) + + ". Error: " + error); } } // NOLINT [readability/fn_size] diff --git a/fuse_core/src/constraint.cpp b/fuse_core/src/constraint.cpp index 89a32ecf4..0a3690cea 100644 --- a/fuse_core/src/constraint.cpp +++ b/fuse_core/src/constraint.cpp @@ -41,14 +41,12 @@ namespace fuse_core { -Constraint::Constraint(const std::string & source, std::initializer_list variable_uuid_list) -: source_(source), - uuid_(uuid::generate()), - variables_(variable_uuid_list) +Constraint::Constraint(const std::string& source, std::initializer_list variable_uuid_list) + : source_(source), uuid_(uuid::generate()), variables_(variable_uuid_list) { } -std::ostream & operator<<(std::ostream & stream, const Constraint & constraint) +std::ostream& operator<<(std::ostream& stream, const Constraint& constraint) { constraint.print(stream); return stream; diff --git a/fuse_core/src/fuse_echo.cpp b/fuse_core/src/fuse_echo.cpp index 9cf3f4a01..2d2f6c0d9 100644 --- a/fuse_core/src/fuse_echo.cpp +++ b/fuse_core/src/fuse_echo.cpp @@ -50,20 +50,13 @@ namespace fuse_core class FuseEcho : public rclcpp::Node { public: - explicit FuseEcho(rclcpp::NodeOptions options) - : Node("fuse_echo", options) + explicit FuseEcho(rclcpp::NodeOptions options) : Node("fuse_echo", options) { // Subscribe to the constraint topic graph_sub_ = this->create_subscription( - "graph", - rclcpp::QoS(100), - std::bind(&FuseEcho::graphCallback, this, std::placeholders::_1) - ); + "graph", rclcpp::QoS(100), std::bind(&FuseEcho::graphCallback, this, std::placeholders::_1)); transaction_sub_ = this->create_subscription( - "transaction", - rclcpp::QoS(100), - std::bind(&FuseEcho::transactionCallback, this, std::placeholders::_1) - ); + "transaction", rclcpp::QoS(100), std::bind(&FuseEcho::transactionCallback, this, std::placeholders::_1)); } private: @@ -72,7 +65,7 @@ class FuseEcho : public rclcpp::Node rclcpp::Subscription::SharedPtr graph_sub_; rclcpp::Subscription::SharedPtr transaction_sub_; - void graphCallback(const fuse_msgs::msg::SerializedGraph & msg) + void graphCallback(const fuse_msgs::msg::SerializedGraph& msg) { std::cout << "-------------------------" << std::endl; std::cout << "GRAPH:" << std::endl; @@ -81,7 +74,7 @@ class FuseEcho : public rclcpp::Node graph->print(); } - void transactionCallback(const fuse_msgs::msg::SerializedTransaction & msg) + void transactionCallback(const fuse_msgs::msg::SerializedTransaction& msg) { std::cout << "-------------------------" << std::endl; std::cout << "TRANSACTION:" << std::endl; @@ -93,7 +86,6 @@ class FuseEcho : public rclcpp::Node } // namespace fuse_core - #include RCLCPP_COMPONENTS_REGISTER_NODE(fuse_core::FuseEcho) diff --git a/fuse_core/src/graph.cpp b/fuse_core/src/graph.cpp index 06ecbd9b2..ddd9cf153 100644 --- a/fuse_core/src/graph.cpp +++ b/fuse_core/src/graph.cpp @@ -41,48 +41,48 @@ namespace fuse_core { -std::ostream & operator<<(std::ostream & stream, const Graph & graph) +std::ostream& operator<<(std::ostream& stream, const Graph& graph) { graph.print(stream); return stream; } -Graph::const_variable_range Graph::getConnectedVariables(const UUID & constraint_uuid) const +Graph::const_variable_range Graph::getConnectedVariables(const UUID& constraint_uuid) const { - std::function uuid_to_variable_ref = - [this](const UUID & variable_uuid) -> const Variable & - { - return this->getVariable(variable_uuid); - }; + std::function uuid_to_variable_ref = + [this](const UUID& variable_uuid) -> const Variable& { return this->getVariable(variable_uuid); }; - const auto & constraint = getConstraint(constraint_uuid); - const auto & variable_uuids = constraint.variables(); + const auto& constraint = getConstraint(constraint_uuid); + const auto& variable_uuids = constraint.variables(); - return const_variable_range( - boost::make_transform_iterator(variable_uuids.cbegin(), uuid_to_variable_ref), - boost::make_transform_iterator(variable_uuids.cend(), uuid_to_variable_ref)); + return const_variable_range(boost::make_transform_iterator(variable_uuids.cbegin(), uuid_to_variable_ref), + boost::make_transform_iterator(variable_uuids.cend(), uuid_to_variable_ref)); } -void Graph::update(const Transaction & transaction) +void Graph::update(const Transaction& transaction) { // Update the graph with a new transaction. In order to keep the graph consistent, variables are // added first, followed by the constraints which might use the newly added variables. Then // constraints are removed so that the variable usage is updated. Finally, variables are removed. // Insert the new variables into the graph - for (const auto & variable : transaction.addedVariables()) { + for (const auto& variable : transaction.addedVariables()) + { addVariable(variable.clone()); } // Insert the new constraints into the graph - for (const auto & constraint : transaction.addedConstraints()) { + for (const auto& constraint : transaction.addedConstraints()) + { addConstraint(constraint.clone()); } // Delete constraints from the graph - for (const auto & constraint_uuid : transaction.removedConstraints()) { + for (const auto& constraint_uuid : transaction.removedConstraints()) + { removeConstraint(constraint_uuid); } // Delete variables from the graph - for (const auto & variable_uuid : transaction.removedVariables()) { + for (const auto& variable_uuid : transaction.removedVariables()) + { removeVariable(variable_uuid); } } diff --git a/fuse_core/src/graph_deserializer.cpp b/fuse_core/src/graph_deserializer.cpp index e21e9e94c..76fb19d53 100644 --- a/fuse_core/src/graph_deserializer.cpp +++ b/fuse_core/src/graph_deserializer.cpp @@ -38,7 +38,7 @@ namespace fuse_core { -void serializeGraph(const fuse_core::Graph & graph, fuse_msgs::msg::SerializedGraph & msg) +void serializeGraph(const fuse_core::Graph& graph, fuse_msgs::msg::SerializedGraph& msg) { // Serialize the graph into the msg.data field boost::iostreams::stream stream(msg.data); @@ -54,27 +54,29 @@ void serializeGraph(const fuse_core::Graph & graph, fuse_msgs::msg::SerializedGr } GraphDeserializer::GraphDeserializer() -: variable_loader_("fuse_core", "fuse_core::Variable"), - constraint_loader_("fuse_core", "fuse_core::Constraint"), - loss_loader_("fuse_core", "fuse_core::Loss"), - graph_loader_("fuse_core", "fuse_core::Graph") + : variable_loader_("fuse_core", "fuse_core::Variable") + , constraint_loader_("fuse_core", "fuse_core::Constraint") + , loss_loader_("fuse_core", "fuse_core::Loss") + , graph_loader_("fuse_core", "fuse_core::Graph") { // Load all known plugin libraries // I believe the library containing a given Variable or Constraint type must be loaded in order to // deserialize an object of that type. But I haven't actually tested that theory. - for (const auto & class_name : variable_loader_.getDeclaredClasses()) { + for (const auto& class_name : variable_loader_.getDeclaredClasses()) + { variable_loader_.loadLibraryForClass(class_name); } - for (const auto & class_name : constraint_loader_.getDeclaredClasses()) { + for (const auto& class_name : constraint_loader_.getDeclaredClasses()) + { constraint_loader_.loadLibraryForClass(class_name); } - for (const auto & class_name : loss_loader_.getDeclaredClasses()) { + for (const auto& class_name : loss_loader_.getDeclaredClasses()) + { loss_loader_.loadLibraryForClass(class_name); } } -fuse_core::Graph::UniquePtr GraphDeserializer::deserialize( - const fuse_msgs::msg::SerializedGraph & msg) const +fuse_core::Graph::UniquePtr GraphDeserializer::deserialize(const fuse_msgs::msg::SerializedGraph& msg) const { // Create a Graph object using pluginlib. This will throw if the plugin name is not found. // The unique ptr returned by pluginlib has a custom deleter. This makes it annoying to return diff --git a/fuse_core/src/loss.cpp b/fuse_core/src/loss.cpp index 44e0e8311..328cf3fb6 100644 --- a/fuse_core/src/loss.cpp +++ b/fuse_core/src/loss.cpp @@ -38,7 +38,7 @@ namespace fuse_core { -std::ostream & operator<<(std::ostream & stream, const Loss & loss) +std::ostream& operator<<(std::ostream& stream, const Loss& loss) { loss.print(stream); return stream; diff --git a/fuse_core/src/parameter.cpp b/fuse_core/src/parameter.cpp index a2d01a2d7..2ffc13446 100644 --- a/fuse_core/src/parameter.cpp +++ b/fuse_core/src/parameter.cpp @@ -38,43 +38,44 @@ #include #include - namespace fuse_core { std::unordered_set -list_parameter_override_prefixes( - node_interfaces::NodeInterfaces interfaces, - std::string prefix) +list_parameter_override_prefixes(node_interfaces::NodeInterfaces interfaces, + std::string prefix) { - const std::map & overrides = - interfaces.get_node_parameters_interface()->get_parameter_overrides(); + const std::map& overrides = + interfaces.get_node_parameters_interface()->get_parameter_overrides(); return detail::list_parameter_override_prefixes(overrides, prefix); } std::unordered_set -detail::list_parameter_override_prefixes( - const std::map & overrides, - std::string prefix) +detail::list_parameter_override_prefixes(const std::map& overrides, + std::string prefix) { // TODO(sloretz) ROS 2 must have this in a header somewhere, right? const char kParamSeparator = '.'; // Find all overrides starting with "prefix.", unless the prefix is empty. // If the prefix is empty then look at all parameter overrides. - if (!prefix.empty() && prefix.back() != kParamSeparator) { + if (!prefix.empty() && prefix.back() != kParamSeparator) + { prefix += kParamSeparator; } std::unordered_set output_names; - for (const auto & kv : overrides) { - const std::string & name = kv.first; - if (name.size() <= prefix.size()) { + for (const auto& kv : overrides) + { + const std::string& name = kv.first; + if (name.size() <= prefix.size()) + { // Too short, no point in checking continue; } assert(prefix.size() < name.size()); // TODO(sloretz) use string::starts_with in c++20 - if (name.rfind(prefix, 0) == 0) { // if name starts with prefix + if (name.rfind(prefix, 0) == 0) + { // if name starts with prefix // Truncate names to the next separator size_t separator_pos = name.find(kParamSeparator, prefix.size()); // Insert truncated name @@ -84,18 +85,22 @@ detail::list_parameter_override_prefixes( return output_names; } -std::string joinParameterName(const std::string & left, const std::string & right) +std::string joinParameterName(const std::string& left, const std::string& right) { - if (left.empty()) { + if (left.empty()) + { return right; } - if (right.empty()) { + if (right.empty()) + { return left; } - if ('.' != left.back() && '.' != right.front() ) { + if ('.' != left.back() && '.' != right.front()) + { return left + '.' + right; } - if ('.' == left.back() && '.' == right.front()) { + if ('.' == left.back() && '.' == right.front()) + { return left + right.substr(1); } return left + right; diff --git a/fuse_core/src/serialization.cpp b/fuse_core/src/serialization.cpp index 36bf7af60..3debb1b39 100644 --- a/fuse_core/src/serialization.cpp +++ b/fuse_core/src/serialization.cpp @@ -39,30 +39,30 @@ namespace fuse_core { -MessageBufferStreamSource::MessageBufferStreamSource(const std::vector & data) -: data_(data), - index_(0) +MessageBufferStreamSource::MessageBufferStreamSource(const std::vector& data) : data_(data), index_(0) { } -std::streamsize MessageBufferStreamSource::read(char_type * s, std::streamsize n) +std::streamsize MessageBufferStreamSource::read(char_type* s, std::streamsize n) { std::streamsize result = std::min(n, static_cast(data_.size() - index_)); - if (result != 0) { + if (result != 0) + { std::copy(data_.begin() + index_, data_.begin() + index_ + result, s); index_ += result; return result; - } else { + } + else + { return -1; // EOF } } -MessageBufferStreamSink::MessageBufferStreamSink(std::vector & data) -: data_(data) +MessageBufferStreamSink::MessageBufferStreamSink(std::vector& data) : data_(data) { } -std::streamsize MessageBufferStreamSink::write(const char_type * s, std::streamsize n) +std::streamsize MessageBufferStreamSink::write(const char_type* s, std::streamsize n) { data_.insert(data_.end(), s, s + n); return n; diff --git a/fuse_core/src/timestamp_manager.cpp b/fuse_core/src/timestamp_manager.cpp index cfcfdecd2..80031b584 100644 --- a/fuse_core/src/timestamp_manager.cpp +++ b/fuse_core/src/timestamp_manager.cpp @@ -43,31 +43,25 @@ namespace fuse_core { -TimestampManager::TimestampManager( - MotionModelFunction generator, - const rclcpp::Duration & buffer_length) -: generator_(generator), - buffer_length_(buffer_length) +TimestampManager::TimestampManager(MotionModelFunction generator, const rclcpp::Duration& buffer_length) + : generator_(generator), buffer_length_(buffer_length) { } -void TimestampManager::query( - Transaction & transaction, - bool update_variables) +void TimestampManager::query(Transaction& transaction, bool update_variables) { // Handle the trivial cases first - const auto & stamps = transaction.involvedStamps(); - if (stamps.empty()) { + const auto& stamps = transaction.involvedStamps(); + if (stamps.empty()) + { return; } // Verify the query is within the buffer length - if ( (!motion_model_history_.empty()) && - (buffer_length_ != rclcpp::Duration::max()) && - (stamps.front() < motion_model_history_.begin()->first) && - (stamps.front() < (motion_model_history_.rbegin()->first - buffer_length_))) + if ((!motion_model_history_.empty()) && (buffer_length_ != rclcpp::Duration::max()) && + (stamps.front() < motion_model_history_.begin()->first) && + (stamps.front() < (motion_model_history_.rbegin()->first - buffer_length_))) { - throw std::invalid_argument( - "All timestamps must be within the defined buffer length of the motion model"); + throw std::invalid_argument("All timestamps must be within the defined buffer length of the motion model"); } // Create a list of all the required timestamps involved in motion model segments that must be // created Add all of the existing timestamps between the first and last input stamp @@ -77,46 +71,45 @@ void TimestampManager::query( auto last_stamp = *augmented_stamps.rbegin(); { auto begin = motion_model_history_.upper_bound(first_stamp); - if (begin != motion_model_history_.begin()) { + if (begin != motion_model_history_.begin()) + { --begin; } auto end = motion_model_history_.upper_bound(last_stamp); - for (auto iter = begin; iter != end; ++iter) { + for (auto iter = begin; iter != end; ++iter) + { augmented_stamps.insert(iter->first); } - if (end != motion_model_history_.end()) { + if (end != motion_model_history_.end()) + { augmented_stamps.insert(end->first); } } // Convert the sequence of stamps into stamp pairs that must be generated std::vector> stamp_pairs; { - for (auto previous_iter = augmented_stamps.begin(), - current_iter = std::next(augmented_stamps.begin()); - current_iter != augmented_stamps.end(); - ++previous_iter, ++current_iter) + for (auto previous_iter = augmented_stamps.begin(), current_iter = std::next(augmented_stamps.begin()); + current_iter != augmented_stamps.end(); ++previous_iter, ++current_iter) { - const rclcpp::Time & previous_stamp = *previous_iter; - const rclcpp::Time & current_stamp = *current_iter; + const rclcpp::Time& previous_stamp = *previous_iter; + const rclcpp::Time& current_stamp = *current_iter; // Check if the timestamp pair is exactly an existing pair. If so, don't add it. auto history_iter = motion_model_history_.lower_bound(previous_stamp); - if ((history_iter != motion_model_history_.end()) && - (history_iter->second.beginning_stamp == previous_stamp) && - (history_iter->second.ending_stamp == current_stamp)) + if ((history_iter != motion_model_history_.end()) && (history_iter->second.beginning_stamp == previous_stamp) && + (history_iter->second.ending_stamp == current_stamp)) { - if (update_variables) { + if (update_variables) + { // Add the motion model version of the variables involved in this motion model segment // This ensures that the variables in the final transaction will be overwritten with the // motion model version auto transaction_variables = transaction.addedVariables(); - for (const auto & variable : history_iter->second.variables) { - if (std::any_of( - transaction_variables.begin(), - transaction_variables.end(), - [variable_uuid = variable->uuid()](const auto & input_variable) - { - return input_variable.uuid() == variable_uuid; - })) // NOLINT + for (const auto& variable : history_iter->second.variables) + { + if (std::any_of(transaction_variables.begin(), transaction_variables.end(), + [variable_uuid = variable->uuid()](const auto& input_variable) { + return input_variable.uuid() == variable_uuid; + })) // NOLINT { motion_model_transaction.addVariable(variable, update_variables); } @@ -125,9 +118,8 @@ void TimestampManager::query( continue; } // Check if this stamp is in the middle of an existing entry. If so, delete it. - if ((history_iter != motion_model_history_.end()) && - (history_iter->second.beginning_stamp < current_stamp) && - (history_iter->second.ending_stamp >= current_stamp)) + if ((history_iter != motion_model_history_.end()) && (history_iter->second.beginning_stamp < current_stamp) && + (history_iter->second.ending_stamp >= current_stamp)) { removeSegment(history_iter, motion_model_transaction); } @@ -136,12 +128,15 @@ void TimestampManager::query( } } // Create the required segments - for (const auto & stamp_pair : stamp_pairs) { + for (const auto& stamp_pair : stamp_pairs) + { addSegment(stamp_pair.first, stamp_pair.second, motion_model_transaction); } // Add a dummy entry for the last stamp if one does not already exist - if (motion_model_history_.empty() || (motion_model_history_.rbegin()->first < last_stamp)) { - if (motion_model_history_.empty()) { + if (motion_model_history_.empty() || (motion_model_history_.rbegin()->first < last_stamp)) + { + if (motion_model_history_.empty()) + { // Call the motion model generator so it inserts the last timestamp into its state history. std::vector constraints; std::vector variables; @@ -160,21 +155,15 @@ void TimestampManager::query( TimestampManager::const_stamp_range TimestampManager::stamps() const { - std::function extract_stamp = - [](const MotionModelHistory::value_type & element) -> const rclcpp::Time & - { - return element.first; - }; + std::function 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 const_stamp_range(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, - Transaction & transaction) +void TimestampManager::addSegment(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + Transaction& transaction) { // Generate the set of constraints and variables to add std::vector constraints; @@ -183,28 +172,25 @@ void TimestampManager::addSegment( // Update the transaction with the generated constraints/variables transaction.addInvolvedStamp(beginning_stamp); transaction.addInvolvedStamp(ending_stamp); - for (const auto & constraint : constraints) { + for (const auto& constraint : constraints) + { transaction.addConstraint(constraint); } - for (const auto & variable : variables) { + for (const auto& variable : variables) + { transaction.addVariable(variable); } - motion_model_history_.insert_or_assign( - beginning_stamp, MotionModelSegment( - beginning_stamp, - ending_stamp, - constraints, - variables)); + motion_model_history_.insert_or_assign(beginning_stamp, + MotionModelSegment(beginning_stamp, ending_stamp, constraints, variables)); } -void TimestampManager::removeSegment( - MotionModelHistory::iterator & iter, - Transaction & transaction) +void TimestampManager::removeSegment(MotionModelHistory::iterator& iter, Transaction& transaction) { // Mark the previously generated constraints for removal transaction.addInvolvedStamp(iter->second.beginning_stamp); transaction.addInvolvedStamp(iter->second.ending_stamp); - for (const auto & constraint : iter->second.constraints) { + for (const auto& constraint : iter->second.constraints) + { transaction.removeConstraint(constraint->uuid()); } // We do not remove variables here. It is assumed the variables are still in use by other @@ -214,10 +200,8 @@ void TimestampManager::removeSegment( motion_model_history_.erase(iter); } -void TimestampManager::splitSegment( - MotionModelHistory::iterator & iter, - const rclcpp::Time & stamp, - Transaction & transaction) +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; @@ -234,7 +218,8 @@ void TimestampManager::purgeHistory() // Purge any motion model segments that are more than buffer_length_ seconds older than the most // recent entry A setting of rclcpp::Duration::max() means "keep everything" And we want to keep // at least one entry in motion model history, regardless of the stamps. - if ((buffer_length_ == rclcpp::Duration::max()) || (motion_model_history_.size() <= 1)) { + if ((buffer_length_ == rclcpp::Duration::max()) || (motion_model_history_.size() <= 1)) + { return; } // Continue to remove the first entry from the history until we: @@ -242,8 +227,8 @@ void TimestampManager::purgeHistory() // (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; - while ( (motion_model_history_.size() > 1) && - ((ending_stamp - motion_model_history_.begin()->second.ending_stamp) > buffer_length_)) + while ((motion_model_history_.size() > 1) && + ((ending_stamp - motion_model_history_.begin()->second.ending_stamp) > buffer_length_)) { motion_model_history_.erase(motion_model_history_.begin()); } diff --git a/fuse_core/src/transaction.cpp b/fuse_core/src/transaction.cpp index cbee6c839..dc1c088a6 100644 --- a/fuse_core/src/transaction.cpp +++ b/fuse_core/src/transaction.cpp @@ -41,40 +41,42 @@ namespace fuse_core { -const rclcpp::Time & Transaction::minStamp() const +const rclcpp::Time& Transaction::minStamp() const { - if (involved_stamps_.empty()) { + if (involved_stamps_.empty()) + { return stamp_; - } else { + } + else + { return std::min(*involved_stamps_.begin(), stamp_); } } -const rclcpp::Time & Transaction::maxStamp() const +const rclcpp::Time& Transaction::maxStamp() const { - if (involved_stamps_.empty()) { + if (involved_stamps_.empty()) + { return stamp_; - } else { + } + else + { return std::max(*involved_stamps_.rbegin(), stamp_); } } -void Transaction::addInvolvedStamp(const rclcpp::Time & stamp) +void Transaction::addInvolvedStamp(const rclcpp::Time& stamp) { involved_stamps_.insert(stamp); } Transaction::const_constraint_range Transaction::addedConstraints() const { - std::function to_constraint_ref = - [](const Constraint::SharedPtr & constraint)->const Constraint & - { - return *constraint; - }; + std::function 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 const_constraint_range(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) @@ -82,49 +84,45 @@ void Transaction::addConstraint(Constraint::SharedPtr constraint, bool overwrite // If the constraint being added is in the 'removed' container, then delete it from the 'removed' // container instead of adding it to the 'added' container. UUID constraint_uuid = constraint->uuid(); - auto removed_constraints_iter = std::find( - removed_constraints_.begin(), - removed_constraints_.end(), constraint_uuid); - if (removed_constraints_iter != removed_constraints_.end()) { + auto removed_constraints_iter = std::find(removed_constraints_.begin(), removed_constraints_.end(), constraint_uuid); + if (removed_constraints_iter != removed_constraints_.end()) + { removed_constraints_.erase(removed_constraints_iter); return; } // Also don't add the same constraint twice - auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr & added_constraint) - { - return constraint_uuid == added_constraint->uuid(); - }; - auto added_constraints_iter = std::find_if( - added_constraints_.begin(), - added_constraints_.end(), is_constraint_added); - if (added_constraints_iter == added_constraints_.end()) { + auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr& added_constraint) { + return constraint_uuid == added_constraint->uuid(); + }; + auto added_constraints_iter = std::find_if(added_constraints_.begin(), added_constraints_.end(), is_constraint_added); + if (added_constraints_iter == added_constraints_.end()) + { added_constraints_.push_back(std::move(constraint)); - } else if (overwrite) { + } + else if (overwrite) + { *added_constraints_iter = std::move(constraint); } } -void Transaction::removeConstraint(const UUID & constraint_uuid) +void Transaction::removeConstraint(const UUID& constraint_uuid) { // If the constraint being removed is in the 'added' container, then delete it from the 'added' // container instead of adding it to the 'removed' container. - auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr & added_constraint) - { - return constraint_uuid == added_constraint->uuid(); - }; - auto added_constraints_iter = std::find_if( - added_constraints_.begin(), - added_constraints_.end(), is_constraint_added); - if (added_constraints_iter != added_constraints_.end()) { + auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr& added_constraint) { + return constraint_uuid == added_constraint->uuid(); + }; + auto added_constraints_iter = std::find_if(added_constraints_.begin(), added_constraints_.end(), is_constraint_added); + if (added_constraints_iter != added_constraints_.end()) + { added_constraints_.erase(added_constraints_iter); return; } // Also don't remove the same constraint twice - auto removed_constraints_iter = std::find( - removed_constraints_.begin(), - removed_constraints_.end(), constraint_uuid); - if (removed_constraints_iter == removed_constraints_.end()) { + auto removed_constraints_iter = std::find(removed_constraints_.begin(), removed_constraints_.end(), constraint_uuid); + if (removed_constraints_iter == removed_constraints_.end()) + { removed_constraints_.push_back(constraint_uuid); return; } @@ -132,22 +130,17 @@ void Transaction::removeConstraint(const UUID & constraint_uuid) Transaction::const_variable_range Transaction::addedVariables() const { - std::function to_variable_ref = - [](const Variable::SharedPtr & variable)->const Variable & - { - return *variable; - }; + std::function 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 const_variable_range(boost::make_transform_iterator(added_variables_.cbegin(), to_variable_ref), + boost::make_transform_iterator(added_variables_.cend(), to_variable_ref)); } bool Transaction::empty() const { - return boost::empty(added_variables_) && boost::empty(removed_variables_) && - boost::empty(added_constraints_) && boost::empty(removed_constraints_) && - involved_stamps_.empty(); + return boost::empty(added_variables_) && boost::empty(removed_variables_) && boost::empty(added_constraints_) && + boost::empty(removed_constraints_) && involved_stamps_.empty(); } void Transaction::addVariable(Variable::SharedPtr variable, bool overwrite) @@ -156,94 +149,99 @@ void Transaction::addVariable(Variable::SharedPtr variable, bool overwrite) // container instead of adding it to the 'added' container. UUID variable_uuid = variable->uuid(); - auto removed_variables_iter = std::find( - removed_variables_.begin(), - removed_variables_.end(), variable_uuid); - if (removed_variables_iter != removed_variables_.end()) { + auto removed_variables_iter = std::find(removed_variables_.begin(), removed_variables_.end(), variable_uuid); + if (removed_variables_iter != removed_variables_.end()) + { removed_variables_.erase(removed_variables_iter); return; } // Also don't add the same variable twice - auto is_variable_added = [&variable_uuid](const Variable::SharedPtr & added_variable) - { - return variable_uuid == added_variable->uuid(); - }; - auto added_variables_iter = std::find_if( - added_variables_.begin(), - added_variables_.end(), is_variable_added); - if (added_variables_iter == added_variables_.end()) { + auto is_variable_added = [&variable_uuid](const Variable::SharedPtr& added_variable) { + return variable_uuid == added_variable->uuid(); + }; + auto added_variables_iter = std::find_if(added_variables_.begin(), added_variables_.end(), is_variable_added); + if (added_variables_iter == added_variables_.end()) + { added_variables_.push_back(std::move(variable)); - } else if (overwrite) { + } + else if (overwrite) + { *added_variables_iter = std::move(variable); } } -void Transaction::removeVariable(const UUID & variable_uuid) +void Transaction::removeVariable(const UUID& variable_uuid) { // If the variable being removed is in the 'added' container, then delete it from the 'added' // container instead of adding it to the 'removed' container. - auto is_variable_added = [&variable_uuid](const Variable::SharedPtr & added_variable) - { - return variable_uuid == added_variable->uuid(); - }; - auto added_variables_iter = std::find_if( - added_variables_.begin(), - added_variables_.end(), is_variable_added); - if (added_variables_iter != added_variables_.end()) { + auto is_variable_added = [&variable_uuid](const Variable::SharedPtr& added_variable) { + return variable_uuid == added_variable->uuid(); + }; + auto added_variables_iter = std::find_if(added_variables_.begin(), added_variables_.end(), is_variable_added); + if (added_variables_iter != added_variables_.end()) + { added_variables_.erase(added_variables_iter); return; } // Also don't remove the same variable twice - auto removed_variables_iter = std::find( - removed_variables_.begin(), - removed_variables_.end(), variable_uuid); - if (removed_variables_iter == removed_variables_.end()) { + auto removed_variables_iter = std::find(removed_variables_.begin(), removed_variables_.end(), variable_uuid); + if (removed_variables_iter == removed_variables_.end()) + { removed_variables_.push_back(variable_uuid); return; } } -void Transaction::merge(const Transaction & other, bool overwrite) +void Transaction::merge(const Transaction& other, bool overwrite) { stamp_ = std::max(stamp_, other.stamp_); involved_stamps_.insert(other.involved_stamps_.begin(), other.involved_stamps_.end()); - for (const auto & added_constraint : other.added_constraints_) { + for (const auto& added_constraint : other.added_constraints_) + { addConstraint(added_constraint, overwrite); } - for (const auto & removed_constraint : other.removed_constraints_) { + for (const auto& removed_constraint : other.removed_constraints_) + { removeConstraint(removed_constraint); } - for (const auto & added_variable : other.added_variables_) { + for (const auto& added_variable : other.added_variables_) + { addVariable(added_variable, overwrite); } - for (const auto & removed_variable : other.removed_variables_) { + for (const auto& removed_variable : other.removed_variables_) + { removeVariable(removed_variable); } } -void Transaction::print(std::ostream & stream) const +void Transaction::print(std::ostream& stream) const { stream << "Stamp: " << stamp_.nanoseconds() << "\n"; stream << "Involved Timestamps:\n"; - for (const auto & involved_stamp : involved_stamps_) { + for (const auto& involved_stamp : involved_stamps_) + { stream << " - " << involved_stamp.nanoseconds() << "\n"; } stream << "Added Variables:\n"; - for (const auto & added_variable : added_variables_) { + for (const auto& added_variable : added_variables_) + { stream << " - " << *added_variable << "\n"; } stream << "Added Constraints:\n"; - for (const auto & added_constraint : added_constraints_) { + for (const auto& added_constraint : added_constraints_) + { stream << " - " << *added_constraint << "\n"; } stream << "Removed Variables:\n"; - for (const auto & removed_variable : removed_variables_) { + for (const auto& removed_variable : removed_variables_) + { stream << " - " << removed_variable << "\n"; } stream << "Removed Constraints:\n"; - for (const auto & removed_constraint : removed_constraints_) { + for (const auto& removed_constraint : removed_constraints_) + { stream << " - " << removed_constraint << "\n"; } } @@ -253,27 +251,27 @@ Transaction::UniquePtr Transaction::clone() const return Transaction::make_unique(*this); } -void Transaction::serialize(fuse_core::BinaryOutputArchive & archive) const +void Transaction::serialize(fuse_core::BinaryOutputArchive& archive) const { archive << *this; } -void Transaction::serialize(fuse_core::TextOutputArchive & archive) const +void Transaction::serialize(fuse_core::TextOutputArchive& archive) const { archive << *this; } -void Transaction::deserialize(fuse_core::BinaryInputArchive & archive) +void Transaction::deserialize(fuse_core::BinaryInputArchive& archive) { archive >> *this; } -void Transaction::deserialize(fuse_core::TextInputArchive & archive) +void Transaction::deserialize(fuse_core::TextInputArchive& archive) { archive >> *this; } -std::ostream & operator<<(std::ostream & stream, const Transaction & transaction) +std::ostream& operator<<(std::ostream& stream, const Transaction& transaction) { transaction.print(stream); return stream; diff --git a/fuse_core/src/transaction_deserializer.cpp b/fuse_core/src/transaction_deserializer.cpp index b2c1f82d4..9308b5c9f 100644 --- a/fuse_core/src/transaction_deserializer.cpp +++ b/fuse_core/src/transaction_deserializer.cpp @@ -38,9 +38,7 @@ namespace fuse_core { -void serializeTransaction( - const fuse_core::Transaction & transaction, - fuse_msgs::msg::SerializedTransaction & msg) +void serializeTransaction(const fuse_core::Transaction& transaction, fuse_msgs::msg::SerializedTransaction& msg) { // Serialize the transaction into the msg.data field boost::iostreams::stream stream(msg.data); @@ -53,26 +51,29 @@ void serializeTransaction( } TransactionDeserializer::TransactionDeserializer() -: variable_loader_("fuse_core", "fuse_core::Variable"), - constraint_loader_("fuse_core", "fuse_core::Constraint"), - loss_loader_("fuse_core", "fuse_core::Loss") + : variable_loader_("fuse_core", "fuse_core::Variable") + , constraint_loader_("fuse_core", "fuse_core::Constraint") + , loss_loader_("fuse_core", "fuse_core::Loss") { // Load all known plugin libraries // I believe the library containing a given Variable or Constraint must be loaded in order to // deserialize an object of that type. But I haven't actually tested that theory. - for (const auto & class_name : variable_loader_.getDeclaredClasses()) { + for (const auto& class_name : variable_loader_.getDeclaredClasses()) + { variable_loader_.loadLibraryForClass(class_name); } - for (const auto & class_name : constraint_loader_.getDeclaredClasses()) { + for (const auto& class_name : constraint_loader_.getDeclaredClasses()) + { constraint_loader_.loadLibraryForClass(class_name); } - for (const auto & class_name : loss_loader_.getDeclaredClasses()) { + for (const auto& class_name : loss_loader_.getDeclaredClasses()) + { loss_loader_.loadLibraryForClass(class_name); } } -fuse_core::Transaction::UniquePtr TransactionDeserializer::deserialize( - const fuse_msgs::msg::SerializedTransaction & msg) const +fuse_core::Transaction::UniquePtr +TransactionDeserializer::deserialize(const fuse_msgs::msg::SerializedTransaction& msg) const { // The Transaction object is not a plugin and has no derived types. That makes it much easier to // use. diff --git a/fuse_core/src/uuid.cpp b/fuse_core/src/uuid.cpp index 608d3d4a2..fe139576a 100644 --- a/fuse_core/src/uuid.cpp +++ b/fuse_core/src/uuid.cpp @@ -61,7 +61,7 @@ UUID generate() return uuid; } -UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp) +UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp) { const auto nanoseconds = stamp.nanoseconds(); constexpr size_t buffer_size = sizeof(nanoseconds); @@ -69,7 +69,8 @@ UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp) // Explicitly pack nanosecond bits from LSB -> MSB by masking and shifting // E.g. 10 would be 0x00000000'0000000A, so the buffer would store 0x0A at buffer[0] - for (size_t i = 0; i < sizeof(nanoseconds); i++) { + for (size_t i = 0; i < sizeof(nanoseconds); i++) + { auto mask = (nanoseconds & (static_cast(0xFF) << 8 * i)); buffer[i] = static_cast(mask >> 8 * i); } @@ -77,14 +78,15 @@ UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp) return generate(namespace_string, buffer.data(), buffer.size()); } -UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp, const UUID & id) +UUID generate(const std::string& namespace_string, const rclcpp::Time& stamp, const UUID& id) { const auto nanoseconds = stamp.nanoseconds(); constexpr size_t buffer_size = sizeof(nanoseconds) + UUID::static_size(); std::array buffer; // Explicitly pack nanosecond bits from LSB -> MSB by masking and shifting - for (size_t i = 0; i < sizeof(nanoseconds); i++) { + for (size_t i = 0; i < sizeof(nanoseconds); i++) + { auto mask = (nanoseconds & (static_cast(0xFF) << 8 * i)); buffer[i] = static_cast(mask >> 8 * i); } @@ -96,11 +98,9 @@ UUID generate(const std::string & namespace_string, const rclcpp::Time & stamp, return generate(namespace_string, buffer.data(), buffer.size()); } -UUID generate(const std::string & namespace_string, const uint64_t & user_id) +UUID generate(const std::string& namespace_string, const uint64_t& user_id) { - return generate( - namespace_string, reinterpret_cast(&user_id), - sizeof(user_id)); + return generate(namespace_string, reinterpret_cast(&user_id), sizeof(user_id)); } } // namespace uuid diff --git a/fuse_core/src/variable.cpp b/fuse_core/src/variable.cpp index 00b098ccb..6b8f40749 100644 --- a/fuse_core/src/variable.cpp +++ b/fuse_core/src/variable.cpp @@ -38,12 +38,11 @@ namespace fuse_core { -Variable::Variable(const UUID & uuid) -: uuid_(uuid) +Variable::Variable(const UUID& uuid) : uuid_(uuid) { } -std::ostream & operator<<(std::ostream & stream, const Variable & variable) +std::ostream& operator<<(std::ostream& stream, const Variable& variable) { variable.print(stream); return stream; diff --git a/fuse_core/test/CMakeLists.txt b/fuse_core/test/CMakeLists.txt index 60fcb5491..012e67ea0 100644 --- a/fuse_core/test/CMakeLists.txt +++ b/fuse_core/test/CMakeLists.txt @@ -1,4 +1,5 @@ -# CORE GTESTS ====================================================================================== +# CORE GTESTS +# ====================================================================================== ament_add_gtest(test_constraint test_constraint.cpp) target_link_libraries(test_constraint ${PROJECT_NAME}) @@ -29,11 +30,12 @@ target_link_libraries(test_util ${PROJECT_NAME}) ament_add_gtest(test_uuid test_uuid.cpp) target_link_libraries(test_uuid ${PROJECT_NAME}) -ament_add_gtest(test_variable test_variable.cpp WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) +ament_add_gtest(test_variable test_variable.cpp WORKING_DIRECTORY + ${PROJECT_SOURCE_DIR}/test) target_link_libraries(test_variable ${PROJECT_NAME}) - -# ROS TESTS (NO LAUNCH) ============================================================================ +# ROS TESTS (NO LAUNCH) +# ============================================================================ ament_add_gtest(test_async_motion_model test_async_motion_model.cpp) target_link_libraries(test_async_motion_model ${PROJECT_NAME}) @@ -48,17 +50,18 @@ target_link_libraries(test_callback_wrapper ${PROJECT_NAME}) find_package(geometry_msgs REQUIRED) ament_add_gtest(test_throttled_callback test_throttled_callback.cpp) -target_link_libraries(test_throttled_callback ${PROJECT_NAME} ${geometry_msgs_TARGETS}) - +target_link_libraries(test_throttled_callback ${PROJECT_NAME} + ${geometry_msgs_TARGETS}) -# ROS TESTS (WITH LAUNCH) ========================================================================== +# ROS TESTS (WITH LAUNCH) +# ========================================================================== find_package(ament_cmake_pytest REQUIRED) ament_add_gtest_executable(test_parameters launch_tests/test_parameters.cpp) target_link_libraries(test_parameters ${PROJECT_NAME}) ament_add_pytest_test(test_parameters_py "launch_tests/test_parameters.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -) + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") -configure_file("launch_tests/test_parameters.yaml" "launch_tests/test_parameters.yaml" COPYONLY) +configure_file("launch_tests/test_parameters.yaml" + "launch_tests/test_parameters.yaml" COPYONLY) diff --git a/fuse_core/test/example_constraint.hpp b/fuse_core/test/example_constraint.hpp index 4185a7784..4df0ef035 100644 --- a/fuse_core/test/example_constraint.hpp +++ b/fuse_core/test/example_constraint.hpp @@ -56,25 +56,24 @@ class ExampleConstraint : public fuse_core::Constraint ExampleConstraint() = default; - ExampleConstraint( - const std::string & source, - std::initializer_list variable_uuid_list) - : fuse_core::Constraint(source, variable_uuid_list), - data(0.0) + ExampleConstraint(const std::string& source, std::initializer_list variable_uuid_list) + : fuse_core::Constraint(source, variable_uuid_list), data(0.0) { } - template - ExampleConstraint( - const std::string & source, VariableUuidIterator first, - VariableUuidIterator last) - : fuse_core::Constraint(source, first, last), - data(0.0) + template + ExampleConstraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last) + : fuse_core::Constraint(source, first, last), data(0.0) { } - void print(std::ostream & /*stream = std::cout*/) const override {} - ceres::CostFunction * costFunction() const override {return nullptr;} + void print(std::ostream& /*stream = std::cout*/) const override + { + } + ceres::CostFunction* costFunction() const override + { + return nullptr; + } double data; // Public member variable just for testing @@ -89,11 +88,11 @@ class ExampleConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data; + archive& boost::serialization::base_object(*this); + archive& data; } }; diff --git a/fuse_core/test/example_loss.hpp b/fuse_core/test/example_loss.hpp index 1421d1fca..2f8c11533 100644 --- a/fuse_core/test/example_loss.hpp +++ b/fuse_core/test/example_loss.hpp @@ -52,27 +52,27 @@ class ExampleLoss : public fuse_core::Loss public: FUSE_LOSS_DEFINITIONS(ExampleLoss) - explicit ExampleLoss(const double a = 1.0) - : a(a) + explicit ExampleLoss(const double a = 1.0) : a(a) { } void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - >/*interfaces*/, - const std::string & /*name*/) override {} + fuse_core::node_interfaces::NodeInterfaces /*interfaces*/, + const std::string& /*name*/) override + { + } - void print(std::ostream & /*stream = std::cout*/) const override {} + void print(std::ostream& /*stream = std::cout*/) const override + { + } - ceres::LossFunction * lossFunction() const override + ceres::LossFunction* lossFunction() const override { return new ceres::HuberLoss(a); } - double a{1.0}; //!< Public member variable just for testing + double a{ 1.0 }; //!< Public member variable just for testing private: // Allow Boost Serialization access to private methods @@ -85,11 +85,11 @@ class ExampleLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a; + archive& boost::serialization::base_object(*this); + archive& a; } }; diff --git a/fuse_core/test/example_variable.hpp b/fuse_core/test/example_variable.hpp index 95ea87a6e..a0e6b80c0 100644 --- a/fuse_core/test/example_variable.hpp +++ b/fuse_core/test/example_variable.hpp @@ -53,16 +53,25 @@ class ExampleVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(ExampleVariable) - ExampleVariable() - : fuse_core::Variable(fuse_core::uuid::generate()), - data_(0.0) + ExampleVariable() : fuse_core::Variable(fuse_core::uuid::generate()), data_(0.0) { } - size_t size() const override {return 1;} - const double * data() const override {return &data_;} - double * data() override {return &data_;} - void print(std::ostream & /*stream = std::cout*/) const override {} + size_t size() const override + { + return 1; + } + const double* data() const override + { + return &data_; + } + double* data() override + { + return &data_; + } + void print(std::ostream& /*stream = std::cout*/) const override + { + } #if CERES_SUPPORTS_MANIFOLDS /** @@ -70,7 +79,10 @@ class ExampleVariable : public fuse_core::Variable * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -86,11 +98,11 @@ class ExampleVariable : public fuse_core::Variable * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; @@ -102,11 +114,17 @@ class LegacyParameterization : public fuse_core::LocalParameterization public: FUSE_SMART_PTR_DEFINITIONS(LegacyParameterization) - int GlobalSize() const override {return 4;} + int GlobalSize() const override + { + return 4; + } - int LocalSize() const override {return 3;} + int LocalSize() const override + { + return 3; + } - bool Plus(const double * x, const double * delta, double * x_plus_delta) const override + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { double q_delta[4]; ceres::AngleAxisToQuaternion(delta, q_delta); @@ -114,22 +132,30 @@ class LegacyParameterization : public fuse_core::LocalParameterization return true; } - bool ComputeJacobian(const double * x, double * jacobian) const override + bool ComputeJacobian(const double* x, double* jacobian) const override { double x0 = x[0] / 2; double x1 = x[1] / 2; double x2 = x[2] / 2; double x3 = x[3] / 2; /* *INDENT-OFF* */ - jacobian[0] = -x1; jacobian[1] = -x2; jacobian[2] = -x3; // NOLINT - jacobian[3] = x0; jacobian[4] = -x3; jacobian[5] = x2; // NOLINT - jacobian[6] = x3; jacobian[7] = x0; jacobian[8] = -x1; // NOLINT - jacobian[9] = -x2; jacobian[10] = x1; jacobian[11] = x0; // NOLINT + jacobian[0] = -x1; + jacobian[1] = -x2; + jacobian[2] = -x3; // NOLINT + jacobian[3] = x0; + jacobian[4] = -x3; + jacobian[5] = x2; // NOLINT + jacobian[6] = x3; + jacobian[7] = x0; + jacobian[8] = -x1; // NOLINT + jacobian[9] = -x2; + jacobian[10] = x1; + jacobian[11] = x0; // NOLINT /* *INDENT-ON* */ return true; } - bool Minus(const double * x, const double * y, double * y_minus_x) const override + bool Minus(const double* x, const double* y, double* y_minus_x) const override { double x_inverse[4]; x_inverse[0] = x[0]; @@ -143,16 +169,25 @@ class LegacyParameterization : public fuse_core::LocalParameterization return true; } - bool ComputeMinusJacobian(const double * x, double * jacobian) const override + bool ComputeMinusJacobian(const double* x, double* jacobian) const override { double x0 = x[0] * 2; double x1 = x[1] * 2; double x2 = x[2] * 2; double x3 = x[3] * 2; /* *INDENT-OFF* */ - jacobian[0] = -x1; jacobian[1] = x0; jacobian[2] = x3; jacobian[3] = -x2; // NOLINT - jacobian[4] = -x2; jacobian[5] = -x3; jacobian[6] = x0; jacobian[7] = x1; // NOLINT - jacobian[8] = -x3; jacobian[9] = x2; jacobian[10] = -x1; jacobian[11] = x0; // NOLINT + jacobian[0] = -x1; + jacobian[1] = x0; + jacobian[2] = x3; + jacobian[3] = -x2; // NOLINT + jacobian[4] = -x2; + jacobian[5] = -x3; + jacobian[6] = x0; + jacobian[7] = x1; // NOLINT + jacobian[8] = -x3; + jacobian[9] = x2; + jacobian[10] = -x1; + jacobian[11] = x0; // NOLINT /* *INDENT-ON* */ return true; } @@ -168,10 +203,10 @@ class LegacyParameterization : public fuse_core::LocalParameterization * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; @@ -180,16 +215,25 @@ class LegacyVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(LegacyVariable); - LegacyVariable() - : fuse_core::Variable(fuse_core::uuid::generate()), - data_{1.0, 0.0, 0.0, 0.0} + LegacyVariable() : fuse_core::Variable(fuse_core::uuid::generate()), data_{ 1.0, 0.0, 0.0, 0.0 } { } - size_t size() const override {return 4;} - const double * data() const override {return data_;} - double * data() override {return data_;} - void print(std::ostream & /*stream = std::cout*/) const override {} + size_t size() const override + { + return 4; + } + const double* data() const override + { + return data_; + } + double* data() override + { + return data_; + } + void print(std::ostream& /*stream = std::cout*/) const override + { + } /** * @brief Returns the number of elements of the local parameterization space. @@ -197,14 +241,17 @@ class LegacyVariable : public fuse_core::Variable * While a quaternion has 4 parameters, a 3D rotation only has 3 degrees of freedom. Hence, the local * parameterization space is only size 3. */ - size_t localSize() const override {return 3u;} + size_t localSize() const override + { + return 3u; + } /** * @brief Provides a Ceres local parameterization for the quaternion * * @return A pointer to a local parameterization object that indicates how to "add" increments to the quaternion */ - fuse_core::LocalParameterization * localParameterization() const override + fuse_core::LocalParameterization* localParameterization() const override { return new LegacyParameterization(); } @@ -221,11 +268,11 @@ class LegacyVariable : public fuse_core::Variable * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; diff --git a/fuse_core/test/launch_tests/test_parameters.cpp b/fuse_core/test/launch_tests/test_parameters.cpp index 36da20160..17197b019 100644 --- a/fuse_core/test/launch_tests/test_parameters.cpp +++ b/fuse_core/test/launch_tests/test_parameters.cpp @@ -46,16 +46,14 @@ class TestParameters : public ::testing::Test void SetUp() override { executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); @@ -68,34 +66,34 @@ class TestParameters : public ::testing::Test TEST_F(TestParameters, getPositiveParam) { // Load parameters enforcing they are positive: - const double default_value{1.0}; + const double default_value{ 1.0 }; auto node = rclcpp::Node::make_shared("test_parameters_node"); // Load a positive parameter: { - double parameter{default_value}; + double parameter{ default_value }; fuse_core::getPositiveParam(*node, "positive_parameter", parameter); EXPECT_EQ(3.0, parameter); } // Load a negative parameter: { - double parameter{default_value}; + double parameter{ default_value }; fuse_core::getPositiveParam(*node, "negative_parameter", parameter); EXPECT_EQ(default_value, parameter); } // Load a zero parameter: { - double parameter{default_value}; + double parameter{ default_value }; fuse_core::getPositiveParam(*node, "zero_parameter", parameter); EXPECT_EQ(default_value, parameter); } // Load a zero parameter allowing zero (not strict): { - double parameter{default_value}; + double parameter{ default_value }; fuse_core::getPositiveParam(*node, "zero_parameter", parameter, false); EXPECT_EQ(0.0, parameter); } @@ -119,91 +117,91 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) // Load covariance matrix diagonal from the parameter server: // A covariance diagonal with the expected size and valid should be the same as the expected one: { - const std::string parameter_name{"covariance_diagonal"}; + const std::string parameter_name{ "covariance_diagonal" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - try { - const auto covariance = - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance); + try + { + const auto covariance = fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance); EXPECT_EQ(Size, covariance.rows()); EXPECT_EQ(Size, covariance.cols()); - EXPECT_EQ( - expected_covariance.rows() * expected_covariance.cols(), - expected_covariance.cwiseEqual(covariance).count()) - << "Expected\n" << expected_covariance << "\nActual\n" << covariance; - } catch (const std::exception & ex) { + EXPECT_EQ(expected_covariance.rows() * expected_covariance.cols(), + expected_covariance.cwiseEqual(covariance).count()) + << "Expected\n" + << expected_covariance << "\nActual\n" + << covariance; + } + catch (const std::exception& ex) + { FAIL() << "Failed to get " << parameter_name.c_str() << ": " << ex.what(); } } // If the parameter does not exist we should get the default covariance: { - const std::string parameter_name{"non_existent_parameter"}; + const std::string parameter_name{ "non_existent_parameter" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - try { - const auto covariance = - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance); + try + { + const auto covariance = fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance); EXPECT_EQ(Size, covariance.rows()); EXPECT_EQ(Size, covariance.cols()); - EXPECT_EQ( - default_covariance.rows() * default_covariance.cols(), - default_covariance.cwiseEqual(covariance).count()) - << "Expected\n" << default_covariance << "\nActual\n" << covariance; - } catch (const std::exception & ex) { + EXPECT_EQ(default_covariance.rows() * default_covariance.cols(), default_covariance.cwiseEqual(covariance).count()) + << "Expected\n" + << default_covariance << "\nActual\n" + << covariance; + } + catch (const std::exception& ex) + { FAIL() << "Failed to get " << parameter_name.c_str() << ": " << ex.what(); } } // A covariance diagonal with negative values should throw std::invalid_argument: { - const std::string parameter_name{"covariance_diagonal_with_negative_values"}; + const std::string parameter_name{ "covariance_diagonal_with_negative_values" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW( - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), - std::invalid_argument); + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + std::invalid_argument); } // A covariance diagonal with size 2, smaller than expected, should throw std::invalid_argument: { - const std::string parameter_name{"covariance_diagonal_with_size_2"}; + const std::string parameter_name{ "covariance_diagonal_with_size_2" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW( - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), - std::invalid_argument); + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + std::invalid_argument); } // A covariance diagonal with size 4, larger than expected, should throw std::invalid_argument: { - const std::string parameter_name{"covariance_diagonal_with_size_4"}; + const std::string parameter_name{ "covariance_diagonal_with_size_4" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW( - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), - std::invalid_argument); + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + std::invalid_argument); } - // A covariance diagonal with an invalid element should throw // rclcpp::exceptions::InvalidParameterTypeException: { - const std::string parameter_name{"covariance_diagonal_with_strings"}; + const std::string parameter_name{ "covariance_diagonal_with_strings" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW( - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), - rclcpp::exceptions::InvalidParameterTypeException); + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + rclcpp::exceptions::InvalidParameterTypeException); // NOTE(CH3): A covariance diagonal with invalid element type used to not throw, and used to // instead get substituted with default covariance. But now with strongly typed @@ -216,12 +214,11 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) // A covariance diagonal with an invalid element should throw // rclcpp::exceptions::InvalidParameterTypeException: { - const std::string parameter_name{"covariance_diagonal_with_string"}; + const std::string parameter_name{ "covariance_diagonal_with_string" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW( - fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), - rclcpp::exceptions::InvalidParameterTypeException); + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + rclcpp::exceptions::InvalidParameterTypeException); // NOTE(CH3): A covariance diagonal with invalid element type used to not throw, and used to // instead get substituted with default covariance. But now with strongly typed @@ -232,9 +229,8 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) } } - // NOTE(CH3): This main is required because the test is manually run by a launch test -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); diff --git a/fuse_core/test/launch_tests/test_parameters.py b/fuse_core/test/launch_tests/test_parameters.py index 10a000721..d7b00eae5 100755 --- a/fuse_core/test/launch_tests/test_parameters.py +++ b/fuse_core/test/launch_tests/test_parameters.py @@ -28,13 +28,13 @@ @pytest.fixture def test_proc(): - test_root = '.' + test_root = "." - param_path = os.path.join(test_root, 'launch_tests', 'test_parameters.yaml') - test_path = os.path.join(test_root, 'test_parameters') + param_path = os.path.join(test_root, "launch_tests", "test_parameters.yaml") + test_path = os.path.join(test_root, "test_parameters") - cmd = [test_path, '--ros-args', '--params-file', param_path] - return ExecuteProcess(cmd=cmd, shell=True, output='screen', cached_output=True) + cmd = [test_path, "--ros-args", "--params-file", param_path] + return ExecuteProcess(cmd=cmd, shell=True, output="screen", cached_output=True) @launch_pytest.fixture @@ -45,4 +45,4 @@ def generate_test_description(test_proc): @pytest.mark.launch(fixture=generate_test_description) async def test_no_failed_gtests(test_proc, launch_context): await process_tools.wait_for_exit(launch_context, test_proc, timeout=10) - assert test_proc.return_code == 0, 'GTests failed' + assert test_proc.return_code == 0, "GTests failed" diff --git a/fuse_core/test/test_async_motion_model.cpp b/fuse_core/test/test_async_motion_model.cpp index 3e60abc15..3fb601983 100644 --- a/fuse_core/test/test_async_motion_model.cpp +++ b/fuse_core/test/test_async_motion_model.cpp @@ -42,15 +42,13 @@ class MyMotionModel : public fuse_core::AsyncMotionModel { public: - MyMotionModel() - : fuse_core::AsyncMotionModel(1), - initialized(false) + MyMotionModel() : fuse_core::AsyncMotionModel(1), initialized(false) { } virtual ~MyMotionModel() = default; - bool applyCallback(fuse_core::Transaction & /*transaction*/) + bool applyCallback(fuse_core::Transaction& /*transaction*/) { rclcpp::sleep_for(std::chrono::milliseconds(1000)); transaction_received = true; @@ -89,7 +87,8 @@ class TestAsyncMotionModel : public ::testing::Test TEST_F(TestAsyncMotionModel, OnInit) { - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) + { MyMotionModel motion_model; auto node = rclcpp::Node::make_shared("test_async_motion_model_node"); motion_model.initialize(*node, "my_motion_model_" + std::to_string(i)); @@ -120,13 +119,15 @@ TEST_F(TestAsyncMotionModel, OnGraphUpdate) auto clock = rclcpp::Clock(RCL_SYSTEM_TIME); // Test for multiple cycles of graphCallback to be sure - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) + { motion_model.graph_received = false; motion_model.graphCallback(graph); EXPECT_FALSE(motion_model.graph_received); rclcpp::Time wait_time_elapsed = clock.now() + rclcpp::Duration::from_seconds(10); - while (!motion_model.graph_received && clock.now() < wait_time_elapsed) { + while (!motion_model.graph_received && clock.now() < wait_time_elapsed) + { rclcpp::sleep_for(std::chrono::milliseconds(10)); } EXPECT_TRUE(motion_model.graph_received); diff --git a/fuse_core/test/test_async_publisher.cpp b/fuse_core/test/test_async_publisher.cpp index c5dc42b34..fb3cf3e20 100644 --- a/fuse_core/test/test_async_publisher.cpp +++ b/fuse_core/test/test_async_publisher.cpp @@ -44,18 +44,13 @@ class MyPublisher : public fuse_core::AsyncPublisher { public: - MyPublisher() - : fuse_core::AsyncPublisher(1), - callback_processed(false), - initialized(false) + MyPublisher() : fuse_core::AsyncPublisher(1), callback_processed(false), initialized(false) { } virtual ~MyPublisher() = default; - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr /*transaction*/, - fuse_core::Graph::ConstSharedPtr /*graph*/) + void notifyCallback(fuse_core::Transaction::ConstSharedPtr /*transaction*/, fuse_core::Graph::ConstSharedPtr /*graph*/) { rclcpp::sleep_for(std::chrono::milliseconds(10)); callback_processed = true; @@ -86,7 +81,8 @@ class TestAsyncPublisher : public ::testing::Test TEST_F(TestAsyncPublisher, OnInit) { - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) + { auto node = rclcpp::Node::make_shared("test_async_pub_node"); MyPublisher publisher; publisher.initialize(*node, "my_publisher_" + std::to_string(i)); @@ -114,17 +110,19 @@ TEST_F(TestAsyncPublisher, notifyCallback) // MyPublisher's async spinner. There is a time delay there. So, this call should return almost // immediately, then we have to wait a bit before the "callback_processed" flag gets flipped. fuse_core::Transaction::ConstSharedPtr transaction; // nullptr is ok as we don't actually use it - fuse_core::Graph::ConstSharedPtr graph; // nullptr is ok as we don't actually use it + fuse_core::Graph::ConstSharedPtr graph; // nullptr is ok as we don't actually use it auto clock = rclcpp::Clock(RCL_SYSTEM_TIME); // Test for multiple cycles of notify to be sure - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) + { publisher.callback_processed = false; publisher.notify(transaction, graph); EXPECT_FALSE(publisher.callback_processed); rclcpp::Time wait_time_elapsed = clock.now() + rclcpp::Duration::from_seconds(10); - while (!publisher.callback_processed && clock.now() < wait_time_elapsed) { + while (!publisher.callback_processed && clock.now() < wait_time_elapsed) + { rclcpp::sleep_for(std::chrono::milliseconds(10)); } EXPECT_TRUE(publisher.callback_processed); diff --git a/fuse_core/test/test_async_sensor_model.cpp b/fuse_core/test/test_async_sensor_model.cpp index f19d9c5fe..52b70cbf6 100644 --- a/fuse_core/test/test_async_sensor_model.cpp +++ b/fuse_core/test/test_async_sensor_model.cpp @@ -56,9 +56,7 @@ void transactionCallback(fuse_core::Transaction::SharedPtr /*transaction*/) class MySensor : public fuse_core::AsyncSensorModel { public: - MySensor() - : fuse_core::AsyncSensorModel(1), - initialized(false) + MySensor() : fuse_core::AsyncSensorModel(1), initialized(false) { } @@ -95,7 +93,8 @@ class TestAsyncSensorModel : public ::testing::Test TEST_F(TestAsyncSensorModel, OnInit) { - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) + { MySensor sensor; auto node = rclcpp::Node::make_shared("test_async_sensor_model_node"); sensor.initialize(*node, "my_sensor_" + std::to_string(i), &transactionCallback); @@ -126,12 +125,14 @@ TEST_F(TestAsyncSensorModel, OnGraphUpdate) auto clock = rclcpp::Clock(RCL_SYSTEM_TIME); // Test for multiple cycles of graphCallback to be sure - for (int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) + { sensor.graph_received = false; sensor.graphCallback(graph); EXPECT_FALSE(sensor.graph_received); rclcpp::Time wait_time_elapsed = clock.now() + rclcpp::Duration::from_seconds(10); - while (!sensor.graph_received && clock.now() < wait_time_elapsed) { + while (!sensor.graph_received && clock.now() < wait_time_elapsed) + { rclcpp::sleep_for(std::chrono::milliseconds(10)); } EXPECT_TRUE(sensor.graph_received); diff --git a/fuse_core/test/test_callback_wrapper.cpp b/fuse_core/test/test_callback_wrapper.cpp index 9bc4932d3..502ff2491 100644 --- a/fuse_core/test/test_callback_wrapper.cpp +++ b/fuse_core/test/test_callback_wrapper.cpp @@ -44,12 +44,12 @@ class MyClass { public: - double processData(const std::vector & data) + double processData(const std::vector& data) { return std::accumulate(data.begin(), data.end(), 0.0); } - void processDataInPlace(const std::vector & data, double & output) + void processDataInPlace(const std::vector& data, double& output) { output = std::accumulate(data.begin(), data.end(), 0.0); } @@ -72,16 +72,14 @@ class TestCallbackWrapper : public ::testing::Test TEST_F(TestCallbackWrapper, Double) { MyClass my_object; - std::vector data = {1.0, 2.0, 3.0, 4.0, 5.0}; + std::vector data = { 1.0, 2.0, 3.0, 4.0, 5.0 }; auto node = rclcpp::Node::make_shared("callback_wrapper_double_test_node"); - auto callback_queue = - std::make_shared(node->get_node_base_interface()->get_context()); - node->get_node_waitables_interface()->add_waitable( - callback_queue, (rclcpp::CallbackGroup::SharedPtr) nullptr); + auto callback_queue = std::make_shared(node->get_node_base_interface()->get_context()); + node->get_node_waitables_interface()->add_waitable(callback_queue, (rclcpp::CallbackGroup::SharedPtr) nullptr); auto callback = std::make_shared>( - std::bind(&MyClass::processData, &my_object, std::ref(data))); + std::bind(&MyClass::processData, &my_object, std::ref(data))); auto result = callback->getFuture(); callback_queue->addCallback(callback); @@ -95,17 +93,15 @@ TEST_F(TestCallbackWrapper, Double) TEST_F(TestCallbackWrapper, Void) { MyClass my_object; - std::vector data = {1.0, 2.0, 3.0, 4.0, 5.0}; + std::vector data = { 1.0, 2.0, 3.0, 4.0, 5.0 }; double output; auto node = rclcpp::Node::make_shared("callback_wrapper_void_test_node"); - auto callback_queue = - std::make_shared(node->get_node_base_interface()->get_context()); - node->get_node_waitables_interface()->add_waitable( - callback_queue, (rclcpp::CallbackGroup::SharedPtr) nullptr); + auto callback_queue = std::make_shared(node->get_node_base_interface()->get_context()); + node->get_node_waitables_interface()->add_waitable(callback_queue, (rclcpp::CallbackGroup::SharedPtr) nullptr); auto callback = std::make_shared>( - std::bind(&MyClass::processDataInPlace, &my_object, std::ref(data), std::ref(output))); + std::bind(&MyClass::processDataInPlace, &my_object, std::ref(data), std::ref(output))); auto result = callback->getFuture(); callback_queue->addCallback(callback); diff --git a/fuse_core/test/test_constraint.cpp b/fuse_core/test/test_constraint.cpp index b278c2153..39e6c4cc8 100644 --- a/fuse_core/test/test_constraint.cpp +++ b/fuse_core/test/test_constraint.cpp @@ -43,7 +43,7 @@ TEST(Constraint, Constructor) // Create a constraint with a single UUID { fuse_core::UUID variable_uuid1 = fuse_core::uuid::generate(); - ExampleConstraint constraint("test", {variable_uuid1}); // NOLINT + ExampleConstraint constraint("test", { variable_uuid1 }); // NOLINT ASSERT_EQ(1u, constraint.variables().size()); ASSERT_EQ(variable_uuid1, constraint.variables().at(0)); } @@ -52,7 +52,7 @@ TEST(Constraint, Constructor) fuse_core::UUID variable_uuid1 = fuse_core::uuid::generate(); fuse_core::UUID variable_uuid2 = fuse_core::uuid::generate(); fuse_core::UUID variable_uuid3 = fuse_core::uuid::generate(); - ExampleConstraint constraint("test", {variable_uuid1, variable_uuid2, variable_uuid3}); // NOLINT + ExampleConstraint constraint("test", { variable_uuid1, variable_uuid2, variable_uuid3 }); // NOLINT ASSERT_EQ(3u, constraint.variables().size()); ASSERT_EQ(variable_uuid1, constraint.variables().at(0)); ASSERT_EQ(variable_uuid2, constraint.variables().at(1)); @@ -67,7 +67,8 @@ TEST(Constraint, Constructor) variable_uuids.push_back(fuse_core::uuid::generate()); ExampleConstraint constraint("test", variable_uuids.begin(), variable_uuids.end()); ASSERT_EQ(variable_uuids.size(), constraint.variables().size()); - for (size_t i = 0; i < variable_uuids.size(); ++i) { + for (size_t i = 0; i < variable_uuids.size(); ++i) + { ASSERT_EQ(variable_uuids.at(i), constraint.variables().at(i)); } } @@ -76,12 +77,13 @@ TEST(Constraint, Constructor) fuse_core::UUID variable_uuid1 = fuse_core::uuid::generate(); fuse_core::UUID variable_uuid2 = fuse_core::uuid::generate(); fuse_core::UUID variable_uuid3 = fuse_core::uuid::generate(); - ExampleConstraint constraint1("test", {variable_uuid1, variable_uuid2, variable_uuid3}); // NOLINT + ExampleConstraint constraint1("test", { variable_uuid1, variable_uuid2, variable_uuid3 }); // NOLINT ExampleConstraint constraint2(constraint1); ASSERT_EQ(constraint1.uuid(), constraint2.uuid()); ASSERT_EQ(constraint1.variables().size(), constraint2.variables().size()); - for (size_t i = 0; i < constraint1.variables().size(); ++i) { + for (size_t i = 0; i < constraint1.variables().size(); ++i) + { ASSERT_EQ(constraint1.variables().at(i), constraint2.variables().at(i)); } } @@ -90,6 +92,6 @@ TEST(Constraint, Constructor) TEST(Constraint, Type) { fuse_core::UUID variable_uuid1 = fuse_core::uuid::generate(); - ExampleConstraint constraint("test", {variable_uuid1}); // NOLINT + ExampleConstraint constraint("test", { variable_uuid1 }); // NOLINT ASSERT_EQ("ExampleConstraint", constraint.type()); } diff --git a/fuse_core/test/test_eigen.cpp b/fuse_core/test/test_eigen.cpp index c7b2ff6db..f28c9679a 100644 --- a/fuse_core/test/test_eigen.cpp +++ b/fuse_core/test/test_eigen.cpp @@ -43,8 +43,7 @@ TEST(Eigen, isSymmetric) const auto symmetric_matrix = (0.5 * (random_matrix + random_matrix.transpose())).eval(); EXPECT_TRUE(fuse_core::isSymmetric(symmetric_matrix)) << "Matrix\n" - << symmetric_matrix << - "\n expected to be symmetric."; + << symmetric_matrix << "\n expected to be symmetric."; // A non-symmetric matrix: const double asymmetry_error = 1.0e-6; @@ -53,15 +52,15 @@ TEST(Eigen, isSymmetric) non_symmetric_matrix(0, 1) += asymmetry_error; EXPECT_FALSE(fuse_core::isSymmetric(non_symmetric_matrix)) - << "Matrix\n" - << non_symmetric_matrix << "\n expected to not be symmetric."; + << "Matrix\n" + << non_symmetric_matrix << "\n expected to not be symmetric."; // Checking symmetry with precision larger than asymmetry error in non-symmetric matrix: const double precision = 1.0e2 * asymmetry_error; EXPECT_TRUE(fuse_core::isSymmetric(non_symmetric_matrix, precision)) - << "Matrix\n" - << non_symmetric_matrix << "\n expected to be symmetric with precision " << precision << "."; + << "Matrix\n" + << non_symmetric_matrix << "\n expected to be symmetric with precision " << precision << "."; // fuse_core::isSymmetric is not defined for non-square matrices. The following will simply fail // to compile because it is not allowed, as intended: @@ -80,16 +79,15 @@ TEST(Eigen, isPositiveDefinite) const auto psd_matrix = (symmetric_matrix + 3 * fuse_core::Matrix3d::Identity()).eval(); EXPECT_TRUE(fuse_core::isPositiveDefinite(psd_matrix)) << "Matrix\n" - << psd_matrix << - "\n expected to be Positive Definite."; + << psd_matrix << "\n expected to be Positive Definite."; // A non Positive Definite matrix: auto non_psd_matrix = psd_matrix; non_psd_matrix(0, 0) *= -1.0; EXPECT_FALSE(fuse_core::isPositiveDefinite(non_psd_matrix)) - << "Matrix\n" - << non_psd_matrix << "\n expected to not be Positive Definite."; + << "Matrix\n" + << non_psd_matrix << "\n expected to not be Positive Definite."; // fuse_core::isPositiveDefinite is not defined for non-square matrices. The following will simply // fail to compile because it is allowed, as intended: diff --git a/fuse_core/test/test_local_parameterization.cpp b/fuse_core/test/test_local_parameterization.cpp index 29273ed58..bf5c7185e 100644 --- a/fuse_core/test/test_local_parameterization.cpp +++ b/fuse_core/test/test_local_parameterization.cpp @@ -42,8 +42,8 @@ struct Plus { - template - bool operator()(const T * x, const T * delta, T * x_plus_delta) const + template + bool operator()(const T* x, const T* delta, T* x_plus_delta) const { x_plus_delta[0] = x[0] + 2.0 * delta[0]; x_plus_delta[1] = x[1] + 5.0 * delta[1]; @@ -54,8 +54,8 @@ struct Plus struct Minus { - template - bool operator()(const T * x, const T * y, T * y_minus_x) const + template + bool operator()(const T* x, const T* y, T* y_minus_x) const { y_minus_x[0] = (y[0] - x[0]) / 2.0; y_minus_x[1] = (y[1] - x[1]) / 5.0; @@ -65,14 +65,13 @@ struct Minus using TestLocalParameterization = fuse_core::AutoDiffLocalParameterization; - TEST(LocalParameterization, Plus) { TestLocalParameterization parameterization; - double x[3] = {1.0, 2.0, 3.0}; - double delta[2] = {0.5, 1.0}; - double actual[3] = {0.0, 0.0, 0.0}; + double x[3] = { 1.0, 2.0, 3.0 }; + double delta[2] = { 0.5, 1.0 }; + double actual[3] = { 0.0, 0.0, 0.0 }; bool success = parameterization.Plus(x, delta, actual); EXPECT_TRUE(success); @@ -85,16 +84,14 @@ TEST(LocalParameterization, PlusJacobian) { TestLocalParameterization parameterization; - double x[3] = {1.0, 2.0, 3.0}; + double x[3] = { 1.0, 2.0, 3.0 }; fuse_core::MatrixXd actual(3, 2); bool success = parameterization.ComputeJacobian(x, actual.data()); fuse_core::MatrixXd expected(3, 2); /* *INDENT-OFF* */ // Bypass uncrustify - expected << 2.0, 0.0, - 0.0, 5.0, - 0.0, 0.0; + expected << 2.0, 0.0, 0.0, 5.0, 0.0, 0.0; /* *INDENT-ON* */ EXPECT_TRUE(success); @@ -105,9 +102,9 @@ TEST(LocalParameterization, Minus) { TestLocalParameterization parameterization; - double x1[3] = {1.0, 2.0, 3.0}; - double x2[3] = {2.0, 7.0, 3.0}; - double actual[2] = {0.0, 0.0}; + double x1[3] = { 1.0, 2.0, 3.0 }; + double x2[3] = { 2.0, 7.0, 3.0 }; + double actual[2] = { 0.0, 0.0 }; bool success = parameterization.Minus(x1, x2, actual); EXPECT_TRUE(success); @@ -119,15 +116,14 @@ TEST(LocalParameterization, MinusJacobian) { TestLocalParameterization parameterization; - double x[3] = {1.0, 2.0, 3.0}; + double x[3] = { 1.0, 2.0, 3.0 }; fuse_core::MatrixXd actual(2, 3); bool success = parameterization.ComputeMinusJacobian(x, actual.data()); fuse_core::MatrixXd expected(2, 3); /* *INDENT-OFF* */ // Bypass uncrustify - expected << 0.5, 0.0, 0.0, - 0.0, 0.2, 0.0; + expected << 0.5, 0.0, 0.0, 0.0, 0.2, 0.0; /* *INDENT-ON* */ EXPECT_TRUE(success); @@ -138,8 +134,8 @@ TEST(LocalParameterization, MinusSameVariablesIsZero) { TestLocalParameterization parameterization; - double x1[3] = {1.0, 2.0, 3.0}; - double actual[2] = {0.0, 0.0}; + double x1[3] = { 1.0, 2.0, 3.0 }; + double actual[2] = { 0.0, 0.0 }; bool success = parameterization.Minus(x1, x1, actual); EXPECT_TRUE(success); @@ -151,14 +147,14 @@ TEST(LocalParameterization, PlusMinus) { TestLocalParameterization parameterization; - const double x1[3] = {1.0, 2.0, 3.0}; - const double delta[2] = {0.5, 1.0}; - double x2[3] = {0.0, 0.0, 0.0}; + const double x1[3] = { 1.0, 2.0, 3.0 }; + const double delta[2] = { 0.5, 1.0 }; + double x2[3] = { 0.0, 0.0, 0.0 }; bool success = parameterization.Plus(x1, delta, x2); ASSERT_TRUE(success); - double actual[2] = {0.0, 0.0}; + double actual[2] = { 0.0, 0.0 }; success = parameterization.Minus(x1, x2, actual); EXPECT_TRUE(success); diff --git a/fuse_core/test/test_loss.cpp b/fuse_core/test/test_loss.cpp index 7992979d0..4628836ee 100644 --- a/fuse_core/test/test_loss.cpp +++ b/fuse_core/test/test_loss.cpp @@ -37,7 +37,7 @@ TEST(Loss, Constructor) { - const double a{0.3}; + const double a{ 0.3 }; ExampleLoss loss(a); ASSERT_EQ(a, loss.a); @@ -45,7 +45,8 @@ TEST(Loss, Constructor) ASSERT_NE(nullptr, loss_function); - if (fuse_core::Loss::Ownership == ceres::Ownership::TAKE_OWNERSHIP) { + if (fuse_core::Loss::Ownership == ceres::Ownership::TAKE_OWNERSHIP) + { delete loss_function; } } diff --git a/fuse_core/test/test_message_buffer.cpp b/fuse_core/test/test_message_buffer.cpp index 33ab74e77..65a430e3a 100644 --- a/fuse_core/test/test_message_buffer.cpp +++ b/fuse_core/test/test_message_buffer.cpp @@ -43,8 +43,7 @@ class MessageBufferTestFixture : public ::testing::Test { public: - MessageBufferTestFixture() - : buffer(rclcpp::Duration::max()) + MessageBufferTestFixture() : buffer(rclcpp::Duration::max()) { } @@ -63,9 +62,7 @@ class MessageBufferTestFixture : public ::testing::Test TEST_F(MessageBufferTestFixture, Exceptions) { // Call the query with the parameters in the wrong order. This should throw. - EXPECT_THROW( - buffer.query(rclcpp::Time(20, 0), rclcpp::Time(10, 0), false), - std::invalid_argument); + EXPECT_THROW(buffer.query(rclcpp::Time(20, 0), rclcpp::Time(10, 0), false), std::invalid_argument); // Call the query when the buffer is empty. This should throw. EXPECT_THROW(buffer.query(rclcpp::Time(10, 0), rclcpp::Time(25, 0), false), std::out_of_range); diff --git a/fuse_core/test/test_parameter.cpp b/fuse_core/test/test_parameter.cpp index bc334ede5..3013c89fe 100644 --- a/fuse_core/test/test_parameter.cpp +++ b/fuse_core/test/test_parameter.cpp @@ -42,53 +42,41 @@ TEST(parameter, list_parameter_override_prefixes) { const std::map overrides = { - {"foo", {}}, - {"foo.baz", {}}, - {"foo.bar", {}}, - {"foo.bar.baz", {}}, - {"foo.bar.baz.bing", {}}, - {"foobar.baz", {}}, - {"baz", {}} + { "foo", {} }, { "foo.baz", {} }, { "foo.bar", {} }, { "foo.bar.baz", {} }, { "foo.bar.baz.bing", {} }, + { "foobar.baz", {} }, { "baz", {} } }; { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, "foo"); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, "foo"); EXPECT_EQ(2u, matches.size()); EXPECT_NE(matches.end(), matches.find("foo.baz")); EXPECT_NE(matches.end(), matches.find("foo.bar")); } { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, "foo.bar"); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, "foo.bar"); EXPECT_EQ(1u, matches.size()); EXPECT_NE(matches.end(), matches.find("foo.bar.baz")); } { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, "foo.bar.baz"); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, "foo.bar.baz"); EXPECT_EQ(1u, matches.size()); EXPECT_NE(matches.end(), matches.find("foo.bar.baz.bing")); } { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, "foo.baz"); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, "foo.baz"); EXPECT_EQ(0u, matches.size()); } { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, "foobar"); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, "foobar"); EXPECT_EQ(1u, matches.size()); EXPECT_NE(matches.end(), matches.find("foobar.baz")); } { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, "dne"); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, "done"); EXPECT_EQ(0u, matches.size()); } { - auto matches = fuse_core::detail::list_parameter_override_prefixes( - overrides, ""); + auto matches = fuse_core::detail::list_parameter_override_prefixes(overrides, ""); EXPECT_EQ(3u, matches.size()); EXPECT_NE(matches.end(), matches.find("foo")); EXPECT_NE(matches.end(), matches.find("foobar")); diff --git a/fuse_core/test/test_throttled_callback.cpp b/fuse_core/test/test_throttled_callback.cpp index da1ccefa3..844e9472a 100644 --- a/fuse_core/test/test_throttled_callback.cpp +++ b/fuse_core/test/test_throttled_callback.cpp @@ -52,9 +52,7 @@ class PointPublisher : public rclcpp::Node * * @param[in] frequency The publishing frequency in Hz */ - explicit PointPublisher(const double frequency) - : Node("point_publisher_node") - , frequency_(frequency) + explicit PointPublisher(const double frequency) : Node("point_publisher_node"), frequency_(frequency) { publisher_ = this->create_publisher("point", 1); } @@ -78,7 +76,8 @@ class PointPublisher : public rclcpp::Node { // Wait for the subscriptions to be ready before sending them data: rclcpp::Time subscription_timeout = this->now() + rclcpp::Duration::from_seconds(1.0); - while (publisher_->get_subscription_count() < 1u && this->now() < subscription_timeout) { + while (publisher_->get_subscription_count() < 1u && this->now() < subscription_timeout) + { rclcpp::sleep_for(std::chrono::milliseconds(10)); } @@ -86,7 +85,8 @@ class PointPublisher : public rclcpp::Node // Send data: rclcpp::Rate rate(frequency_); - for (size_t i = 0; i < num_messages; ++i) { + for (size_t i = 0; i < num_messages; ++i) + { geometry_msgs::msg::Point point_message; point_message.x = i; publisher_->publish(point_message); @@ -96,7 +96,7 @@ class PointPublisher : public rclcpp::Node private: rclcpp::Publisher::SharedPtr publisher_; //!< The publisher - double frequency_{10.0}; //!< The publish rate frequency + double frequency_{ 10.0 }; //!< The publish rate frequency }; /** @@ -115,20 +115,15 @@ class PointSensorModel : public rclcpp::Node * * @param[in] throttle_period The throttle period duration in seconds */ - explicit PointSensorModel(const rclcpp::Duration & throttle_period) - : Node("point_sensor_model_node") - , throttled_callback_( - std::bind(&PointSensorModel::keepCallback, this, std::placeholders::_1), - std::bind(&PointSensorModel::dropCallback, this, std::placeholders::_1), - throttle_period - ) + explicit PointSensorModel(const rclcpp::Duration& throttle_period) + : Node("point_sensor_model_node") + , throttled_callback_(std::bind(&PointSensorModel::keepCallback, this, std::placeholders::_1), + std::bind(&PointSensorModel::dropCallback, this, std::placeholders::_1), throttle_period) { subscription_ = this->create_subscription( - "point", 10, - std::bind( - &PointThrottledCallback::callback, - &throttled_callback_, std::placeholders::_1) - ); + "point", 10, + std::bind(&PointThrottledCallback::callback, &throttled_callback_, + std::placeholders::_1)); } /** @@ -178,7 +173,7 @@ class PointSensorModel : public rclcpp::Node * * @param[in] msg A geometry_msgs::msg::Point message */ - void keepCallback(const geometry_msgs::msg::Point & msg) + void keepCallback(const geometry_msgs::msg::Point& msg) { ++kept_messages_; last_kept_message_ = std::make_shared(msg); @@ -190,18 +185,18 @@ class PointSensorModel : public rclcpp::Node * @param[in] msg A geometry_msgs::msg::Point message (not used) */ // NOTE(CH3): The msg arg here is necessary to allow binding the throttled callback - void dropCallback(const geometry_msgs::msg::Point & /*msg*/) + void dropCallback(const geometry_msgs::msg::Point& /*msg*/) { ++dropped_messages_; } - rclcpp::Subscription::SharedPtr subscription_; //!< The subscription + rclcpp::Subscription::SharedPtr subscription_; //!< The subscription using PointThrottledCallback = fuse_core::ThrottledMessageCallback; PointThrottledCallback throttled_callback_; //!< The throttled callback - size_t kept_messages_{0}; //!< Messages kept - size_t dropped_messages_{0}; //!< Messages dropped + size_t kept_messages_{ 0 }; //!< Messages kept + size_t dropped_messages_{ 0 }; //!< Messages dropped // We use a SharedPtr to check for nullptr just for this test geometry_msgs::msg::Point::SharedPtr last_kept_message_; //!< The last message kept @@ -214,17 +209,15 @@ class TestThrottledCallback : public ::testing::Test { rclcpp::init(0, nullptr); executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); rclcpp::shutdown(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); @@ -242,9 +235,7 @@ TEST_F(TestThrottledCallback, NoDroppedMessagesIfThrottlePeriodIsZero) executor_->add_node(sensor_model); // Time should be valid after the context is initialized. But it doesn't hurt to verify. - ASSERT_TRUE( - sensor_model->getNode()->get_clock()->wait_until_started( - rclcpp::Duration::from_seconds(1.0))); + ASSERT_TRUE(sensor_model->getNode()->get_clock()->wait_until_started(rclcpp::Duration::from_seconds(1.0))); // Publish some messages: const size_t num_messages = 10; @@ -268,9 +259,7 @@ TEST_F(TestThrottledCallback, DropMessagesIfThrottlePeriodIsGreaterThanPublishPe executor_->add_node(sensor_model); // Time should be valid after the context is initialized. But it doesn't hurt to verify. - ASSERT_TRUE( - sensor_model->getNode()->get_clock()->wait_until_started( - rclcpp::Duration::from_seconds(1.0))); + ASSERT_TRUE(sensor_model->getNode()->get_clock()->wait_until_started(rclcpp::Duration::from_seconds(1.0))); // Publish some messages at half the throttled period: const size_t num_messages = 10; @@ -298,9 +287,7 @@ TEST_F(TestThrottledCallback, AlwaysKeepFirstMessageEvenIfThrottlePeriodIsTooLar executor_->add_node(sensor_model); // Time should be valid after the context is initialized. But it doesn't hurt to verify. - ASSERT_TRUE( - sensor_model->getNode()->get_clock()->wait_until_started( - rclcpp::Duration::from_seconds(1.0))); + ASSERT_TRUE(sensor_model->getNode()->get_clock()->wait_until_started(rclcpp::Duration::from_seconds(1.0))); ASSERT_EQ(nullptr, sensor_model->getLastKeptMessage()); diff --git a/fuse_core/test/test_timestamp_manager.cpp b/fuse_core/test/test_timestamp_manager.cpp index 125ff1eeb..b5a7a8530 100644 --- a/fuse_core/test/test_timestamp_manager.cpp +++ b/fuse_core/test/test_timestamp_manager.cpp @@ -52,13 +52,9 @@ class TimestampManagerTestFixture : public ::testing::Test { public: TimestampManagerTestFixture() - : manager(std::bind(&TimestampManagerTestFixture::generator, - this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4), - rclcpp::Duration::max()) + : manager(std::bind(&TimestampManagerTestFixture::generator, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4), + rclcpp::Duration::max()) { } @@ -74,11 +70,9 @@ class TimestampManagerTestFixture : public ::testing::Test generated_time_spans.clear(); } - void generator( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & /*constraints*/, - std::vector & /*variables*/) + void generator(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& /*constraints*/, + std::vector& /*variables*/) { generated_time_spans.emplace_back(beginning_stamp, ending_stamp); } @@ -87,7 +81,6 @@ class TimestampManagerTestFixture : public ::testing::Test std::vector> generated_time_spans; }; - TEST_F(TimestampManagerTestFixture, Empty) { // Test: diff --git a/fuse_core/test/test_transaction.cpp b/fuse_core/test/test_transaction.cpp index 159acd848..7f60fa279 100644 --- a/fuse_core/test/test_transaction.cpp +++ b/fuse_core/test/test_transaction.cpp @@ -46,7 +46,6 @@ using fuse_core::Transaction; using fuse_core::UUID; - /** * @brief Test that a collection of stamps exist in a Transaction object's involved stamps container * @@ -59,29 +58,31 @@ using fuse_core::UUID; * @return True if the expected stamps, and only the expected stamps, exist in the * transaction, False otherwise. */ -template -bool testInvolvedStamps(const TimeRange & expected, const Transaction & transaction) +template +bool testInvolvedStamps(const TimeRange& expected, const Transaction& transaction) { auto range = transaction.involvedStamps(); - if (std::distance( - expected.begin(), - expected.end()) != std::distance(range.begin(), range.end())) + if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) { return false; } - for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto & actual_stamp = *iter; + for (auto iter = range.begin(); iter != range.end(); ++iter) + { + const auto& actual_stamp = *iter; bool found = false; - for (const auto & expected_stamp : expected) { - if (actual_stamp == expected_stamp) { + for (const auto& expected_stamp : expected) + { + if (actual_stamp == expected_stamp) + { found = true; break; } } - if (!found) { + if (!found) + { return false; } } @@ -103,43 +104,44 @@ bool testInvolvedStamps(const TimeRange & expected, const Transaction & transact * @return True if the expected constraints, and only the expected constraints, * exist in the transaction, False otherwise. */ -template -bool testAddedConstraints(const ConstraintRange & expected, const Transaction & transaction) +template +bool testAddedConstraints(const ConstraintRange& expected, const Transaction& transaction) { auto range = transaction.addedConstraints(); - if (std::distance( - expected.begin(), - expected.end()) != std::distance(range.begin(), range.end())) + if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) { return false; } - for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto & actual_constraint = dynamic_cast(*iter); + for (auto iter = range.begin(); iter != range.end(); ++iter) + { + const auto& actual_constraint = dynamic_cast(*iter); bool found = false; - for (const auto & expected_constraint : expected) { - if (actual_constraint.uuid() == expected_constraint.uuid()) { + for (const auto& expected_constraint : expected) + { + if (actual_constraint.uuid() == expected_constraint.uuid()) + { found = true; bool is_equal = true; is_equal = is_equal && (expected_constraint.type() == actual_constraint.type()); - is_equal = is_equal && - (expected_constraint.variables().size() == actual_constraint.variables().size()); - for (size_t i = 0; i < expected_constraint.variables().size(); ++i) { - is_equal = is_equal && - (expected_constraint.variables().at(i) == actual_constraint.variables().at(i)); + is_equal = is_equal && (expected_constraint.variables().size() == actual_constraint.variables().size()); + for (size_t i = 0; i < expected_constraint.variables().size(); ++i) + { + is_equal = is_equal && (expected_constraint.variables().at(i) == actual_constraint.variables().at(i)); } - const auto & expected_derived = - dynamic_cast(expected_constraint); + const auto& expected_derived = dynamic_cast(expected_constraint); is_equal = is_equal && (expected_derived.data == actual_constraint.data); - if (!is_equal) { + if (!is_equal) + { return false; } } } - if (!found) { + if (!found) + { return false; } } @@ -162,29 +164,31 @@ bool testAddedConstraints(const ConstraintRange & expected, const Transaction & * @return True if the expected constraints, and only the expected constraints, exist in * the transaction, False otherwise. */ -template -bool testRemovedConstraints(const UuidRange & expected, const Transaction & transaction) +template +bool testRemovedConstraints(const UuidRange& expected, const Transaction& transaction) { auto range = transaction.removedConstraints(); - if (std::distance( - expected.begin(), - expected.end()) != std::distance(range.begin(), range.end())) + if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) { return false; } - for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto & actual_constraint_uuid = *iter; + for (auto iter = range.begin(); iter != range.end(); ++iter) + { + const auto& actual_constraint_uuid = *iter; bool found = false; - for (const auto & expected_constraint_uuid : expected) { - if (actual_constraint_uuid == expected_constraint_uuid) { + for (const auto& expected_constraint_uuid : expected) + { + if (actual_constraint_uuid == expected_constraint_uuid) + { found = true; break; } } - if (!found) { + if (!found) + { return false; } } @@ -205,36 +209,39 @@ bool testRemovedConstraints(const UuidRange & expected, const Transaction & tran * @return True if the expected variables, and only the expected variables, exist in * the transaction, False otherwise. */ -template -bool testAddedVariables(const VariableRange & expected, const Transaction & transaction) +template +bool testAddedVariables(const VariableRange& expected, const Transaction& transaction) { auto range = transaction.addedVariables(); - if (std::distance( - expected.begin(), - expected.end()) != std::distance(range.begin(), range.end())) + if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) { return false; } - for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto & actual_variable = dynamic_cast(*iter); + for (auto iter = range.begin(); iter != range.end(); ++iter) + { + const auto& actual_variable = dynamic_cast(*iter); bool found = false; - for (const auto & expected_variable : expected) { - if (actual_variable.uuid() == expected_variable.uuid()) { + for (const auto& expected_variable : expected) + { + if (actual_variable.uuid() == expected_variable.uuid()) + { found = true; bool is_equal = true; is_equal = is_equal && (expected_variable.type() == actual_variable.type()); is_equal = is_equal && (expected_variable.size() == actual_variable.size()); is_equal = is_equal && (expected_variable.data()[0] == actual_variable.data()[0]); - if (!is_equal) { + if (!is_equal) + { return false; } } } - if (!found) { + if (!found) + { return false; } } @@ -256,29 +263,31 @@ bool testAddedVariables(const VariableRange & expected, const Transaction & tran * @return True if the expected variables, and only the expected variables, exist in the * transaction, False otherwise. */ -template -bool testRemovedVariables(const UuidRange & expected, const Transaction & transaction) +template +bool testRemovedVariables(const UuidRange& expected, const Transaction& transaction) { auto range = transaction.removedVariables(); - if (std::distance( - expected.begin(), - expected.end()) != std::distance(range.begin(), range.end())) + if (std::distance(expected.begin(), expected.end()) != std::distance(range.begin(), range.end())) { return false; } - for (auto iter = range.begin(); iter != range.end(); ++iter) { - const auto & actual_variable_uuid = *iter; + for (auto iter = range.begin(); iter != range.end(); ++iter) + { + const auto& actual_variable_uuid = *iter; bool found = false; - for (const auto & expected_variable_uuid : expected) { - if (actual_variable_uuid == expected_variable_uuid) { + for (const auto& expected_variable_uuid : expected) + { + if (actual_variable_uuid == expected_variable_uuid) + { found = true; break; } } - if (!found) { + if (!found) + { return false; } } @@ -299,8 +308,7 @@ TEST(Transaction, Empty) // A transaction with added constraints cannot be empty { const auto variable_uuid = fuse_core::uuid::generate(); - const auto constraint = ExampleConstraint::make_shared( - "test", std::initializer_list{variable_uuid}); + const auto constraint = ExampleConstraint::make_shared("test", std::initializer_list{ variable_uuid }); Transaction transaction; transaction.addConstraint(constraint); @@ -354,8 +362,7 @@ TEST(Transaction, AddConstraint) // Add a single constraint and verify it exists in the added constraints { UUID variable_uuid = fuse_core::uuid::generate(); - auto constraint = ExampleConstraint::make_shared( - "test", std::initializer_list{variable_uuid}); + auto constraint = ExampleConstraint::make_shared("test", std::initializer_list{ variable_uuid }); Transaction transaction; transaction.addConstraint(constraint); @@ -368,8 +375,7 @@ TEST(Transaction, AddConstraint) // Add the same constraint multiple times. Verify only one constraint exists in the Transaction. { UUID variable_uuid = fuse_core::uuid::generate(); - auto constraint = ExampleConstraint::make_shared( - "test", std::initializer_list{variable_uuid}); + auto constraint = ExampleConstraint::make_shared("test", std::initializer_list{ variable_uuid }); Transaction transaction; transaction.addConstraint(constraint); @@ -384,14 +390,11 @@ TEST(Transaction, AddConstraint) // Add multiple constraints. Verify they all exist in the Transaction. { UUID variable1_uuid = fuse_core::uuid::generate(); - auto constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable1_uuid}); + auto constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable1_uuid }); UUID variable2_uuid = fuse_core::uuid::generate(); - auto constraint2 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable2_uuid}); + auto constraint2 = ExampleConstraint::make_shared("test", std::initializer_list{ variable2_uuid }); UUID variable3_uuid = fuse_core::uuid::generate(); - auto constraint3 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable3_uuid}); + auto constraint3 = ExampleConstraint::make_shared("test", std::initializer_list{ variable3_uuid }); Transaction transaction; transaction.addConstraint(constraint1); @@ -409,8 +412,7 @@ TEST(Transaction, AddConstraint) // also be deleted from the 'removed' container. { UUID variable1_uuid = fuse_core::uuid::generate(); - auto constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable1_uuid}); + auto constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable1_uuid }); UUID constraint2_uuid = fuse_core::uuid::generate(); @@ -430,8 +432,7 @@ TEST(Transaction, AddConstraint) { // Create and add the constraint to the transaction UUID variable_uuid = fuse_core::uuid::generate(); - auto constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable_uuid}); + auto constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable_uuid }); constraint1->data = 1.0; Transaction transaction; @@ -521,12 +522,10 @@ TEST(Transaction, RemoveConstraint) // marked for removal; instead it should be deleted from the added constraints. { UUID variable1_uuid = fuse_core::uuid::generate(); - auto constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable1_uuid}); + auto constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable1_uuid }); UUID variable2_uuid = fuse_core::uuid::generate(); - auto constraint2 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable2_uuid}); + auto constraint2 = ExampleConstraint::make_shared("test", std::initializer_list{ variable2_uuid }); Transaction transaction; transaction.addConstraint(constraint1); @@ -729,12 +728,9 @@ TEST(Transaction, Merge) UUID variable1_uuid = fuse_core::uuid::generate(); UUID variable2_uuid = fuse_core::uuid::generate(); UUID variable3_uuid = fuse_core::uuid::generate(); - auto added_constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable1_uuid}); - auto added_constraint2 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable2_uuid}); - auto added_constraint3 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable3_uuid}); + auto added_constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable1_uuid }); + auto added_constraint2 = ExampleConstraint::make_shared("test", std::initializer_list{ variable2_uuid }); + auto added_constraint3 = ExampleConstraint::make_shared("test", std::initializer_list{ variable3_uuid }); UUID removed_constraint1 = fuse_core::uuid::generate(); UUID removed_constraint2 = fuse_core::uuid::generate(); @@ -818,10 +814,8 @@ TEST(Transaction, Clone) UUID variable1_uuid = fuse_core::uuid::generate(); UUID variable2_uuid = fuse_core::uuid::generate(); - auto added_constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable1_uuid}); - auto added_constraint2 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable2_uuid}); + auto added_constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable1_uuid }); + auto added_constraint2 = ExampleConstraint::make_shared("test", std::initializer_list{ variable2_uuid }); UUID removed_constraint1 = fuse_core::uuid::generate(); UUID removed_constraint2 = fuse_core::uuid::generate(); @@ -882,10 +876,8 @@ TEST(Transaction, Serialize) UUID variable1_uuid = fuse_core::uuid::generate(); UUID variable2_uuid = fuse_core::uuid::generate(); - auto added_constraint1 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable1_uuid}); - auto added_constraint2 = ExampleConstraint::make_shared( - "test", std::initializer_list{variable2_uuid}); + auto added_constraint1 = ExampleConstraint::make_shared("test", std::initializer_list{ variable1_uuid }); + auto added_constraint2 = ExampleConstraint::make_shared("test", std::initializer_list{ variable2_uuid }); UUID removed_constraint1 = fuse_core::uuid::generate(); UUID removed_constraint2 = fuse_core::uuid::generate(); diff --git a/fuse_core/test/test_uuid.cpp b/fuse_core/test/test_uuid.cpp index 17041a8dd..5f6225c74 100644 --- a/fuse_core/test/test_uuid.cpp +++ b/fuse_core/test/test_uuid.cpp @@ -43,7 +43,6 @@ using fuse_core::UUID; using UUIDs = std::vector; - TEST(UUID, Generate) { // These tests are mostly just calling the different generate() signatures to verify they compile @@ -59,8 +58,7 @@ TEST(UUID, Generate) // UUID. { std::string buffer1 = "Curse your sudden but inevitable betrayal!"; - std::string buffer2 = - "Man walks down the street in a hat like that, you know he's not afraid of anything."; + std::string buffer2 = "Man walks down the street in a hat like that, you know he's not afraid of anything."; UUID id1 = fuse_core::uuid::generate(buffer1.data(), buffer1.size()); UUID id2 = fuse_core::uuid::generate(buffer1.data(), buffer1.size()); UUID id3 = fuse_core::uuid::generate(buffer2.data(), buffer2.size()); @@ -86,8 +84,7 @@ TEST(UUID, Generate) { std::string name1 = "Jayne"; std::string name2 = "Hoban"; - std::string buffer1 = - "Ten percent of nothing is, let me do the math here. Nothing into nothin'. Carry the nothin'"; + std::string buffer1 = "Ten percent of nothing is, let me do the math here. Nothing into nothin'. Carry the nothin'"; std::string buffer2 = "Some people juggle geese."; UUID id1 = fuse_core::uuid::generate(name1, buffer1.data(), buffer1.size()); @@ -167,11 +164,12 @@ TEST(UUID, Generate) } } -void generateUUIDs(UUIDs & uuids) +void generateUUIDs(UUIDs& uuids) { constexpr size_t uuid_count = 100000; uuids.reserve(uuid_count); - for (size_t i = 0; i < uuid_count; ++i) { + for (size_t i = 0; i < uuid_count; ++i) + { auto uuid = fuse_core::uuid::generate(); uuids.push_back(uuid); } @@ -185,10 +183,9 @@ TEST(UUID, CollisionSingleThread) // Check for duplicates std::unordered_set unique_uuids; - for (const auto & uuid : raw_uuids) { - ASSERT_TRUE( - unique_uuids.find(uuid) == - unique_uuids.end()) << "UUIDs before duplicate " << unique_uuids.size(); + for (const auto& uuid : raw_uuids) + { + ASSERT_TRUE(unique_uuids.find(uuid) == unique_uuids.end()) << "UUIDs before duplicate " << unique_uuids.size(); unique_uuids.insert(uuid); } } @@ -199,20 +196,22 @@ TEST(UUID, CollisionManyThreads) constexpr size_t thread_count = 12; std::vector raw_uuids(thread_count); std::vector threads(thread_count); - for (size_t i = 0; i < threads.size(); ++i) { + for (size_t i = 0; i < threads.size(); ++i) + { threads[i] = std::thread(generateUUIDs, std::ref(raw_uuids[i])); } - for (size_t i = 0; i < threads.size(); ++i) { + for (size_t i = 0; i < threads.size(); ++i) + { threads[i].join(); } // Check for duplicates std::unordered_set unique_uuids; - for (size_t i = 0; i < raw_uuids.size(); ++i) { - for (const auto & uuid : raw_uuids[i]) { - ASSERT_TRUE( - unique_uuids.find(uuid) == - unique_uuids.end()) << "UUIDs before duplicate " << unique_uuids.size(); + for (size_t i = 0; i < raw_uuids.size(); ++i) + { + for (const auto& uuid : raw_uuids[i]) + { + ASSERT_TRUE(unique_uuids.find(uuid) == unique_uuids.end()) << "UUIDs before duplicate " << unique_uuids.size(); unique_uuids.insert(uuid); } } diff --git a/fuse_core/test/test_variable.cpp b/fuse_core/test/test_variable.cpp index d4c8caa9e..4198913a6 100644 --- a/fuse_core/test/test_variable.cpp +++ b/fuse_core/test/test_variable.cpp @@ -43,12 +43,14 @@ #include "example_variable.hpp" -TEST(Variable, Type) { +TEST(Variable, Type) +{ ExampleVariable variable; ASSERT_EQ("ExampleVariable", variable.type()); } -TEST(LegacyVariable, Serialization) { +TEST(LegacyVariable, Serialization) +{ // Create an Orientation3DStamped LegacyVariable expected; expected.data()[0] = 0.952; @@ -80,7 +82,7 @@ TEST(LegacyVariable, Serialization) { #if CERES_SUPPORTS_MANIFOLDS struct QuaternionCostFunction { - explicit QuaternionCostFunction(double * observation) + explicit QuaternionCostFunction(double* observation) { observation_[0] = observation[0]; observation_[1] = observation[1]; @@ -88,24 +90,12 @@ struct QuaternionCostFunction observation_[3] = observation[3]; } - template - bool operator()(const T * quaternion, T * residual) const + template + bool operator()(const T* quaternion, T* residual) const { - T inverse_quaternion[4] = - { - quaternion[0], - -quaternion[1], - -quaternion[2], - -quaternion[3] - }; - - T observation[4] = - { - T(observation_[0]), - T(observation_[1]), - T(observation_[2]), - T(observation_[3]) - }; + T inverse_quaternion[4] = { quaternion[0], -quaternion[1], -quaternion[2], -quaternion[3] }; + + T observation[4] = { T(observation_[0]), T(observation_[1]), T(observation_[2]), T(observation_[3]) }; T output[4]; @@ -122,7 +112,8 @@ struct QuaternionCostFunction double observation_[4]; }; -TEST(LegacyVariable, ManifoldAdapter) { +TEST(LegacyVariable, ManifoldAdapter) +{ // Create an Orientation3DStamped with R, P, Y values of 10, -20, 30 degrees LegacyVariable orientation; orientation.data()[0] = 0.952; @@ -131,19 +122,16 @@ TEST(LegacyVariable, ManifoldAdapter) { orientation.data()[3] = 0.239; // 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)); + double target_quat[4] = { 1.0, 0.0, 0.0, 0.0 }; + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction(new QuaternionCostFunction(target_quat)); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.manifold()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); - problem.AddResidualBlock( - cost_function, - nullptr, - parameter_blocks); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver ceres::Solver::Summary summary; @@ -157,7 +145,8 @@ TEST(LegacyVariable, ManifoldAdapter) { EXPECT_NEAR(target_quat[3], orientation.data()[3], 1.0e-3); } -TEST(LegacyVariable, Deserialization) { +TEST(LegacyVariable, Deserialization) +{ // Test loading a LegacyVariable that was serialized without manifold support. // Verify the deserialization works, and that a manifold pointer can be generated. @@ -190,10 +179,10 @@ TEST(LegacyVariable, Deserialization) { // Test the manifold interface, and that the Legacy LocalParameterization is wrapped // in a ManifoldAdapter - fuse_core::Manifold * actual_manifold = nullptr; + 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_doc/doc/conf.py b/fuse_doc/doc/conf.py index 8e19c3e42..5bd320ed8 100644 --- a/fuse_doc/doc/conf.py +++ b/fuse_doc/doc/conf.py @@ -10,169 +10,162 @@ } extensions = [ - 'sphinx.ext.autodoc', - 'sphinx.ext.doctest', - 'sphinx.ext.intersphinx', - 'sphinx.ext.todo', - 'sphinx.ext.coverage', - 'sphinx.ext.mathjax', + "sphinx.ext.autodoc", + "sphinx.ext.doctest", + "sphinx.ext.intersphinx", + "sphinx.ext.todo", + "sphinx.ext.coverage", + "sphinx.ext.mathjax", ] -templates_path = ['.templates'] -source_suffix = '.rst' -master_doc = 'index' +templates_path = [".templates"] +source_suffix = ".rst" +master_doc = "index" -project = u'fuse' -copyright = u'2018, Locus Robotics' -author = u'Stephen Williams' +project = "fuse" +copyright = "2018, Locus Robotics" +author = "Stephen Williams" # TODO(sloretz) how to get this information using rosdoc2? # version = catkin_package.version # release = catkin_package.version language = None -exclude_patterns = ['.build'] -pygments_style = 'sphinx' +exclude_patterns = [".build"] +pygments_style = "sphinx" todo_include_todos = True -html_theme = 'bizstyle' +html_theme = "bizstyle" -html_theme_options = { - "sidebarwidth": "350" -} +html_theme_options = {"sidebarwidth": "350"} # The name of an image file (relative to this directory) to place at the top # of the sidebar. -html_logo = 'images/fuse_small.png' +html_logo = "images/fuse_small.png" # The name of an image file (relative to this directory) to use as a favicon of # the docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 # pixels large. -#html_favicon = None +# html_favicon = None # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -#html_static_path = ['.static'] +# html_static_path = ['.static'] # Add any extra paths that contain custom files (such as robots.txt or # .htaccess) here, relative to this directory. These files are copied # directly to the root of the documentation. -#html_extra_path = [] +# html_extra_path = [] # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, # using the given strftime format. -#html_last_updated_fmt = '%b %d, %Y' +# html_last_updated_fmt = '%b %d, %Y' # If true, SmartyPants will be used to convert quotes and dashes to # typographically correct entities. -#html_use_smartypants = True +# html_use_smartypants = True # Custom sidebar templates, maps document names to template names. -html_sidebars = { '**': ['full_globaltoc.html', 'sourcelink.html', 'searchbox.html'], } +html_sidebars = { + "**": ["full_globaltoc.html", "sourcelink.html", "searchbox.html"], +} # Additional templates that should be rendered to pages, maps page names to # template names. -#html_additional_pages = {} +# html_additional_pages = {} # If false, no module index is generated. -#html_domain_indices = True +# html_domain_indices = True # If false, no index is generated. -#html_use_index = True +# html_use_index = True # If true, the index is split into individual pages for each letter. -#html_split_index = False +# html_split_index = False # If true, links to the reST sources are added to the pages. -#html_show_sourcelink = True +# html_show_sourcelink = True # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. -#html_show_sphinx = True +# html_show_sphinx = True # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. -#html_show_copyright = True +# html_show_copyright = True # If true, an OpenSearch description file will be output, and all pages will # contain a tag referring to it. The value of this option must be the # base URL from which the finished HTML is served. -#html_use_opensearch = '' +# html_use_opensearch = '' # This is the file name suffix for HTML files (e.g. ".xhtml"). -#html_file_suffix = None +# html_file_suffix = None # Language to be used for generating the HTML full-text search index. # Sphinx supports the following languages: # 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja' # 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr' -#html_search_language = 'en' +# html_search_language = 'en' # A dictionary with options for the search language support, empty by default. # Now only 'ja' uses this config value -#html_search_options = {'type': 'default'} +# html_search_options = {'type': 'default'} # The name of a javascript file (relative to the configuration directory) that # implements a search results scorer. If empty, the default will be used. -#html_search_scorer = 'scorer.js' +# html_search_scorer = 'scorer.js' # Output file base name for HTML help builder. -htmlhelp_basename = 'fusedoc' +htmlhelp_basename = "fusedoc" # -- Options for LaTeX output --------------------------------------------- latex_elements = { -# The paper size ('letterpaper' or 'a4paper'). -#'papersize': 'letterpaper', - -# The font size ('10pt', '11pt' or '12pt'). -#'pointsize': '10pt', - -# Additional stuff for the LaTeX preamble. -#'preamble': '', - -# Latex figure (float) alignment -#'figure_align': 'htbp', + # The paper size ('letterpaper' or 'a4paper'). + #'papersize': 'letterpaper', + # The font size ('10pt', '11pt' or '12pt'). + #'pointsize': '10pt', + # Additional stuff for the LaTeX preamble. + #'preamble': '', + # Latex figure (float) alignment + #'figure_align': 'htbp', } # Grouping the document tree into LaTeX files. List of tuples # (source start file, target name, title, # author, documentclass [howto, manual, or own class]). latex_documents = [ - (master_doc, 'fuse.tex', u'fuse Documentation', - u'Stephen Williams', 'manual'), + (master_doc, "fuse.tex", "fuse Documentation", "Stephen Williams", "manual"), ] # The name of an image file (relative to this directory) to place at the top of # the title page. -#latex_logo = None +# latex_logo = None # For "manual" documents, if this is true, then toplevel headings are parts, # not chapters. -#latex_use_parts = False +# latex_use_parts = False # If true, show page references after internal links. -#latex_show_pagerefs = False +# latex_show_pagerefs = False # If true, show URL addresses after external links. -#latex_show_urls = False +# latex_show_urls = False # Documents to append as an appendix to all manuals. -#latex_appendices = [] +# latex_appendices = [] # If false, no module index is generated. -#latex_domain_indices = True +# latex_domain_indices = True # -- Options for manual page output --------------------------------------- # One entry per manual page. List of tuples # (source start file, name, description, authors, manual section). -man_pages = [ - (master_doc, 'fuse', u'fuse Documentation', - [author], 1) -] +man_pages = [(master_doc, "fuse", "fuse Documentation", [author], 1)] # If true, show URL addresses after external links. -#man_show_urls = False +# man_show_urls = False # -- Options for Texinfo output ------------------------------------------- @@ -181,22 +174,28 @@ # (source start file, target name, title, author, # dir menu entry, description, category) texinfo_documents = [ - (master_doc, 'fuse', u'fuse Documentation', - author, 'fuse', 'graph-based nonlinear state estimation', - 'Miscellaneous'), + ( + master_doc, + "fuse", + "fuse Documentation", + author, + "fuse", + "graph-based nonlinear state estimation", + "Miscellaneous", + ), ] # Documents to append as an appendix to all manuals. -#texinfo_appendices = [] +# texinfo_appendices = [] # If false, no module index is generated. -#texinfo_domain_indices = True +# texinfo_domain_indices = True # How to display URL addresses: 'footnote', 'no', or 'inline'. -#texinfo_show_urls = 'footnote' +# texinfo_show_urls = 'footnote' # If true, do not generate a @detailmenu in the "Top" node's menu. -#texinfo_no_detailmenu = False +# texinfo_no_detailmenu = False # Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {'https://docs.python.org/': None} +intersphinx_mapping = {"https://docs.python.org/": None} diff --git a/fuse_doc/rosdoc2.yaml b/fuse_doc/rosdoc2.yaml index 8fe6fdac1..17a6f2b92 100644 --- a/fuse_doc/rosdoc2.yaml +++ b/fuse_doc/rosdoc2.yaml @@ -2,7 +2,7 @@ type: 'rosdoc2 config' version: 1 --- settings: - # Don't generate a defalt index file, this package provides its own. + # Don't generate a default index file, this package provides its own. generate_package_index: false builders: - sphinx: { diff --git a/fuse_graphs/CHANGELOG.rst b/fuse_graphs/CHANGELOG.rst index feb8b09a4..94ddf94f9 100644 --- a/fuse_graphs/CHANGELOG.rst +++ b/fuse_graphs/CHANGELOG.rst @@ -7,7 +7,7 @@ Changelog for package fuse_graphs 1.1.1 (2024-05-02) ------------------ -* Required formatting changes for the lastest version of ROS 2 Rolling (`#368 `_) +* Required formatting changes for the latest version of ROS 2 Rolling (`#368 `_) * Contributors: Stephen Williams 1.1.0 (2024-04-20) diff --git a/fuse_graphs/CMakeLists.txt b/fuse_graphs/CMakeLists.txt index 9a0bc4dc5..45838b883 100644 --- a/fuse_graphs/CMakeLists.txt +++ b/fuse_graphs/CMakeLists.txt @@ -20,94 +20,103 @@ 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: +# ############################################################################## +# 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, -# | ^~~~~~~~~~ +# 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(${PROJECT_NAME} PUBLIC - "$" - "$" -) -target_link_libraries(${PROJECT_NAME} PUBLIC - Boost::serialization - Ceres::ceres - fuse_core::fuse_core - pluginlib::pluginlib - rclcpp::rclcpp -) - -############# -## Testing ## -############# +# fuse_graphs library +add_library(${PROJECT_NAME} src/hash_graph.cpp) +target_include_directories( + ${PROJECT_NAME} + PUBLIC "$" + "$") +target_link_libraries( + ${PROJECT_NAME} PUBLIC Boost::serialization Ceres::ceres fuse_core::fuse_core + pluginlib::pluginlib rclcpp::rclcpp) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() +# ############################################################################## +# Testing ## +# ############################################################################## +if(BUILD_TESTING) add_subdirectory(test) # Benchmarks find_package(benchmark QUIET) if(benchmark_FOUND) # Create Problem benchmark - add_executable(benchmark_create_problem benchmark/benchmark_create_problem.cpp) + add_executable(benchmark_create_problem + benchmark/benchmark_create_problem.cpp) target_include_directories(benchmark_create_problem PRIVATE "test/") - target_link_libraries(benchmark_create_problem - ${PROJECT_NAME} - benchmark::benchmark - ) + target_link_libraries(benchmark_create_problem ${PROJECT_NAME} + benchmark::benchmark) endif() endif() -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## -install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-export ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) pluginlib_export_plugin_description_file(fuse_core fuse_plugins.xml) ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET) -ament_export_dependencies( - ament_cmake_ros - fuse_core - pluginlib - rclcpp - Ceres -) +ament_export_dependencies(ament_cmake_ros fuse_core pluginlib rclcpp Ceres) ament_package(CONFIG_EXTRAS boost-extras.cmake) diff --git a/fuse_graphs/benchmark/benchmark_create_problem.cpp b/fuse_graphs/benchmark/benchmark_create_problem.cpp index a187e4a8e..eb1fdc7d5 100644 --- a/fuse_graphs/benchmark/benchmark_create_problem.cpp +++ b/fuse_graphs/benchmark/benchmark_create_problem.cpp @@ -63,15 +63,15 @@ class TestableHashGraph : public fuse_graphs::HashGraph class ExampleFunctor { public: - explicit ExampleFunctor(const std::vector & b) - : b_(b) + explicit ExampleFunctor(const std::vector& b) : b_(b) { } - template - bool operator()(T const * const * variables, T * residuals) const + template + bool operator()(T const* const* variables, T* residuals) const { - for (size_t i = 0; i < b_.size(); ++i) { + for (size_t i = 0; i < b_.size(); ++i) + { residuals[i] = variables[i][0] - T(b_[i]); } return true; @@ -91,22 +91,21 @@ class ExampleConstraint : public fuse_core::Constraint ExampleConstraint() = default; - template - explicit ExampleConstraint( - const std::string & source, VariableUuidIterator first, - VariableUuidIterator last) - : fuse_core::Constraint(source, first, last), - data(std::distance(first, last), 0.0) + template + explicit ExampleConstraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last) + : fuse_core::Constraint(source, first, last), data(std::distance(first, last), 0.0) { } - void print(std::ostream & /*stream = std::cout*/) const override {} - ceres::CostFunction * costFunction() const override + void print(std::ostream& /*stream = std::cout*/) const override { - auto cost_function = - new ceres::DynamicAutoDiffCostFunction(new ExampleFunctor(data)); + } + ceres::CostFunction* costFunction() const override + { + auto cost_function = new ceres::DynamicAutoDiffCostFunction(new ExampleFunctor(data)); - for (size_t i = 0; i < data.size(); ++i) { + for (size_t i = 0; i < data.size(); ++i) + { cost_function->AddParameterBlock(1); } @@ -128,11 +127,11 @@ class ExampleConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data; + archive& boost::serialization::base_object(*this); + archive& data; } }; @@ -145,52 +144,48 @@ BOOST_CLASS_EXPORT(ExampleConstraint); * @param[in] num_variables_per_constraint Number of variables the constraints should have * @return The TestableHashGraph */ -TestableHashGraph makeTestableHashGraph( - const size_t num_constraints, - const size_t num_variables_per_constraint) +TestableHashGraph makeTestableHashGraph(const size_t num_constraints, const size_t num_variables_per_constraint) { TestableHashGraph graph; - for (size_t i = 0; i < num_constraints; ++i) { + for (size_t i = 0; i < num_constraints; ++i) + { // Generate variables std::vector variables; variables.reserve(num_variables_per_constraint); - std::generate_n( - std::back_inserter(variables), num_variables_per_constraint, - []() {return ExampleVariable::make_shared();}); // NOLINT + std::generate_n(std::back_inserter(variables), num_variables_per_constraint, + []() { return ExampleVariable::make_shared(); }); // NOLINT // Add variables to the graph - for (const auto & variable : variables) { + for (const auto& variable : variables) + { graph.addVariable(variable); } // Add constraint with the generated variables std::vector variable_uuids; variable_uuids.reserve(variables.size()); - std::transform( - variables.begin(), variables.end(), std::back_inserter(variable_uuids), - [](const auto & variable) {return variable->uuid();}); // NOLINT - - graph.addConstraint( - ExampleConstraint::make_shared( - "test", variable_uuids.begin(), - variable_uuids.end())); + std::transform(variables.begin(), variables.end(), std::back_inserter(variable_uuids), + [](const auto& variable) { return variable->uuid(); }); // NOLINT + + graph.addConstraint(ExampleConstraint::make_shared("test", variable_uuids.begin(), variable_uuids.end())); } return graph; } -static void BM_createProblem(benchmark::State & state) +static void BM_createProblem(benchmark::State& state) { const auto graph = makeTestableHashGraph(state.range(0), state.range(1)); ceres::Problem problem; - for (auto _ : state) { + for (auto _ : state) + { graph.createProblem(problem); } } -BENCHMARK(BM_createProblem)->RangeMultiplier(2)->Ranges({{200, 4000}, {2, 12}}); // NOLINT +BENCHMARK(BM_createProblem)->RangeMultiplier(2)->Ranges({ { 200, 4000 }, { 2, 12 } }); // NOLINT BENCHMARK_MAIN(); diff --git a/fuse_graphs/boost-extras.cmake b/fuse_graphs/boost-extras.cmake index 12c9f2f6c..a7331140d 100644 --- a/fuse_graphs/boost-extras.cmake +++ b/fuse_graphs/boost-extras.cmake @@ -3,27 +3,26 @@ # 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 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. +# * 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 Open Source Robotics Foundation, Inc. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# * Neither the name of the Open Source Robotics Foundation, Inc. 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. +# 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. find_package(Boost REQUIRED COMPONENTS serialization) diff --git a/fuse_graphs/include/fuse_graphs/hash_graph.hpp b/fuse_graphs/include/fuse_graphs/hash_graph.hpp index 8eb1b8f10..6b33df7dd 100644 --- a/fuse_graphs/include/fuse_graphs/hash_graph.hpp +++ b/fuse_graphs/include/fuse_graphs/hash_graph.hpp @@ -59,7 +59,6 @@ #include #include - namespace fuse_graphs { @@ -86,14 +85,14 @@ class HashGraph : public fuse_core::Graph * * @param[in] params HashGraph parameters. */ - explicit HashGraph(const HashGraphParams & params = HashGraphParams()); + explicit HashGraph(const HashGraphParams& params = HashGraphParams()); /** * @brief Copy constructor * * Performs a deep copy of the graph */ - HashGraph(const HashGraph & other); + HashGraph(const HashGraph& other); /** * @brief Destructor @@ -105,7 +104,7 @@ class HashGraph : public fuse_core::Graph * * Performs a deep copy of the graph */ - HashGraph & operator=(const HashGraph & other); + HashGraph& operator=(const HashGraph& other); /** * @brief Clear all variables and constraints from the graph object. @@ -130,7 +129,7 @@ class HashGraph : public fuse_core::Graph * @param[in] constraint_uuid The UUID of the constraint being searched for * @return True if this constraint already exists, False otherwise */ - bool constraintExists(const fuse_core::UUID & constraint_uuid) const noexcept override; + bool constraintExists(const fuse_core::UUID& constraint_uuid) const noexcept override; /** * @brief Add a new constraint to the graph @@ -160,7 +159,7 @@ class HashGraph : public fuse_core::Graph * @param[in] constraint_uuid The UUID of the constraint to be removed * @return True if the constraint was removed, false otherwise */ - bool removeConstraint(const fuse_core::UUID & constraint_uuid) override; + bool removeConstraint(const fuse_core::UUID& constraint_uuid) override; /** * @brief Read-only access to a constraint from the graph by UUID @@ -172,8 +171,7 @@ class HashGraph : public fuse_core::Graph * @param[in] constraint_uuid The UUID of the requested constraint * @return The constraint in the graph with the specified UUID */ - const fuse_core::Constraint & getConstraint(const fuse_core::UUID & constraint_uuid) const - override; + const fuse_core::Constraint& getConstraint(const fuse_core::UUID& constraint_uuid) const override; /** * @brief Read-only access to all of the constraints in the graph @@ -195,8 +193,7 @@ class HashGraph : public fuse_core::Graph * @return A read-only iterator range containing all constraints that involve the specified * variable */ - fuse_core::Graph::const_constraint_range getConnectedConstraints( - const fuse_core::UUID & variable_uuid) const override; + fuse_core::Graph::const_constraint_range getConnectedConstraints(const fuse_core::UUID& variable_uuid) const override; /** * @brief Check if the variable already exists in the graph @@ -207,7 +204,7 @@ class HashGraph : public fuse_core::Graph * @param[in] variable_uuid The UUID of the variable being searched for * @return True if this variable already exists, False otherwise */ - bool variableExists(const fuse_core::UUID & variable_uuid) const noexcept override; + bool variableExists(const fuse_core::UUID& variable_uuid) const noexcept override; /** * @brief Add a new variable to the graph @@ -235,7 +232,7 @@ class HashGraph : public fuse_core::Graph * @param[in] variable_uuid The UUID of the variable to be removed * @return True if the variable was removed, false otherwise */ - bool removeVariable(const fuse_core::UUID & variable_uuid) override; + bool removeVariable(const fuse_core::UUID& variable_uuid) override; /** * @brief Read-only access to a variable in the graph by UUID @@ -246,7 +243,7 @@ class HashGraph : public fuse_core::Graph * @param[in] variable_uuid The UUID of the requested variable * @return The variable in the graph with the specified UUID */ - const fuse_core::Variable & getVariable(const fuse_core::UUID & variable_uuid) const override; + const fuse_core::Variable& getVariable(const fuse_core::UUID& variable_uuid) const override; /** * @brief Read-only access to all of the variables in the graph @@ -275,7 +272,7 @@ class HashGraph : public fuse_core::Graph * optimization, or if the variable's value is allowed to change during * optimization. */ - void holdVariable(const fuse_core::UUID & variable_uuid, bool hold_constant = true) override; + void holdVariable(const fuse_core::UUID& variable_uuid, bool hold_constant = true) override; /** * @brief Check whether a variable is on hold or not @@ -283,7 +280,7 @@ class HashGraph : public fuse_core::Graph * @param[in] variable_uuid The variable to test * @return True if the variable is on hold, false otherwise */ - bool isVariableOnHold(const fuse_core::UUID & variable_uuid) const override; + bool isVariableOnHold(const fuse_core::UUID& variable_uuid) const override; /** * @brief Compute the marginal covariance blocks for the requested set of variable pairs. @@ -310,11 +307,10 @@ class HashGraph : public fuse_core::Graph * variable's tangent space/local coordinates. Otherwise it is * computed in the variable's parameter space. */ - 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 override; + 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 override; /** * @brief Optimize the values of the current set of variables, given the current set of @@ -332,8 +328,7 @@ class HashGraph : public fuse_core::Graph * @return A Ceres Solver Summary structure containing information about the * optimization process */ - ceres::Solver::Summary optimize(const ceres::Solver::Options & options = ceres::Solver::Options()) - override; + ceres::Solver::Summary optimize(const ceres::Solver::Options& options = ceres::Solver::Options()) override; /** * @brief Optimize the values of the current set of variables, given the current set of @@ -350,13 +345,12 @@ class HashGraph : public fuse_core::Graph * @return A Ceres Solver Summary structure containing information about the * optimization process */ - ceres::Solver::Summary optimizeFor( - const rclcpp::Duration & max_optimization_time, - const ceres::Solver::Options & options = ceres::Solver::Options(), - rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME)) override; + ceres::Solver::Summary optimizeFor(const rclcpp::Duration& max_optimization_time, + const ceres::Solver::Options& options = ceres::Solver::Options(), + rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME)) override; /** - * @brief Evalute the values of the current set of variables, given the current set of + * @brief Evaluate the values of the current set of variables, given the current set of * constraints. * * The values in the graph do not change after the call. @@ -378,35 +372,29 @@ class HashGraph : public fuse_core::Graph * solver.googlesource.com/ceres-solver/+/master/include/ceres/problem.h#401 * @return True if the problem evaluation was successful; False, otherwise. */ - bool evaluate( - double * cost, std::vector * residuals = nullptr, - std::vector * gradient = nullptr, - const ceres::Problem::EvaluateOptions & options = ceres::Problem::EvaluateOptions()) const - override; + bool evaluate(double* cost, std::vector* residuals = nullptr, std::vector* gradient = nullptr, + const ceres::Problem::EvaluateOptions& options = ceres::Problem::EvaluateOptions()) const override; /** * @brief Print a human-readable description of the graph to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; protected: // Define some helpful typedefs - using Constraints = std::unordered_map; - using Variables = std::unordered_map; + using Constraints = std::unordered_map; + using Variables = std::unordered_map; using VariableSet = std::unordered_set; - using CrossReference = std::unordered_map, - fuse_core::uuid::hash>; + using CrossReference = std::unordered_map, fuse_core::uuid::hash>; - Constraints constraints_; //!< The set of all constraints + Constraints constraints_; //!< The set of all constraints CrossReference constraints_by_variable_uuid_; //!< Index all of the constraints by variable uuids - ceres::Problem::Options problem_options_; //!< User-defined options to be applied to all - //!< constructed ceres::Problems - Variables variables_; //!< The set of all variables - VariableSet variables_on_hold_; //!< The set of variables that should be held constant + ceres::Problem::Options problem_options_; //!< User-defined options to be applied to all + //!< constructed ceres::Problems + Variables variables_; //!< The set of all variables + VariableSet variables_on_hold_; //!< The set of variables that should be held constant /** * @brief Populate a ceres::Problem object using the current set of variables and constraints @@ -416,7 +404,7 @@ class HashGraph : public fuse_core::Graph * * @param[out] problem The ceres::Problem object to modify */ - void createProblem(ceres::Problem & problem) const; + void createProblem(ceres::Problem& problem) const; private: // Allow Boost Serialization access to private methods @@ -429,15 +417,15 @@ class HashGraph : public fuse_core::Graph * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & constraints_; - archive & constraints_by_variable_uuid_; - archive & problem_options_; - archive & variables_; - archive & variables_on_hold_; + archive& boost::serialization::base_object(*this); + archive& constraints_; + archive& constraints_by_variable_uuid_; + archive& problem_options_; + archive& variables_; + archive& variables_on_hold_; } }; @@ -451,24 +439,22 @@ namespace serialization /** * @brief Serialize a ceres::Problem::Options object using Boost Serialization */ -template -void serialize( - Archive & archive, ceres::Problem::Options & options, - const unsigned int /* version */) +template +void serialize(Archive& archive, ceres::Problem::Options& options, const unsigned int /* version */) { - archive & options.cost_function_ownership; - archive & options.disable_all_safety_checks; - archive & options.enable_fast_removal; + archive& options.cost_function_ownership; + archive& options.disable_all_safety_checks; + archive& options.enable_fast_removal; #if !CERES_SUPPORTS_MANIFOLDS - archive & options.local_parameterization_ownership; + archive& options.local_parameterization_ownership; #else // Local parameterizations got marked as deprecated in favour of Manifold in version 2.1.0, see // https://github.com/ceres-solver/ceres-solver/commit/0141ca090c315db2f3c38e1731f0fe9754a4e4cc // and they got removed in 2.2.0, see // https://github.com/ceres-solver/ceres-solver/commit/68c53bb39552cd4abfd6381df08638285f7386b3 - archive & options.manifold_ownership; + archive& options.manifold_ownership; #endif - archive & options.loss_function_ownership; + archive& options.loss_function_ownership; } } // namespace serialization diff --git a/fuse_graphs/include/fuse_graphs/hash_graph_params.hpp b/fuse_graphs/include/fuse_graphs/hash_graph_params.hpp index ec200fe87..7f9b80e29 100644 --- a/fuse_graphs/include/fuse_graphs/hash_graph_params.hpp +++ b/fuse_graphs/include/fuse_graphs/hash_graph_params.hpp @@ -39,7 +39,6 @@ #include #include - namespace fuse_graphs { @@ -61,10 +60,7 @@ struct HashGraphParams * * @param[in] interfaces - The node interfaces with which to load parameters */ - void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Parameters - > interfaces) + void loadFromROS(fuse_core::node_interfaces::NodeInterfaces interfaces) { fuse_core::loadProblemOptionsFromROS(interfaces, problem_options, "problem_options"); } diff --git a/fuse_graphs/package.xml b/fuse_graphs/package.xml index 60bb46bf9..1f8f028b4 100644 --- a/fuse_graphs/package.xml +++ b/fuse_graphs/package.xml @@ -25,8 +25,6 @@ benchmark ament_cmake_gtest - ament_lint_auto - ament_lint_common ament_cmake diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index 0f14bb7fc..14e385447 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -50,39 +50,30 @@ namespace fuse_graphs { -HashGraph::HashGraph(const HashGraphParams & params) -: problem_options_(params.problem_options) +HashGraph::HashGraph(const HashGraphParams& params) : problem_options_(params.problem_options) { // Set Ceres loss function ownership according to the fuse_core::Loss specification problem_options_.loss_function_ownership = fuse_core::Loss::Ownership; } -HashGraph::HashGraph(const HashGraph & other) -: constraints_by_variable_uuid_(other.constraints_by_variable_uuid_), - problem_options_(other.problem_options_), - variables_on_hold_(other.variables_on_hold_) +HashGraph::HashGraph(const HashGraph& other) + : constraints_by_variable_uuid_(other.constraints_by_variable_uuid_) + , problem_options_(other.problem_options_) + , variables_on_hold_(other.variables_on_hold_) { // 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()}; - }); // NOLINT(whitespace/braces) + 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() }; + }); // 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()}; - }); // NOLINT(whitespace/braces) + 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() }; + }); // NOLINT(whitespace/braces) } -HashGraph & HashGraph::operator=(const HashGraph & other) +HashGraph& HashGraph::operator=(const HashGraph& other) { // Make a copy (might throw an exception) HashGraph tmp(other); @@ -108,7 +99,7 @@ fuse_core::Graph::UniquePtr HashGraph::clone() const return HashGraph::make_unique(*this); } -bool HashGraph::constraintExists(const fuse_core::UUID & constraint_uuid) const noexcept +bool HashGraph::constraintExists(const fuse_core::UUID& constraint_uuid) const noexcept { // map.find() does not itself throw exceptions, but may as a result of the key comparison // operator. Because the UUID comparisons are marked as noexcept by Boost, I feel safe marking @@ -120,103 +111,101 @@ bool HashGraph::constraintExists(const fuse_core::UUID & constraint_uuid) const bool HashGraph::addConstraint(fuse_core::Constraint::SharedPtr constraint) { // Do nothing if the constraint is empty, or the constraint already exists - if (!constraint || constraintExists(constraint->uuid())) { + if (!constraint || constraintExists(constraint->uuid())) + { return false; } // Check that all of the referenced variables exist. Throw a logic_error if they do not. - for (const auto & variable_uuid : constraint->variables()) { - if (!variableExists(variable_uuid)) { - throw std::logic_error( - "Attempting to add a constraint (" + fuse_core::uuid::to_string(constraint->uuid()) + - ") that uses an unknown variable (" + fuse_core::uuid::to_string(variable_uuid) + - ")."); + for (const auto& variable_uuid : constraint->variables()) + { + if (!variableExists(variable_uuid)) + { + throw std::logic_error("Attempting to add a constraint (" + fuse_core::uuid::to_string(constraint->uuid()) + + ") that uses an unknown variable (" + fuse_core::uuid::to_string(variable_uuid) + ")."); } } // Add the constraint to the list of known constraints constraints_.emplace(constraint->uuid(), constraint); // Also add it to the variable-constraint cross reference - for (const auto & variable_uuid : constraint->variables()) { + for (const auto& variable_uuid : constraint->variables()) + { constraints_by_variable_uuid_[variable_uuid].push_back(constraint->uuid()); } return true; } -bool HashGraph::removeConstraint(const fuse_core::UUID & constraint_uuid) +bool HashGraph::removeConstraint(const fuse_core::UUID& constraint_uuid) { // Check if the constraint exists auto constraints_iter = constraints_.find(constraint_uuid); - if (constraints_iter == constraints_.end()) { + if (constraints_iter == constraints_.end()) + { return false; } // Remove the constraint from the cross-reference data structure - for (const auto & variable_uuid : constraints_iter->second->variables()) { - auto & constraints = constraints_by_variable_uuid_.at(variable_uuid); - constraints.erase( - std::remove( - constraints.begin(), - constraints.end(), constraint_uuid), constraints.end()); + for (const auto& variable_uuid : constraints_iter->second->variables()) + { + auto& constraints = constraints_by_variable_uuid_.at(variable_uuid); + constraints.erase(std::remove(constraints.begin(), constraints.end(), constraint_uuid), constraints.end()); } // And remove the constraint constraints_.erase(constraints_iter); // This does not throw return true; } -const fuse_core::Constraint & HashGraph::getConstraint(const fuse_core::UUID & constraint_uuid) -const +const fuse_core::Constraint& HashGraph::getConstraint(const fuse_core::UUID& constraint_uuid) const { auto constraints_iter = constraints_.find(constraint_uuid); - if (constraints_iter == constraints_.end()) { - throw std::out_of_range( - "The constraint UUID " + fuse_core::uuid::to_string( - constraint_uuid) + " does not exist."); + if (constraints_iter == constraints_.end()) + { + throw std::out_of_range("The constraint UUID " + fuse_core::uuid::to_string(constraint_uuid) + " does not exist."); } return *constraints_iter->second; } 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 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)); + 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 +fuse_core::Graph::const_constraint_range HashGraph::getConnectedConstraints(const fuse_core::UUID& variable_uuid) const { 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 = - [this](const fuse_core::UUID & constraint_uuid) -> const fuse_core::Constraint & - { - return this->getConstraint(constraint_uuid); - }; + if (cross_reference_iter != constraints_by_variable_uuid_.end()) + { + std::function 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; + 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)); - } else if (variableExists(variable_uuid)) { + 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)) + { // User requested a valid variable, but there are no attached constraints. Return an empty // range. return fuse_core::Graph::const_constraint_range(); - } else { + } + 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."); + 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 +bool HashGraph::variableExists(const fuse_core::UUID& variable_uuid) const noexcept { auto variables_iter = variables_.find(variable_uuid); return variables_iter != variables_.end(); @@ -225,92 +214,90 @@ bool HashGraph::variableExists(const fuse_core::UUID & variable_uuid) const noex bool HashGraph::addVariable(fuse_core::Variable::SharedPtr variable) { // Do nothing if the variable is empty, or the variable already exists - if (!variable || variableExists(variable->uuid())) { + if (!variable || variableExists(variable->uuid())) + { return false; } variables_.emplace(variable->uuid(), variable); - if (variable->holdConstant()) { + if (variable->holdConstant()) + { variables_on_hold_.insert(variable->uuid()); } return true; } -bool HashGraph::removeVariable(const fuse_core::UUID & variable_uuid) +bool HashGraph::removeVariable(const fuse_core::UUID& variable_uuid) { // Check if the variable exists auto variables_iter = variables_.find(variable_uuid); - if (variables_iter == variables_.end()) { + if (variables_iter == variables_.end()) + { return false; } // Check that this variable is not used by any constraint. Throw a logic_error if the variable is // currently used. auto cross_reference_iter = constraints_by_variable_uuid_.find(variable_uuid); - if (cross_reference_iter != constraints_by_variable_uuid_.end() && - !cross_reference_iter->second.empty()) + if (cross_reference_iter != constraints_by_variable_uuid_.end() && !cross_reference_iter->second.empty()) { - throw std::logic_error( - "Attempting to remove a variable (" + fuse_core::uuid::to_string(variable_uuid) + - ") that is used by existing constraints (" + - fuse_core::uuid::to_string(cross_reference_iter->second.front()) + - " plus " + std::to_string(cross_reference_iter->second.size() - 1) + " others)."); + throw std::logic_error("Attempting to remove a variable (" + fuse_core::uuid::to_string(variable_uuid) + + ") that is used by existing constraints (" + + fuse_core::uuid::to_string(cross_reference_iter->second.front()) + " plus " + + std::to_string(cross_reference_iter->second.size() - 1) + " others)."); } // Remove the variable from all containers variables_.erase(variables_iter); // Does not throw - if (cross_reference_iter != constraints_by_variable_uuid_.end()) { + if (cross_reference_iter != constraints_by_variable_uuid_.end()) + { constraints_by_variable_uuid_.erase(cross_reference_iter); } variables_on_hold_.erase(variable_uuid); return true; } -const fuse_core::Variable & HashGraph::getVariable(const fuse_core::UUID & variable_uuid) const +const fuse_core::Variable& HashGraph::getVariable(const fuse_core::UUID& variable_uuid) const { auto variables_iter = variables_.find(variable_uuid); - if (variables_iter == variables_.end()) { - throw std::out_of_range( - "The variable UUID " + fuse_core::uuid::to_string( - variable_uuid) + " does not exist."); + if (variables_iter == variables_.end()) + { + throw std::out_of_range("The variable UUID " + fuse_core::uuid::to_string(variable_uuid) + " does not exist."); } return *variables_iter->second; } 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 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 fuse_core::Graph::const_variable_range(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) +void HashGraph::holdVariable(const fuse_core::UUID& variable_uuid, bool hold_constant) { // Adjust the variable setting in the Ceres Problem object - if (hold_constant) { + if (hold_constant) + { variables_on_hold_.insert(variable_uuid); - } else { + } + else + { variables_on_hold_.erase(variable_uuid); } } -bool HashGraph::isVariableOnHold(const fuse_core::UUID & variable_uuid) const +bool HashGraph::isVariableOnHold(const fuse_core::UUID& variable_uuid) const { return variables_on_hold_.find(variable_uuid) != variables_on_hold_.end(); } -void HashGraph::getCovariance( - const std::vector> & covariance_requests, - std::vector> & covariance_matrices, - const ceres::Covariance::Options & options, - const bool use_tangent_space) const +void HashGraph::getCovariance(const std::vector>& covariance_requests, + std::vector>& covariance_matrices, + const ceres::Covariance::Options& options, const bool use_tangent_space) const { // Avoid doing a bunch of work if the request is empty - if (covariance_requests.empty()) { + if (covariance_requests.empty()) + { return; } // Construct the ceres::Problem object from scratch @@ -319,103 +306,96 @@ void HashGraph::getCovariance( // The Ceres interface requires that the variable pairs not contain duplicates. Since the // covariance matrix is symmetric, requesting Cov(A,B) and Cov(B,A) counts as a duplicate. Create // an expression to test a pair of data pointers such that (A,B) == (A,B) OR (B,A) - auto symmetric_equal = [](const std::pair & x, - const std::pair & y) - { - return ((x.first == y.first) && (x.second == y.second)) || - ((x.first == y.second) && (x.second == y.first)); - }; + auto symmetric_equal = [](const std::pair& x, + const std::pair& y) { + return ((x.first == y.first) && (x.second == y.second)) || ((x.first == y.second) && (x.second == y.first)); + }; // Convert the covariance requests into the input structure needed by Ceres. Namely, we must // convert the variable UUIDs into memory addresses. We create two containers of covariance // blocks: one only contains the unique variable pairs that we give to Ceres, and a second that // contains all requested variable pairs used to keep the output structure in sync with the // request structure. - std::vector> unique_covariance_blocks; - std::vector> all_covariance_blocks; + std::vector> unique_covariance_blocks; + std::vector> all_covariance_blocks; all_covariance_blocks.resize(covariance_requests.size()); covariance_matrices.resize(covariance_requests.size()); - for (size_t i = 0; i < covariance_requests.size(); ++i) { - const auto & request = covariance_requests.at(i); + for (size_t i = 0; i < covariance_requests.size(); ++i) + { + const auto& request = covariance_requests.at(i); auto variable1_iter = variables_.find(request.first); - if (variable1_iter == variables_.end()) { - throw std::out_of_range( - "The variable UUID " + fuse_core::uuid::to_string(request.first) + - " does not exist."); + if (variable1_iter == variables_.end()) + { + throw std::out_of_range("The variable UUID " + fuse_core::uuid::to_string(request.first) + " does not exist."); } auto variable2_iter = variables_.find(request.second); - if (variable2_iter == variables_.end()) { - throw std::out_of_range( - "The variable UUID " + fuse_core::uuid::to_string(request.second) + - " does not exist."); + if (variable2_iter == variables_.end()) + { + throw std::out_of_range("The variable UUID " + fuse_core::uuid::to_string(request.second) + " does not exist."); } // Both variables exist. Continue processing. // Create the output covariance matrix - if (use_tangent_space) { - covariance_matrices[i].resize( - variable1_iter->second->localSize() * variable2_iter->second->localSize()); - } else { - covariance_matrices[i].resize( - variable1_iter->second->size() * - variable2_iter->second->size()); + if (use_tangent_space) + { + covariance_matrices[i].resize(variable1_iter->second->localSize() * variable2_iter->second->localSize()); + } + else + { + covariance_matrices[i].resize(variable1_iter->second->size() * variable2_iter->second->size()); } // Add this covariance block to the container of all covariance blocks. This container is in // sync with the covariance_requests vector. - auto & block = all_covariance_blocks.at(i); + auto& block = all_covariance_blocks.at(i); block.first = variable1_iter->second->data(); block.second = variable2_iter->second->data(); // Also maintain a container of unique covariance blocks. Since the covariance matrix is // symmetric, requesting Cov(X,Y) and Cov(Y,X) counts as a duplicate, so we use our special // symmetric_equal function to test. - if (std::none_of( - unique_covariance_blocks.begin(), - unique_covariance_blocks.end(), - std::bind(symmetric_equal, block, std::placeholders::_1))) + if (std::none_of(unique_covariance_blocks.begin(), unique_covariance_blocks.end(), + std::bind(symmetric_equal, block, std::placeholders::_1))) { unique_covariance_blocks.push_back(block); } } // Call the Ceres function to compute the unique set of requested covariance blocks ceres::Covariance covariance(options); - if (!covariance.Compute(unique_covariance_blocks, &problem)) { + if (!covariance.Compute(unique_covariance_blocks, &problem)) + { throw std::runtime_error("Could not compute requested covariance blocks."); } // Populate the computed covariance blocks into the output variable. - if (use_tangent_space) { - for (size_t i = 0; i < covariance_requests.size(); ++i) { - const auto & block = all_covariance_blocks.at(i); - auto & output_matrix = covariance_matrices.at(i); - if (!covariance.GetCovarianceBlockInTangentSpace( - block.first, - block.second, - output_matrix.data())) + if (use_tangent_space) + { + for (size_t i = 0; i < covariance_requests.size(); ++i) + { + const auto& block = all_covariance_blocks.at(i); + auto& output_matrix = covariance_matrices.at(i); + if (!covariance.GetCovarianceBlockInTangentSpace(block.first, block.second, output_matrix.data())) { - const auto & request = covariance_requests.at(i); - throw std::runtime_error( - "Could not get covariance block for variable UUIDs " + - fuse_core::uuid::to_string(request.first) + " and " + - fuse_core::uuid::to_string(request.second) + "."); + const auto& request = covariance_requests.at(i); + throw std::runtime_error("Could not get covariance block for variable UUIDs " + + fuse_core::uuid::to_string(request.first) + " and " + + fuse_core::uuid::to_string(request.second) + "."); } } - } else { - for (size_t i = 0; i < covariance_requests.size(); ++i) { - const auto & block = all_covariance_blocks.at(i); - auto & output_matrix = covariance_matrices.at(i); - if (!covariance.GetCovarianceBlock( - block.first, - block.second, - output_matrix.data())) + } + else + { + for (size_t i = 0; i < covariance_requests.size(); ++i) + { + const auto& block = all_covariance_blocks.at(i); + auto& output_matrix = covariance_matrices.at(i); + if (!covariance.GetCovarianceBlock(block.first, block.second, output_matrix.data())) { - const auto & request = covariance_requests.at(i); - throw std::runtime_error( - "Could not get covariance block for variable UUIDs " + - fuse_core::uuid::to_string(request.first) + " and " + - fuse_core::uuid::to_string(request.second) + "."); + const auto& request = covariance_requests.at(i); + throw std::runtime_error("Could not get covariance block for variable UUIDs " + + fuse_core::uuid::to_string(request.first) + " and " + + fuse_core::uuid::to_string(request.second) + "."); } } } } -ceres::Solver::Summary HashGraph::optimize(const ceres::Solver::Options & options) +ceres::Solver::Summary HashGraph::optimize(const ceres::Solver::Options& options) { // Construct the ceres::Problem object from scratch ceres::Problem problem(problem_options_); @@ -427,10 +407,8 @@ ceres::Solver::Summary HashGraph::optimize(const ceres::Solver::Options & option return summary; } -ceres::Solver::Summary HashGraph::optimizeFor( - const rclcpp::Duration & max_optimization_time, - const ceres::Solver::Options & options, - rclcpp::Clock clock) +ceres::Solver::Summary HashGraph::optimizeFor(const rclcpp::Duration& max_optimization_time, + const ceres::Solver::Options& options, rclcpp::Clock clock) { auto start = clock.now(); @@ -452,9 +430,8 @@ ceres::Solver::Summary HashGraph::optimizeFor( return summary; } -bool HashGraph::evaluate( - double * cost, std::vector * residuals, std::vector * gradient, - const ceres::Problem::EvaluateOptions & options) const +bool HashGraph::evaluate(double* cost, std::vector* residuals, std::vector* gradient, + const ceres::Problem::EvaluateOptions& options) const { ceres::Problem problem(problem_options_); createProblem(problem); @@ -462,15 +439,17 @@ bool HashGraph::evaluate( return problem.Evaluate(options, cost, residuals, gradient, nullptr); } -void HashGraph::print(std::ostream & stream) const +void HashGraph::print(std::ostream& stream) const { stream << "HashGraph\n" << " constraints:\n"; - for (const auto & constraint : constraints_) { + for (const auto& constraint : constraints_) + { stream << " - " << *constraint.second << "\n"; } stream << " variables:\n"; - for (const auto & variable : variables_) { + for (const auto& variable : variables_) + { const auto is_on_hold = variables_on_hold_.find(variable.first) != variables_on_hold_.end(); stream << " - " << *variable.second << "\n" @@ -478,49 +457,51 @@ void HashGraph::print(std::ostream & stream) const } } -void HashGraph::createProblem(ceres::Problem & problem) const +void HashGraph::createProblem(ceres::Problem& problem) const { // Add all the variables to the problem - for (auto & uuid__variable : variables_) { - fuse_core::Variable & variable = *(uuid__variable.second); - problem.AddParameterBlock( - variable.data(), - variable.size(), + for (auto& uuid__variable : variables_) + { + fuse_core::Variable& variable = *(uuid__variable.second); + problem.AddParameterBlock(variable.data(), variable.size(), #if !CERES_SUPPORTS_MANIFOLDS - variable.localParameterization()); + variable.localParameterization()); #else - variable.manifold()); + variable.manifold()); #endif // Handle optimization bounds - for (size_t index = 0; index < variable.size(); ++index) { + for (size_t index = 0; index < variable.size(); ++index) + { auto lower_bound = variable.lowerBound(index); - if (lower_bound > std::numeric_limits::lowest()) { + if (lower_bound > std::numeric_limits::lowest()) + { problem.SetParameterLowerBound(variable.data(), index, lower_bound); } auto upper_bound = variable.upperBound(index); - if (upper_bound < std::numeric_limits::max()) { + if (upper_bound < std::numeric_limits::max()) + { problem.SetParameterUpperBound(variable.data(), index, upper_bound); } } // Handle variables that are held constant - if (variables_on_hold_.find(variable.uuid()) != variables_on_hold_.end()) { + if (variables_on_hold_.find(variable.uuid()) != variables_on_hold_.end()) + { problem.SetParameterBlockConstant(variable.data()); } } // Add the constraints - std::vector parameter_blocks; - for (auto & uuid__constraint : constraints_) { - fuse_core::Constraint & constraint = *(uuid__constraint.second); + std::vector parameter_blocks; + for (auto& uuid__constraint : constraints_) + { + fuse_core::Constraint& 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()); - for (const auto & uuid : constraint.variables()) { + for (const auto& uuid : constraint.variables()) + { parameter_blocks.push_back(variables_.at(uuid)->data()); } - problem.AddResidualBlock( - constraint.costFunction(), - constraint.lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint.costFunction(), constraint.lossFunction(), parameter_blocks); } } diff --git a/fuse_graphs/test/CMakeLists.txt b/fuse_graphs/test/CMakeLists.txt index c6c8775e3..a5e6c704a 100644 --- a/fuse_graphs/test/CMakeLists.txt +++ b/fuse_graphs/test/CMakeLists.txt @@ -1,3 +1,4 @@ -# CORE GTESTS ====================================================================================== +# CORE GTESTS +# ====================================================================================== ament_add_gtest(test_hash_graph test_hash_graph.cpp) target_link_libraries(test_hash_graph ${PROJECT_NAME}) diff --git a/fuse_graphs/test/covariance_constraint.hpp b/fuse_graphs/test/covariance_constraint.hpp index 4ff6c8b17..a63a9cf09 100644 --- a/fuse_graphs/test/covariance_constraint.hpp +++ b/fuse_graphs/test/covariance_constraint.hpp @@ -48,9 +48,8 @@ #include #include - /** - * @brief Create a cost fuction that implements one of the Ceres unit tests + * @brief Create a cost function that implements one of the Ceres unit tests * * UnaryCostFunctions are added to all three variables * - https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/covariance_test.cc#L423 @@ -71,10 +70,7 @@ class CovarianceCostFunction : public ceres::CostFunction mutable_parameter_block_sizes()->push_back(1); } - bool Evaluate( - double const * const * /*parameters*/, - double * residuals, - double ** jacobians) const override + bool Evaluate(double const* const* /*parameters*/, double* residuals, double** jacobians) const override { residuals[0] = 1; residuals[1] = 1; @@ -85,47 +81,23 @@ class CovarianceCostFunction : public ceres::CostFunction residuals[6] = 2; residuals[7] = 2; - if (jacobians != NULL) { - if (jacobians[0] != NULL) { - static const double jacobian0[] = - { - 1.0, 0.0, - 0.0, 1.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - 0.0, 0.0, - -5.0, -6.0, - 3.0, -2.0 - }; + if (jacobians != NULL) + { + if (jacobians[0] != NULL) + { + static const double jacobian0[] = { 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, -5.0, -6.0, 3.0, -2.0 }; std::copy(jacobian0, jacobian0 + 16, jacobians[0]); } - if (jacobians[1] != NULL) { - static const double jacobian1[] = - { - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 2.0, 0.0, 0.0, - 0.0, 2.0, 0.0, - 0.0, 0.0, 2.0, - 0.0, 0.0, 0.0, - 1.0, 2.0, 3.0, - 0.0, 0.0, 0.0 - }; + if (jacobians[1] != NULL) + { + static const double jacobian1[] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, + 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0 }; std::copy(jacobian1, jacobian1 + 24, jacobians[1]); } - if (jacobians[2] != NULL) { - static const double jacobian2[] = - { - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 5.0, - 0.0, - 2.0 - }; + if (jacobians[2] != NULL) + { + static const double jacobian2[] = { 0.0, 0.0, 0.0, 0.0, 0.0, 5.0, 0.0, 2.0 }; std::copy(jacobian2, jacobian2 + 8, jacobians[2]); } } @@ -144,17 +116,19 @@ class CovarianceConstraint : public fuse_core::Constraint CovarianceConstraint() = default; - CovarianceConstraint( - const std::string & source, - const fuse_core::UUID & variable1_uuid, - const fuse_core::UUID & variable2_uuid, - const fuse_core::UUID & variable3_uuid) - : fuse_core::Constraint(source, {variable1_uuid, variable2_uuid, variable3_uuid}) + CovarianceConstraint(const std::string& source, const fuse_core::UUID& variable1_uuid, + const fuse_core::UUID& variable2_uuid, const fuse_core::UUID& variable3_uuid) + : fuse_core::Constraint(source, { variable1_uuid, variable2_uuid, variable3_uuid }) { } - void print(std::ostream & /*stream = std::cout*/) const override {} - ceres::CostFunction * costFunction() const override {return new CovarianceCostFunction();} + void print(std::ostream& /*stream = std::cout*/) const override + { + } + ceres::CostFunction* costFunction() const override + { + return new CovarianceCostFunction(); + } private: // Allow Boost Serialization access to private methods @@ -167,10 +141,10 @@ class CovarianceConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_graphs/test/example_constraint.hpp b/fuse_graphs/test/example_constraint.hpp index 45d222d73..bbaa07937 100644 --- a/fuse_graphs/test/example_constraint.hpp +++ b/fuse_graphs/test/example_constraint.hpp @@ -47,20 +47,18 @@ #include #include - /** * @brief Dummy cost function used for testing */ class ExampleFunctor { public: - explicit ExampleFunctor(const double & b) - : b_(b) + explicit ExampleFunctor(const double& b) : b_(b) { } - template - bool operator()(const T * const variable, T * residual) const + template + bool operator()(const T* const variable, T* residual) const { residual[0] = variable[0] - T(b_); return true; @@ -80,14 +78,17 @@ class ExampleConstraint : public fuse_core::Constraint ExampleConstraint() = default; - explicit ExampleConstraint(const std::string & source, const fuse_core::UUID & variable_uuid) - : fuse_core::Constraint(source, {variable_uuid}), // NOLINT + explicit ExampleConstraint(const std::string& source, const fuse_core::UUID& variable_uuid) + : fuse_core::Constraint(source, { variable_uuid }) + , // NOLINT data(0.0) { } - void print(std::ostream & /*stream = std::cout*/) const override {} - ceres::CostFunction * costFunction() const override + void print(std::ostream& /*stream = std::cout*/) const override + { + } + ceres::CostFunction* costFunction() const override { return new ceres::AutoDiffCostFunction(new ExampleFunctor(data)); } @@ -105,11 +106,11 @@ class ExampleConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data; + archive& boost::serialization::base_object(*this); + archive& data; } }; diff --git a/fuse_graphs/test/example_loss.hpp b/fuse_graphs/test/example_loss.hpp index bdffbb09c..fdc8ea883 100644 --- a/fuse_graphs/test/example_loss.hpp +++ b/fuse_graphs/test/example_loss.hpp @@ -45,7 +45,6 @@ #include #include - /** * @brief Dummy loss implementation for testing */ @@ -54,27 +53,27 @@ class ExampleLoss : public fuse_core::Loss public: FUSE_LOSS_DEFINITIONS(ExampleLoss) - explicit ExampleLoss(const double a = 1.0) - : a(a) + explicit ExampleLoss(const double a = 1.0) : a(a) { } void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - >/*interfaces*/, - const std::string & /*name*/) override {} + fuse_core::node_interfaces::NodeInterfaces /*interfaces*/, + const std::string& /*name*/) override + { + } - void print(std::ostream & /*stream = std::cout*/) const override {} + void print(std::ostream& /*stream = std::cout*/) const override + { + } - ceres::LossFunction * lossFunction() const override + ceres::LossFunction* lossFunction() const override { return new ceres::HuberLoss(a); } - double a{1.0}; //!< Public member variable just for testing + double a{ 1.0 }; //!< Public member variable just for testing private: // Allow Boost Serialization access to private methods @@ -87,11 +86,11 @@ class ExampleLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a; + archive& boost::serialization::base_object(*this); + archive& a; } }; diff --git a/fuse_graphs/test/example_variable.hpp b/fuse_graphs/test/example_variable.hpp index eccfd56fd..ba0bdca6e 100644 --- a/fuse_graphs/test/example_variable.hpp +++ b/fuse_graphs/test/example_variable.hpp @@ -45,7 +45,6 @@ #include #include - /** * @brief Dummy variable implementation for testing */ @@ -54,16 +53,25 @@ class ExampleVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(ExampleVariable) - explicit ExampleVariable(size_t N = 1) - : fuse_core::Variable(fuse_core::uuid::generate()), - data_(N, 0.0) + explicit ExampleVariable(size_t N = 1) : fuse_core::Variable(fuse_core::uuid::generate()), data_(N, 0.0) { } - size_t size() const override {return data_.size();} - const double * data() const override {return data_.data();} - double * data() override {return data_.data();} - void print(std::ostream & /*stream = std::cout*/) const override {} + size_t size() const override + { + return data_.size(); + } + const double* data() const override + { + return data_.data(); + } + double* data() override + { + return data_.data(); + } + void print(std::ostream& /*stream = std::cout*/) const override + { + } private: std::vector data_; @@ -78,11 +86,11 @@ class ExampleVariable : public fuse_core::Variable * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; diff --git a/fuse_graphs/test/test_hash_graph.cpp b/fuse_graphs/test/test_hash_graph.cpp index 97c6cfc83..484f01dfb 100644 --- a/fuse_graphs/test/test_hash_graph.cpp +++ b/fuse_graphs/test/test_hash_graph.cpp @@ -65,36 +65,51 @@ class HashGraphTestFixture : public ::testing::Test * @param[in] actual - The actual variable * @return True if all the properties match, false otherwise */ - bool compareVariables(const fuse_core::Variable & expected, const fuse_core::Variable & actual) + bool compareVariables(const fuse_core::Variable& expected, const fuse_core::Variable& actual) { failure_description = ""; bool variables_equal = true; - if (expected.type() != actual.type()) { + if (expected.type() != actual.type()) + { variables_equal = false; failure_description += "The variables have different types.\n" - " expected type is '" + expected.type() + "'\n" - " actual type is '" + actual.type() + "'\n"; + " expected type is '" + + expected.type() + + "'\n" + " actual type is '" + + actual.type() + "'\n"; } - if (expected.size() != actual.size()) { + if (expected.size() != actual.size()) + { variables_equal = false; failure_description += "The variables have different sizes.\n" - " expected size is '" + std::to_string(expected.size()) + "'\n" - " actual size is '" + std::to_string(actual.size()) + "'\n"; + " expected size is '" + + std::to_string(expected.size()) + + "'\n" + " actual size is '" + + std::to_string(actual.size()) + "'\n"; } - if (expected.uuid() != actual.uuid()) { + if (expected.uuid() != actual.uuid()) + { variables_equal = false; failure_description += "The variables have different UUIDs.\n" - " expected UUID is '" + fuse_core::uuid::to_string(expected.uuid()) + "'\n" - " actual UUID is '" + fuse_core::uuid::to_string(actual.uuid()) + "'\n"; + " expected UUID is '" + + fuse_core::uuid::to_string(expected.uuid()) + + "'\n" + " actual UUID is '" + + fuse_core::uuid::to_string(actual.uuid()) + "'\n"; } - for (size_t i = 0; i < expected.size(); ++i) { - if (expected.data()[i] != actual.data()[i]) { + for (size_t i = 0; i < expected.size(); ++i) + { + if (expected.data()[i] != actual.data()[i]) + { variables_equal = false; failure_description += "The variables have different values.\n" - " expected data(" + std::to_string(i) + ") is '" + std::to_string(expected.data()[i]) + - "'\n" - " actual data(" + std::to_string(i) + ") is '" + std::to_string(actual.data()[i]) + - "'\n"; + " expected data(" + + std::to_string(i) + ") is '" + std::to_string(expected.data()[i]) + + "'\n" + " actual data(" + + std::to_string(i) + ") is '" + std::to_string(actual.data()[i]) + "'\n"; } } return variables_equal; @@ -107,46 +122,59 @@ class HashGraphTestFixture : public ::testing::Test * @param[in] actual - The actual constraint * @return True if all the properties match, false otherwise */ - bool compareConstraints( - const fuse_core::Constraint & expected, - const fuse_core::Constraint & actual) + bool compareConstraints(const fuse_core::Constraint& expected, const fuse_core::Constraint& actual) { failure_description = ""; bool constraints_equal = true; - if (expected.type() != actual.type()) { + if (expected.type() != actual.type()) + { constraints_equal = false; failure_description += "The constraints have different types.\n" - " expected type is '" + expected.type() + "'\n" - " actual type is '" + actual.type() + "'\n"; + " expected type is '" + + expected.type() + + "'\n" + " actual type is '" + + actual.type() + "'\n"; } - if (expected.uuid() != actual.uuid()) { + if (expected.uuid() != actual.uuid()) + { constraints_equal = false; failure_description += "The constraints have different UUIDs.\n" - " expected UUID is '" + fuse_core::uuid::to_string(expected.uuid()) + "'\n" - " actual UUID is '" + fuse_core::uuid::to_string(actual.uuid()) + "'\n"; + " expected UUID is '" + + fuse_core::uuid::to_string(expected.uuid()) + + "'\n" + " actual UUID is '" + + fuse_core::uuid::to_string(actual.uuid()) + "'\n"; } - if (expected.variables().size() != actual.variables().size()) { + if (expected.variables().size() != actual.variables().size()) + { constraints_equal = false; failure_description += "The constraints involve a different number of variables.\n" - " expected variable count is '" + std::to_string(expected.variables().size()) + "'\n" - " actual variable count is '" + std::to_string(actual.variables().size()) + "'\n"; + " expected variable count is '" + + std::to_string(expected.variables().size()) + + "'\n" + " actual variable count is '" + + std::to_string(actual.variables().size()) + "'\n"; } - for (size_t i = 0; i < expected.variables().size(); ++i) { - if (expected.variables().at(i) != actual.variables().at(i)) { + for (size_t i = 0; i < expected.variables().size(); ++i) + { + if (expected.variables().at(i) != actual.variables().at(i)) + { constraints_equal = false; std::string i_str = std::to_string(i); failure_description += "The constraints involve different variable UUIDs.\n" - " expected variables(" + i_str + ") is '" + fuse_core::uuid::to_string( - expected.variables()[i]) + "'\n" - " actual variables(" + i_str + ") is '" + fuse_core::uuid::to_string( - actual.variables()[i]) + "'\n"; + " expected variables(" + + i_str + ") is '" + fuse_core::uuid::to_string(expected.variables()[i]) + + "'\n" + " actual variables(" + + i_str + ") is '" + fuse_core::uuid::to_string(actual.variables()[i]) + "'\n"; } } return constraints_equal; } - std::string failure_description {""}; //!< The last failure description. Empty if no failure - //!< happened + std::string failure_description{ "" }; //!< The last failure description. Empty if no failure + //!< happened }; TEST_F(HashGraphTestFixture, AddVariable) @@ -272,10 +300,10 @@ TEST_F(HashGraphTestFixture, GetVariable) graph.addVariable(variable2); // Verify all of the variables are available - const fuse_core::Variable & actual1 = graph.getVariable(variable1->uuid()); + const fuse_core::Variable& actual1 = graph.getVariable(variable1->uuid()); EXPECT_TRUE(compareVariables(*variable1, actual1)) << failure_description; - const fuse_core::Variable & actual2 = graph.getVariable(variable2->uuid()); + const fuse_core::Variable& actual2 = graph.getVariable(variable2->uuid()); EXPECT_TRUE(compareVariables(*variable2, actual2)) << failure_description; } @@ -304,16 +332,20 @@ TEST_F(HashGraphTestFixture, GetVariables) ASSERT_EQ(3, std::distance(variables.begin(), variables.end())); // Verify we received the correct variables - for (const auto & actual : variables) { - if (actual.uuid() == variable1->uuid()) { + for (const auto& actual : variables) + { + if (actual.uuid() == variable1->uuid()) + { EXPECT_TRUE(compareVariables(*variable1, actual)) << failure_description; continue; } - if (actual.uuid() == variable2->uuid()) { + if (actual.uuid() == variable2->uuid()) + { EXPECT_TRUE(compareVariables(*variable2, actual)) << failure_description; continue; } - if (actual.uuid() == variable3->uuid()) { + if (actual.uuid() == variable3->uuid()) + { EXPECT_TRUE(compareVariables(*variable3, actual)) << failure_description; continue; } @@ -352,8 +384,10 @@ TEST_F(HashGraphTestFixture, GetConnectedVariables) { auto actual_variables = graph.getConnectedVariables(constraint1->uuid()); ASSERT_EQ(1, std::distance(actual_variables.begin(), actual_variables.end())); - for (const auto & actual : actual_variables) { - if (actual.uuid() == variable1->uuid()) { + for (const auto& actual : actual_variables) + { + if (actual.uuid() == variable1->uuid()) + { EXPECT_TRUE(compareVariables(*variable1, actual)) << failure_description; continue; } @@ -365,8 +399,10 @@ TEST_F(HashGraphTestFixture, GetConnectedVariables) { auto actual_variables = graph.getConnectedVariables(constraint2->uuid()); ASSERT_EQ(1, std::distance(actual_variables.begin(), actual_variables.end())); - for (const auto & actual : actual_variables) { - if (actual.uuid() == variable2->uuid()) { + for (const auto& actual : actual_variables) + { + if (actual.uuid() == variable2->uuid()) + { EXPECT_TRUE(compareVariables(*variable2, actual)) << failure_description; continue; } @@ -511,10 +547,10 @@ TEST_F(HashGraphTestFixture, GetConstraint) graph.addConstraint(constraint2); // Verify all of the constraints are available - const fuse_core::Constraint & actual1 = graph.getConstraint(constraint1->uuid()); + const fuse_core::Constraint& actual1 = graph.getConstraint(constraint1->uuid()); EXPECT_TRUE(compareConstraints(*constraint1, actual1)) << failure_description; - const fuse_core::Constraint & actual2 = graph.getConstraint(constraint2->uuid()); + const fuse_core::Constraint& actual2 = graph.getConstraint(constraint2->uuid()); EXPECT_TRUE(compareConstraints(*constraint2, actual2)) << failure_description; } @@ -547,16 +583,20 @@ TEST_F(HashGraphTestFixture, GetConstraints) ASSERT_EQ(3, std::distance(constraints.begin(), constraints.end())); // Verify we received the correct constraints - for (const auto & actual : constraints) { - if (actual.uuid() == constraint1->uuid()) { + for (const auto& actual : constraints) + { + if (actual.uuid() == constraint1->uuid()) + { EXPECT_TRUE(compareConstraints(*constraint1, actual)) << failure_description; continue; } - if (actual.uuid() == constraint2->uuid()) { + if (actual.uuid() == constraint2->uuid()) + { EXPECT_TRUE(compareConstraints(*constraint2, actual)) << failure_description; continue; } - if (actual.uuid() == constraint3->uuid()) { + if (actual.uuid() == constraint3->uuid()) + { EXPECT_TRUE(compareConstraints(*constraint3, actual)) << failure_description; continue; } @@ -598,32 +638,35 @@ TEST_F(HashGraphTestFixture, GetConnectedConstraints) { auto actual_constraints = graph.getConnectedConstraints(variable1->uuid()); ASSERT_EQ(2, std::distance(actual_constraints.begin(), actual_constraints.end())); - for (const auto & actual : actual_constraints) { - if (actual.uuid() == constraint1->uuid()) { + for (const auto& actual : actual_constraints) + { + if (actual.uuid() == constraint1->uuid()) + { EXPECT_TRUE(compareConstraints(*constraint1, actual)) << failure_description; continue; } - if (actual.uuid() == constraint3->uuid()) { + if (actual.uuid() == constraint3->uuid()) + { EXPECT_TRUE(compareConstraints(*constraint3, actual)) << failure_description; continue; } // The constraint was not one of the expected constraints. Fail the test. - FAIL() << "The actual constraint '" << actual.uuid() << - "' was not in the expected collection."; + FAIL() << "The actual constraint '" << actual.uuid() << "' was not in the expected collection."; } } { auto actual_constraints = graph.getConnectedConstraints(variable2->uuid()); ASSERT_EQ(1, std::distance(actual_constraints.begin(), actual_constraints.end())); - for (const auto & actual : actual_constraints) { - if (actual.uuid() == constraint2->uuid()) { + for (const auto& actual : actual_constraints) + { + if (actual.uuid() == constraint2->uuid()) + { EXPECT_TRUE(compareConstraints(*constraint2, actual)) << failure_description; continue; } // The constraint was not one of the expected constraints. Fail the test. - FAIL() << "The actual constraint '" << actual.uuid() << - "' was not in the expected collection."; + FAIL() << "The actual constraint '" << actual.uuid() << "' was not in the expected collection."; } } @@ -773,11 +816,11 @@ TEST_F(HashGraphTestFixture, GetCovariance) covariance_requests.emplace_back(z->uuid(), y->uuid()); std::vector> covariance_matrices; graph.getCovariance(covariance_requests, covariance_matrices); - const std::vector & actual0 = covariance_matrices.at(0); - const std::vector & actual1 = covariance_matrices.at(1); - const std::vector & actual2 = covariance_matrices.at(2); - const std::vector & actual3 = covariance_matrices.at(3); - const std::vector & actual4 = covariance_matrices.at(4); + const std::vector& actual0 = covariance_matrices.at(0); + const std::vector& actual1 = covariance_matrices.at(1); + const std::vector& actual2 = covariance_matrices.at(2); + const std::vector& actual3 = covariance_matrices.at(3); + const std::vector& actual4 = covariance_matrices.at(4); // Compare with the expected blocks // full covariance = { @@ -792,37 +835,39 @@ TEST_F(HashGraphTestFixture, GetCovariance) // [ XX (2x2), XY(2x3), XZ (2x1)] // [ YX (3x2), YY(3x3), YZ (3x1)] // [ ZX (1x2), ZY(1x3), ZZ (1x1)] - std::vector expected0 = {7.0747e-02, -8.4923e-03, -8.4923e-03, 8.1352e-02}; // XX - std::vector expected1 = - {1.6821e-02, 3.3643e-02, 5.0464e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02}; // XY - std::vector expected2 = - {1.6821e-02, 2.4758e-02, 3.3643e-02, 4.9517e-02, 5.0464e-02, 7.4275e-02}; // YX - std::vector expected3 = - {1.6821e-02, 3.3643e-02, 5.0464e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02}; // XY - std::vector expected4 = {-6.5325e-05, -1.3065e-04, -1.9598e-04}; // ZY + std::vector expected0 = { 7.0747e-02, -8.4923e-03, -8.4923e-03, 8.1352e-02 }; // XX + std::vector expected1 = { 1.6821e-02, 3.3643e-02, 5.0464e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02 }; // XY + std::vector expected2 = { 1.6821e-02, 2.4758e-02, 3.3643e-02, 4.9517e-02, 5.0464e-02, 7.4275e-02 }; // YX + std::vector expected3 = { 1.6821e-02, 3.3643e-02, 5.0464e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02 }; // XY + std::vector expected4 = { -6.5325e-05, -1.3065e-04, -1.9598e-04 }; // ZY ASSERT_EQ(expected0.size(), actual0.size()); - for (size_t i = 0; i < expected0.size(); ++i) { + for (size_t i = 0; i < expected0.size(); ++i) + { EXPECT_NEAR(expected0[i], actual0[i], 1.0e-5); } ASSERT_EQ(expected1.size(), actual1.size()); - for (size_t i = 0; i < expected1.size(); ++i) { + for (size_t i = 0; i < expected1.size(); ++i) + { EXPECT_NEAR(expected1[i], actual1[i], 1.0e-5); } ASSERT_EQ(expected2.size(), actual2.size()); - for (size_t i = 0; i < expected2.size(); ++i) { + for (size_t i = 0; i < expected2.size(); ++i) + { EXPECT_NEAR(expected2[i], actual2[i], 1.0e-5); } ASSERT_EQ(expected3.size(), actual3.size()); - for (size_t i = 0; i < expected3.size(); ++i) { + for (size_t i = 0; i < expected3.size(); ++i) + { EXPECT_NEAR(expected3[i], actual3[i], 1.0e-5); } ASSERT_EQ(expected4.size(), actual4.size()); - for (size_t i = 0; i < expected4.size(); ++i) { + for (size_t i = 0; i < expected4.size(); ++i) + { EXPECT_NEAR(expected4[i], actual4[i], 1.0e-5); } } @@ -855,10 +900,12 @@ TEST_F(HashGraphTestFixture, Copy) { fuse_graphs::HashGraph other(graph); // Verify the copy - for (const auto & constraint : graph.getConstraints()) { + for (const auto& constraint : graph.getConstraints()) + { EXPECT_TRUE(other.constraintExists(constraint.uuid())); } - for (const auto & variable : graph.getVariables()) { + for (const auto& variable : graph.getVariables()) + { EXPECT_TRUE(other.variableExists(variable.uuid())); } // Modify the variable values of the 'other' graph @@ -873,10 +920,12 @@ TEST_F(HashGraphTestFixture, Copy) fuse_graphs::HashGraph other; other = graph; // Verify the copy - for (const auto & constraint : graph.getConstraints()) { + for (const auto& constraint : graph.getConstraints()) + { EXPECT_TRUE(other.constraintExists(constraint.uuid())); } - for (const auto & variable : graph.getVariables()) { + for (const auto& variable : graph.getVariables()) + { EXPECT_TRUE(other.variableExists(variable.uuid())); } // Modify the variable values of the 'other' graph @@ -890,10 +939,12 @@ TEST_F(HashGraphTestFixture, Copy) { auto other = graph.clone(); // Verify the copy - for (const auto & constraint : graph.getConstraints()) { + for (const auto& constraint : graph.getConstraints()) + { EXPECT_TRUE(other->constraintExists(constraint.uuid())); } - for (const auto & variable : graph.getVariables()) { + for (const auto& variable : graph.getVariables()) + { EXPECT_TRUE(other->variableExists(variable.uuid())); } // Modify the variable values of the 'other' graph @@ -942,10 +993,12 @@ TEST_F(HashGraphTestFixture, Serialization) } // Verify the copy - for (const auto & constraint : expected.getConstraints()) { + for (const auto& constraint : expected.getConstraints()) + { EXPECT_TRUE(actual.constraintExists(constraint.uuid())); } - for (const auto & variable : expected.getVariables()) { + for (const auto& variable : expected.getVariables()) + { EXPECT_TRUE(actual.variableExists(variable.uuid())); } } @@ -982,9 +1035,7 @@ TEST_F(HashGraphTestFixture, GetConstraintCosts) constraint_uuids.push_back(constraint1->uuid()); auto costs = std::vector(); - graph.getConstraintCosts( - constraint_uuids.begin(), constraint_uuids.end(), - std::back_inserter(costs)); + graph.getConstraintCosts(constraint_uuids.begin(), constraint_uuids.end(), std::back_inserter(costs)); ASSERT_EQ(costs.size(), 2u); EXPECT_NEAR(costs[0].cost, 2.5, 1.0e-5); diff --git a/fuse_loss/CMakeLists.txt b/fuse_loss/CMakeLists.txt index 9871942c1..c2e857dcf 100644 --- a/fuse_loss/CMakeLists.txt +++ b/fuse_loss/CMakeLists.txt @@ -17,12 +17,13 @@ find_package(pluginlib REQUIRED) find_package(Ceres REQUIRED) -########### -## Build ## -########### +# ############################################################################## +# Build ## +# ############################################################################## -## fuse_loss library -add_library(${PROJECT_NAME} +# fuse_loss library +add_library( + ${PROJECT_NAME} src/arctan_loss.cpp src/cauchy_loss.cpp src/composed_loss.cpp @@ -36,51 +37,38 @@ add_library(${PROJECT_NAME} src/tolerant_loss.cpp src/trivial_loss.cpp src/tukey_loss.cpp - src/welsch_loss.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$" -) -target_link_libraries(${PROJECT_NAME} PUBLIC - Ceres::ceres - fuse_core::fuse_core - pluginlib::pluginlib -) + src/welsch_loss.cpp) +target_include_directories( + ${PROJECT_NAME} + PUBLIC "$" + "$") +target_link_libraries(${PROJECT_NAME} PUBLIC Ceres::ceres fuse_core::fuse_core + pluginlib::pluginlib) -############# -## Testing ## -############# +# ############################################################################## +# Testing ## +# ############################################################################## if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - add_subdirectory(test) endif() -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## -install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-export ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) pluginlib_export_plugin_description_file(fuse_core fuse_plugins.xml) ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET) -ament_export_dependencies( - ament_cmake_ros - fuse_core - pluginlib - Ceres -) +ament_export_dependencies(ament_cmake_ros fuse_core pluginlib Ceres) ament_package() diff --git a/fuse_loss/cmake/FindQWT.cmake b/fuse_loss/cmake/FindQWT.cmake index 95cca2e96..faf5f3b80 100644 --- a/fuse_loss/cmake/FindQWT.cmake +++ b/fuse_loss/cmake/FindQWT.cmake @@ -1,114 +1,106 @@ -# Copyright 2010-2013, Julien Schueller -# All rights reserved. +# Copyright 2010-2013, Julien Schueller All rights reserved. # # Software License Agreement (BSD 2-Clause Simplified License) # # Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: +# 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. +# * 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. # -# 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. - -# The views and conclusions contained in the software and documentation are those -# of the authors and should not be interpreted as representing official policies, -# either expressed or implied, of the FreeBSD Project. - -# Qt Widgets for Technical Applications -# available at http://www.http://qwt.sourceforge.net/ +# 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. + +# The views and conclusions contained in the software and documentation are +# those of the authors and should not be interpreted as representing official +# policies, either expressed or implied, of the FreeBSD Project. + +# Qt Widgets for Technical Applications available at +# http://www.http://qwt.sourceforge.net/ # -# The module defines the following variables: -# QWT_FOUND - the system has Qwt -# QWT_INCLUDE_DIR - where to find qwt_plot.h -# QWT_INCLUDE_DIRS - qwt includes -# QWT_LIBRARY - where to find the Qwt library -# QWT_LIBRARIES - aditional libraries -# QWT_MAJOR_VERSION - major version -# QWT_MINOR_VERSION - minor version -# QWT_PATCH_VERSION - patch version -# QWT_VERSION_STRING - version (ex. 5.2.1) -# QWT_ROOT_DIR - root dir (ex. /usr/local) - - -find_path( QWT_INCLUDE_DIR +# The module defines the following variables: QWT_FOUND - the system has Qwt +# QWT_INCLUDE_DIR - where to find qwt_plot.h QWT_INCLUDE_DIRS - qwt includes +# QWT_LIBRARY - where to find the Qwt library QWT_LIBRARIES - additional +# libraries QWT_MAJOR_VERSION - major version QWT_MINOR_VERSION - minor version +# QWT_PATCH_VERSION - patch version QWT_VERSION_STRING - version (ex. 5.2.1) +# QWT_ROOT_DIR - root dir (ex. /usr/local) + +find_path( + QWT_INCLUDE_DIR NAMES qwt_plot.h HINTS ${QT_INCLUDE_DIR} - PATH_SUFFIXES qwt qwt-qt3 qwt-qt4 qwt-qt5 -) + PATH_SUFFIXES qwt qwt-qt3 qwt-qt4 qwt-qt5) -set( QWT_INCLUDE_DIRS ${QWT_INCLUDE_DIR} ) +set(QWT_INCLUDE_DIRS ${QWT_INCLUDE_DIR}) # version -set( _VERSION_FILE ${QWT_INCLUDE_DIR}/qwt_global.h ) -if( EXISTS ${_VERSION_FILE} ) - file( STRINGS ${_VERSION_FILE} _VERSION_LINE REGEX "define[ ]+QWT_VERSION_STR" ) - if( _VERSION_LINE ) - string(REGEX REPLACE ".*define[ ]+QWT_VERSION_STR[ ]+\"(.*)\".*" "\\1" QWT_VERSION_STRING "${_VERSION_LINE}") - string(REGEX REPLACE "([0-9]+)\\.([0-9]+)\\.([0-9]+)" "\\1" QWT_MAJOR_VERSION "${QWT_VERSION_STRING}") - string(REGEX REPLACE "([0-9]+)\\.([0-9]+)\\.([0-9]+)" "\\2" QWT_MINOR_VERSION "${QWT_VERSION_STRING}") - string(REGEX REPLACE "([0-9]+)\\.([0-9]+)\\.([0-9]+)" "\\3" QWT_PATCH_VERSION "${QWT_VERSION_STRING}") +set(VERSION_FILE ${QWT_INCLUDE_DIR}/qwt_global.h) +if(EXISTS ${VERSION_FILE}) + file(STRINGS ${VERSION_FILE} _VERSION_LINE REGEX "define[ ]+QWT_VERSION_STR") + if(_VERSION_LINE) + string(REGEX REPLACE ".*define[ ]+QWT_VERSION_STR[ ]+\"(.*)\".*" "\\1" + QWT_VERSION_STRING "${_VERSION_LINE}") + string(REGEX REPLACE "([0-9]+)\\.([0-9]+)\\.([0-9]+)" "\\1" + QWT_MAJOR_VERSION "${QWT_VERSION_STRING}") + string(REGEX REPLACE "([0-9]+)\\.([0-9]+)\\.([0-9]+)" "\\2" + QWT_MINOR_VERSION "${QWT_VERSION_STRING}") + string(REGEX REPLACE "([0-9]+)\\.([0-9]+)\\.([0-9]+)" "\\3" + QWT_PATCH_VERSION "${QWT_VERSION_STRING}") endif() endif() - # check version -set( _QWT_VERSION_MATCH TRUE ) -if( Qwt_FIND_VERSION AND QWT_VERSION_STRING ) - if( Qwt_FIND_VERSION_EXACT ) - if( NOT Qwt_FIND_VERSION VERSION_EQUAL QWT_VERSION_STRING ) - set( _QWT_VERSION_MATCH FALSE ) +set(QWT_VERSION_MATCH TRUE) +if(Qwt_FIND_VERSION AND QWT_VERSION_STRING) + if(Qwt_FIND_VERSION_EXACT) + if(NOT Qwt_FIND_VERSION VERSION_EQUAL QWT_VERSION_STRING) + set(QWT_VERSION_MATCH FALSE) endif() else() - if( QWT_VERSION_STRING VERSION_LESS Qwt_FIND_VERSION ) - set( _QWT_VERSION_MATCH FALSE ) + if(QWT_VERSION_STRING VERSION_LESS Qwt_FIND_VERSION) + set(QWT_VERSION_MATCH FALSE) endif() endif() endif() - -find_library( QWT_LIBRARY +find_library( + QWT_LIBRARY NAMES qwt qwt-qt3 qwt-qt4 qwt-qt5 - HINTS ${QT_LIBRARY_DIR} -) - -set( QWT_LIBRARIES ${QWT_LIBRARY} ) + HINTS ${QT_LIBRARY_DIR}) +set(QWT_LIBRARIES ${QWT_LIBRARY}) # try to guess root dir from include dir -if( QWT_INCLUDE_DIR ) +if(QWT_INCLUDE_DIR) string(REGEX REPLACE "(.*)/include.*" "\\1" QWT_ROOT_DIR ${QWT_INCLUDE_DIR}) -# try to guess root dir from library dir -elseif( QWT_LIBRARY ) + # try to guess root dir from library dir +elseif(QWT_LIBRARY) string(REGEX REPLACE "(.*)/lib[/|32|64].*" "\\1" QWT_ROOT_DIR ${QWT_LIBRARY}) endif() - # handle the QUIETLY and REQUIRED arguments -include( FindPackageHandleStandardArgs ) -if( CMAKE_VERSION LESS 2.8.3 ) - find_package_handle_standard_args( QWT DEFAULT_MSG QWT_LIBRARY QWT_INCLUDE_DIR _QWT_VERSION_MATCH ) +include(FindPackageHandleStandardArgs) +if(CMAKE_VERSION LESS 2.8.3) + find_package_handle_standard_args(QWT DEFAULT_MSG QWT_LIBRARY QWT_INCLUDE_DIR + QWT_VERSION_MATCH) else() - find_package_handle_standard_args( QWT REQUIRED_VARS QWT_LIBRARY QWT_INCLUDE_DIR _QWT_VERSION_MATCH VERSION_VAR QWT_VERSION_STRING ) + find_package_handle_standard_args( + QWT + REQUIRED_VARS QWT_LIBRARY QWT_INCLUDE_DIR QWT_VERSION_MATCH + VERSION_VAR QWT_VERSION_STRING) endif() - mark_as_advanced( QWT_LIBRARY QWT_LIBRARIES @@ -118,5 +110,4 @@ mark_as_advanced( QWT_MINOR_VERSION QWT_PATCH_VERSION QWT_VERSION_STRING - QWT_ROOT_DIR -) + QWT_ROOT_DIR) diff --git a/fuse_loss/fuse_plugins.xml b/fuse_loss/fuse_plugins.xml index d9227cafd..349abb111 100644 --- a/fuse_loss/fuse_plugins.xml +++ b/fuse_loss/fuse_plugins.xml @@ -69,7 +69,7 @@ - Composition of two loss functions. The error is the result of first evaluating 'g' followed by 'f' to yeild the + Composition of two loss functions. The error is the result of first evaluating 'g' followed by 'f' to yield the composition 'f(g(s))' for the squared residual 's': rho(s) = f_rho(g_rho(s)) diff --git a/fuse_loss/include/fuse_loss/arctan_loss.hpp b/fuse_loss/include/fuse_loss/arctan_loss.hpp index 692d0e84a..f83cdff0e 100644 --- a/fuse_loss/include/fuse_loss/arctan_loss.hpp +++ b/fuse_loss/include/fuse_loss/arctan_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -84,19 +83,17 @@ class ArctanLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -109,7 +106,7 @@ class ArctanLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -132,7 +129,7 @@ class ArctanLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< ArctanLoss parameter 'a'. See Ceres documentation for more details + double a_{ 1.0 }; //!< ArctanLoss parameter 'a'. See Ceres documentation for more details // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -144,11 +141,11 @@ class ArctanLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/cauchy_loss.hpp b/fuse_loss/include/fuse_loss/cauchy_loss.hpp index d724c57c4..a4b8bb77e 100644 --- a/fuse_loss/include/fuse_loss/cauchy_loss.hpp +++ b/fuse_loss/include/fuse_loss/cauchy_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -84,19 +83,17 @@ class CauchyLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -109,7 +106,7 @@ class CauchyLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -132,7 +129,7 @@ class CauchyLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< CauchyLoss parameter 'a'. See Ceres documentation for more details + double a_{ 1.0 }; //!< CauchyLoss parameter 'a'. See Ceres documentation for more details // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -144,11 +141,11 @@ class CauchyLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/composed_loss.hpp b/fuse_loss/include/fuse_loss/composed_loss.hpp index 049fc674a..62ac6dd80 100644 --- a/fuse_loss/include/fuse_loss/composed_loss.hpp +++ b/fuse_loss/include/fuse_loss/composed_loss.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_loss { @@ -72,9 +71,8 @@ class ComposedLoss : public fuse_core::Loss * 'f(g(s))'. If it is nullptr the fuse_loss::TrivialLoss is used. Defaults to * nullptr. */ - explicit ComposedLoss( - const std::shared_ptr & f_loss = nullptr, - const std::shared_ptr & g_loss = nullptr); + explicit ComposedLoss(const std::shared_ptr& f_loss = nullptr, + const std::shared_ptr& g_loss = nullptr); /** * @brief Destructor @@ -92,19 +90,17 @@ class ComposedLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function. @@ -117,7 +113,7 @@ class ComposedLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'f_loss' accessor. @@ -144,7 +140,7 @@ class ComposedLoss : public fuse_core::Loss * * @param[in] loss Parameter 'f_loss'. */ - void fLoss(const std::shared_ptr & f_loss) + void fLoss(const std::shared_ptr& f_loss) { f_loss_ = f_loss; } @@ -154,16 +150,16 @@ class ComposedLoss : public fuse_core::Loss * * @param[in] loss Parameter 'g_loss'. */ - void gLoss(const std::shared_ptr & g_loss) + void gLoss(const std::shared_ptr& g_loss) { g_loss_ = g_loss; } private: - std::shared_ptr f_loss_{nullptr}; //!< The 'f' loss function, which is + std::shared_ptr f_loss_{ nullptr }; //!< The 'f' loss function, which is //!< evaluated last to yield the composition //!< 'f(g(s))' - std::shared_ptr g_loss_{nullptr}; //!< The 'g' loss function, which is + std::shared_ptr g_loss_{ nullptr }; //!< The 'g' loss function, which is //!< evaluated first to yield the //!< composition 'f(g(s))' @@ -177,12 +173,12 @@ class ComposedLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & f_loss_; - archive & g_loss_; + archive& boost::serialization::base_object(*this); + archive& f_loss_; + archive& g_loss_; } }; diff --git a/fuse_loss/include/fuse_loss/dcs_loss.hpp b/fuse_loss/include/fuse_loss/dcs_loss.hpp index 1c5fe39c0..870933340 100644 --- a/fuse_loss/include/fuse_loss/dcs_loss.hpp +++ b/fuse_loss/include/fuse_loss/dcs_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -87,19 +86,17 @@ class DCSLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -112,7 +109,7 @@ class DCSLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -135,7 +132,7 @@ class DCSLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< DCSLoss parameter 'a' + double a_{ 1.0 }; //!< DCSLoss parameter 'a' // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -147,11 +144,11 @@ class DCSLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/fair_loss.hpp b/fuse_loss/include/fuse_loss/fair_loss.hpp index 9c81ef02f..d4a022c13 100644 --- a/fuse_loss/include/fuse_loss/fair_loss.hpp +++ b/fuse_loss/include/fuse_loss/fair_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -87,19 +86,17 @@ class FairLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -112,7 +109,7 @@ class FairLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -135,7 +132,7 @@ class FairLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< FairLoss parameter 'a' + double a_{ 1.0 }; //!< FairLoss parameter 'a' // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -147,11 +144,11 @@ class FairLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/geman_mcclure_loss.hpp b/fuse_loss/include/fuse_loss/geman_mcclure_loss.hpp index fc7a24408..265aaf12c 100644 --- a/fuse_loss/include/fuse_loss/geman_mcclure_loss.hpp +++ b/fuse_loss/include/fuse_loss/geman_mcclure_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -87,19 +86,17 @@ class GemanMcClureLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -112,7 +109,7 @@ class GemanMcClureLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -135,7 +132,7 @@ class GemanMcClureLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< GemanMcClureLoss parameter 'a' + double a_{ 1.0 }; //!< GemanMcClureLoss parameter 'a' // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -147,11 +144,11 @@ class GemanMcClureLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/huber_loss.hpp b/fuse_loss/include/fuse_loss/huber_loss.hpp index 920a376f5..e38b479da 100644 --- a/fuse_loss/include/fuse_loss/huber_loss.hpp +++ b/fuse_loss/include/fuse_loss/huber_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -84,19 +83,17 @@ class HuberLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -109,7 +106,7 @@ class HuberLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -132,7 +129,7 @@ class HuberLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< HuberLoss parameter 'a'. See Ceres documentation for more details + double a_{ 1.0 }; //!< HuberLoss parameter 'a'. See Ceres documentation for more details // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -144,11 +141,11 @@ class HuberLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/loss_function.hpp b/fuse_loss/include/fuse_loss/loss_function.hpp index 7d5aa2e1d..68ba629eb 100644 --- a/fuse_loss/include/fuse_loss/loss_function.hpp +++ b/fuse_loss/include/fuse_loss/loss_function.hpp @@ -145,19 +145,18 @@ namespace ceres class DCSLoss : public ceres::LossFunction { public: - explicit DCSLoss(const double a) - : a_(a) + explicit DCSLoss(const double a) : a_(a) { } - void Evaluate(double, double * rho) const override; + void Evaluate(double, double* rho) const override; private: const double a_; }; -// Fair, similar to tthe L1 - L2 estimators, that try to take the advantage of the L1 estimators to -// reduce the influence of large erros and that of L2 estimators to be convex. It behave like L2 for +// Fair, similar to the L1 - L2 estimators, that try to take the advantage of the L1 estimators to +// reduce the influence of large errors and that of L2 estimators to be convex. It behave like L2 for // small squared residuals 's' and like L1 for large ones. // // The term is computed as: @@ -170,12 +169,11 @@ class DCSLoss : public ceres::LossFunction class FairLoss : public ceres::LossFunction { public: - explicit FairLoss(const double a) - : a_(a), b_(a * a) + explicit FairLoss(const double a) : a_(a), b_(a * a) { } - void Evaluate(double, double *) const override; + void Evaluate(double, double*) const override; private: const double a_; @@ -195,7 +193,7 @@ class FairLoss : public ceres::LossFunction // http://www2.informatik.uni-freiburg.de/~agarwal/resources/agarwal-thesis.pdf (pp. 89-90) // // where the original Geman-McClure is presented in Eq. 5.21 and the generalized Geman-McClure is -// defined introducing the parametr 'a'. It also shows how it is adapted to be a positive definite +// defined introducing the parameter 'a'. It also shows how it is adapted to be a positive definite // function. // // Remember that in Ceres the implementation of rho(s) must be multiplied by 2 because the cost is @@ -222,12 +220,11 @@ class FairLoss : public ceres::LossFunction class GemanMcClureLoss : public ceres::LossFunction { public: - explicit GemanMcClureLoss(const double a) - : b_(a * a) + explicit GemanMcClureLoss(const double a) : b_(a * a) { } - void Evaluate(double, double *) const override; + void Evaluate(double, double*) const override; private: const double b_; @@ -246,12 +243,11 @@ class GemanMcClureLoss : public ceres::LossFunction class WelschLoss : public ceres::LossFunction { public: - explicit WelschLoss(const double a) - : b_(a * a), c_(-1.0 / b_) + explicit WelschLoss(const double a) : b_(a * a), c_(-1.0 / b_) { } - void Evaluate(double, double *) const override; + void Evaluate(double, double*) const override; private: const double b_; diff --git a/fuse_loss/include/fuse_loss/qwt_loss_plot.hpp b/fuse_loss/include/fuse_loss/qwt_loss_plot.hpp index 8d48986e9..267f28c52 100644 --- a/fuse_loss/include/fuse_loss/qwt_loss_plot.hpp +++ b/fuse_loss/include/fuse_loss/qwt_loss_plot.hpp @@ -62,7 +62,8 @@ class HSVColormap public: explicit HSVColormap(const size_t size = 256) { - if (size == 0) { + if (size == 0) + { return; } @@ -70,7 +71,8 @@ class HSVColormap double hue = 0.0; const double hue_increment = 1.0 / size; - for (size_t i = 0; i < size; ++i, hue += hue_increment) { + for (size_t i = 0; i < size; ++i, hue += hue_increment) + { QColor color; color.setHsvF(hue, 1.0, 1.0); @@ -78,7 +80,7 @@ class HSVColormap } } - const QColor & operator[](const size_t i) const + const QColor& operator[](const size_t i) const { return colormap_[i]; } @@ -95,12 +97,11 @@ class HSVColormap class LossEvaluator { public: - explicit LossEvaluator(const std::vector & residuals) - : residuals_(residuals) + explicit LossEvaluator(const std::vector& residuals) : residuals_(residuals) { } - std::vector rho(const ceres::LossFunction * loss_function) const + std::vector rho(const ceres::LossFunction* loss_function) const { std::vector rhos; rhos.reserve(residuals_.size()); @@ -113,18 +114,17 @@ class LossEvaluator // // See: https://github.com/ceres-solver/ceres- // solver/blob/master/internal/ceres/residual_block.cc#L165 - std::transform( - residuals_.begin(), residuals_.end(), std::back_inserter(rhos), - [&loss_function](const auto & r) { // NOLINT(whitespace/braces) - double rho[3]; - loss_function->Evaluate(r * r, rho); - return 0.5 * rho[0]; - }); // NOLINT(whitespace/braces) + std::transform(residuals_.begin(), residuals_.end(), std::back_inserter(rhos), + [&loss_function](const auto& r) { // NOLINT(whitespace/braces) + double rho[3]; + loss_function->Evaluate(r * r, rho); + return 0.5 * rho[0]; + }); // NOLINT(whitespace/braces) return rhos; } - std::vector influence(const ceres::LossFunction * loss_function) const + std::vector influence(const ceres::LossFunction* loss_function) const { std::vector influence; influence.reserve(residuals_.size()); @@ -154,18 +154,17 @@ class LossEvaluator // // where \rho(r) = 0.5 * rho(r^2) is the inverse of rho(s) = 2 * \rho(sqrt(s)), // because s = r^2 and r = sqrt(s). - std::transform( - residuals_.begin(), residuals_.end(), std::back_inserter(influence), - [&loss_function](const auto & r) { // NOLINT(whitespace/braces) - double rho[3]; - loss_function->Evaluate(r * r, rho); - return r * rho[1]; - }); // NOLINT(whitespace/braces) + std::transform(residuals_.begin(), residuals_.end(), std::back_inserter(influence), + [&loss_function](const auto& r) { // NOLINT(whitespace/braces) + double rho[3]; + loss_function->Evaluate(r * r, rho); + return r * rho[1]; + }); // NOLINT(whitespace/braces) return influence; } - std::vector weight(const ceres::LossFunction * loss_function) const + std::vector weight(const ceres::LossFunction* loss_function) const { std::vector weight; weight.reserve(residuals_.size()); @@ -181,18 +180,17 @@ class LossEvaluator // ds ds // // That is, rho[1]. - std::transform( - residuals_.begin(), residuals_.end(), std::back_inserter(weight), - [&loss_function](const auto & r) { // NOLINT(whitespace/braces) - double rho[3]; - loss_function->Evaluate(r * r, rho); - return rho[1]; - }); // NOLINT(whitespace/braces) + std::transform(residuals_.begin(), residuals_.end(), std::back_inserter(weight), + [&loss_function](const auto& r) { // NOLINT(whitespace/braces) + double rho[3]; + loss_function->Evaluate(r * r, rho); + return rho[1]; + }); // NOLINT(whitespace/braces) return weight; } - std::vector secondDerivative(const ceres::LossFunction * loss_function) const + std::vector secondDerivative(const ceres::LossFunction* loss_function) const { std::vector second_derivative; second_derivative.reserve(residuals_.size()); @@ -204,18 +202,17 @@ class LossEvaluator // ds^2 // // That is, rho[2]. - std::transform( - residuals_.begin(), residuals_.end(), std::back_inserter(second_derivative), - [&loss_function](const auto & r) { // NOLINT(whitespace/braces) - double rho[3]; - loss_function->Evaluate(r * r, rho); - return rho[2]; - }); // NOLINT(whitespace/braces) + std::transform(residuals_.begin(), residuals_.end(), std::back_inserter(second_derivative), + [&loss_function](const auto& r) { // NOLINT(whitespace/braces) + double rho[3]; + loss_function->Evaluate(r * r, rho); + return rho[2]; + }); // NOLINT(whitespace/braces) return second_derivative; } - const std::vector & getResiduals() const + const std::vector& getResiduals() const { return residuals_; } @@ -227,8 +224,8 @@ class LossEvaluator class QwtLossPlot { public: - QwtLossPlot(const std::vector & residuals, const HSVColormap & colormap) - : residuals_(QVector(residuals.begin(), residuals.end())) + QwtLossPlot(const std::vector& residuals, const HSVColormap& colormap) + : residuals_(QVector(residuals.begin(), residuals.end())) , loss_evaluator_(residuals) , colormap_(colormap) , magnifier_(plot_.canvas()) @@ -252,14 +249,14 @@ class QwtLossPlot plot_.insertLegend(&legend_); } - static std::string getName(const std::string & type) + static std::string getName(const std::string& type) { return type.substr(11, type.size() - 15); } - QwtPlotCurve * createCurve(const std::string & name, const std::vector & values) + QwtPlotCurve* createCurve(const std::string& name, const std::vector& values) { - QwtPlotCurve * curve = new QwtPlotCurve(name.c_str()); + QwtPlotCurve* curve = new QwtPlotCurve(name.c_str()); curve->setSamples(residuals_, QVector(values.begin(), values.end())); @@ -269,45 +266,33 @@ class QwtLossPlot return curve; } - void plotRho(const std::shared_ptr & loss) + void plotRho(const std::shared_ptr& loss) { - curves_.push_back( - createCurve( - getName(loss->type()), - loss_evaluator_.rho(loss->lossFunction()))); + curves_.push_back(createCurve(getName(loss->type()), loss_evaluator_.rho(loss->lossFunction()))); } - void plotInfluence(const std::shared_ptr & loss) + void plotInfluence(const std::shared_ptr& loss) { - curves_.push_back( - createCurve( - getName(loss->type()), - loss_evaluator_.influence(loss->lossFunction()))); + curves_.push_back(createCurve(getName(loss->type()), loss_evaluator_.influence(loss->lossFunction()))); } - void plotWeight(const std::shared_ptr & loss) + void plotWeight(const std::shared_ptr& loss) { - curves_.push_back( - createCurve( - getName(loss->type()), - loss_evaluator_.weight(loss->lossFunction()))); + curves_.push_back(createCurve(getName(loss->type()), loss_evaluator_.weight(loss->lossFunction()))); } - void plotSecondDerivative(const std::shared_ptr & loss) + void plotSecondDerivative(const std::shared_ptr& loss) { - curves_.push_back( - createCurve( - getName(loss->type()), - loss_evaluator_.secondDerivative(loss->lossFunction()))); + curves_.push_back(createCurve(getName(loss->type()), loss_evaluator_.secondDerivative(loss->lossFunction()))); } - void save(const std::string & filename) + void save(const std::string& filename) { QwtPlotRenderer renderer; renderer.renderDocument(&plot_, filename.c_str(), QSizeF(300, 200)); } - QwtPlot & plot() + QwtPlot& plot() { return plot_; } @@ -319,7 +304,7 @@ class QwtLossPlot HSVColormap colormap_; // We don't use an std::shared_ptr because QwtPlot takes ownership of the curves // attached to it and deletes them on destruction - std::vector curves_; + std::vector curves_; QwtPlot plot_; QwtPlotGrid grid_; diff --git a/fuse_loss/include/fuse_loss/scaled_loss.hpp b/fuse_loss/include/fuse_loss/scaled_loss.hpp index 6253f5fe4..a8ed6ccab 100644 --- a/fuse_loss/include/fuse_loss/scaled_loss.hpp +++ b/fuse_loss/include/fuse_loss/scaled_loss.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_loss { @@ -68,9 +67,7 @@ class ScaledLoss : public fuse_core::Loss * @param[in] a ScaledLoss parameter 'a'. See Ceres documentation for more details. * @param[in] loss The loss function to scale. Its output is scaled/multiplied by 'a'. */ - explicit ScaledLoss( - const double a = 1.0, - const std::shared_ptr & loss = nullptr); + explicit ScaledLoss(const double a = 1.0, const std::shared_ptr& loss = nullptr); /** * @brief Destructor @@ -88,19 +85,17 @@ class ScaledLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function. @@ -113,7 +108,7 @@ class ScaledLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -150,14 +145,14 @@ class ScaledLoss : public fuse_core::Loss * * @param[in] loss Parameter 'loss'. */ - void loss(const std::shared_ptr & loss) + void loss(const std::shared_ptr& loss) { loss_ = loss; } private: - double a_{1.0}; //!< ScaledLoss parameter 'a'. See Ceres documentation for more details - std::shared_ptr loss_{nullptr}; //!< The loss function to scale + double a_{ 1.0 }; //!< ScaledLoss parameter 'a'. See Ceres documentation for more details + std::shared_ptr loss_{ nullptr }; //!< The loss function to scale // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -169,12 +164,12 @@ class ScaledLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; - archive & loss_; + archive& boost::serialization::base_object(*this); + archive& a_; + archive& loss_; } }; diff --git a/fuse_loss/include/fuse_loss/softlone_loss.hpp b/fuse_loss/include/fuse_loss/softlone_loss.hpp index 840514f9b..740c7034b 100644 --- a/fuse_loss/include/fuse_loss/softlone_loss.hpp +++ b/fuse_loss/include/fuse_loss/softlone_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -84,19 +83,17 @@ class SoftLOneLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -109,7 +106,7 @@ class SoftLOneLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -132,7 +129,7 @@ class SoftLOneLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< SoftLOneLoss parameter 'a'. See Ceres documentation for more details + double a_{ 1.0 }; //!< SoftLOneLoss parameter 'a'. See Ceres documentation for more details // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -144,11 +141,11 @@ class SoftLOneLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/tolerant_loss.hpp b/fuse_loss/include/fuse_loss/tolerant_loss.hpp index 75467c893..26cd8c9cb 100644 --- a/fuse_loss/include/fuse_loss/tolerant_loss.hpp +++ b/fuse_loss/include/fuse_loss/tolerant_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -85,19 +84,17 @@ class TolerantLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -110,7 +107,7 @@ class TolerantLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -153,8 +150,8 @@ class TolerantLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< TolerantLoss parameter 'a'. See Ceres documentation for more details - double b_{0.1}; //!< TolerantLoss parameter 'b'. See Ceres documentation for more details + double a_{ 1.0 }; //!< TolerantLoss parameter 'a'. See Ceres documentation for more details + double b_{ 0.1 }; //!< TolerantLoss parameter 'b'. See Ceres documentation for more details // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -166,12 +163,12 @@ class TolerantLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; - archive & b_; + archive& boost::serialization::base_object(*this); + archive& a_; + archive& b_; } }; diff --git a/fuse_loss/include/fuse_loss/trivial_loss.hpp b/fuse_loss/include/fuse_loss/trivial_loss.hpp index 9cce011d3..130199cce 100644 --- a/fuse_loss/include/fuse_loss/trivial_loss.hpp +++ b/fuse_loss/include/fuse_loss/trivial_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -82,12 +81,9 @@ class TrivialLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - >/*interfaces*/, - const std::string & /*name*/) override + fuse_core::node_interfaces::NodeInterfaces /*interfaces*/, + const std::string& /*name*/) override { } @@ -96,7 +92,7 @@ class TrivialLoss : public fuse_core::Loss * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -109,7 +105,7 @@ class TrivialLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; private: // Allow Boost Serialization access to private methods @@ -122,10 +118,10 @@ class TrivialLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_loss/include/fuse_loss/tukey_loss.hpp b/fuse_loss/include/fuse_loss/tukey_loss.hpp index 7ef4b9c4f..8d43d516d 100644 --- a/fuse_loss/include/fuse_loss/tukey_loss.hpp +++ b/fuse_loss/include/fuse_loss/tukey_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -84,19 +83,17 @@ class TukeyLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -109,7 +106,7 @@ class TukeyLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -132,7 +129,7 @@ class TukeyLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< TukeyLoss parameter 'a'. See Ceres documentation for more details + double a_{ 1.0 }; //!< TukeyLoss parameter 'a'. See Ceres documentation for more details // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -144,11 +141,11 @@ class TukeyLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/include/fuse_loss/welsch_loss.hpp b/fuse_loss/include/fuse_loss/welsch_loss.hpp index 80f384c2d..092b8b681 100644 --- a/fuse_loss/include/fuse_loss/welsch_loss.hpp +++ b/fuse_loss/include/fuse_loss/welsch_loss.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_loss { @@ -87,19 +86,17 @@ class WelschLoss : public fuse_core::Loss * server. */ void initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) override; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) override; /** * @brief Print a human-readable description of the loss function to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function @@ -112,7 +109,7 @@ class WelschLoss : public fuse_core::Loss * * @return A base pointer to an instance of a derived ceres::LossFunction. */ - ceres::LossFunction * lossFunction() const override; + ceres::LossFunction* lossFunction() const override; /** * @brief Parameter 'a' accessor. @@ -135,7 +132,7 @@ class WelschLoss : public fuse_core::Loss } private: - double a_{1.0}; //!< WelschLoss parameter 'a' + double a_{ 1.0 }; //!< WelschLoss parameter 'a' // Allow Boost Serialization access to private methods friend class boost::serialization::access; @@ -147,11 +144,11 @@ class WelschLoss : public fuse_core::Loss * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & a_; + archive& boost::serialization::base_object(*this); + archive& a_; } }; diff --git a/fuse_loss/package.xml b/fuse_loss/package.xml index 27cc710ce..627f3f06e 100644 --- a/fuse_loss/package.xml +++ b/fuse_loss/package.xml @@ -27,8 +27,6 @@ qtbase5-dev libqwt-qt5-dev ament_cmake_gtest - ament_lint_auto - ament_lint_common ament_cmake diff --git a/fuse_loss/src/arctan_loss.cpp b/fuse_loss/src/arctan_loss.cpp index ad1df8744..8736ea0c7 100644 --- a/fuse_loss/src/arctan_loss.cpp +++ b/fuse_loss/src/arctan_loss.cpp @@ -42,29 +42,26 @@ namespace fuse_loss { -ArctanLoss::ArctanLoss(const double a) -: a_(a) +ArctanLoss::ArctanLoss(const double a) : a_(a) { } void ArctanLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void ArctanLoss::print(std::ostream & stream) const +void ArctanLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * ArctanLoss::lossFunction() const +ceres::LossFunction* ArctanLoss::lossFunction() const { return new ceres::ArctanLoss(a_); } diff --git a/fuse_loss/src/cauchy_loss.cpp b/fuse_loss/src/cauchy_loss.cpp index a93066d06..372dd978b 100644 --- a/fuse_loss/src/cauchy_loss.cpp +++ b/fuse_loss/src/cauchy_loss.cpp @@ -42,29 +42,26 @@ namespace fuse_loss { -CauchyLoss::CauchyLoss(const double a) -: a_(a) +CauchyLoss::CauchyLoss(const double a) : a_(a) { } void CauchyLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void CauchyLoss::print(std::ostream & stream) const +void CauchyLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * CauchyLoss::lossFunction() const +ceres::LossFunction* CauchyLoss::lossFunction() const { return new ceres::CauchyLoss(a_); } diff --git a/fuse_loss/src/composed_loss.cpp b/fuse_loss/src/composed_loss.cpp index dc684adb3..bafb40940 100644 --- a/fuse_loss/src/composed_loss.cpp +++ b/fuse_loss/src/composed_loss.cpp @@ -44,43 +44,41 @@ namespace fuse_loss { -ComposedLoss::ComposedLoss( - const std::shared_ptr & f_loss, - const std::shared_ptr & g_loss) -: f_loss_(f_loss), g_loss_(g_loss) +ComposedLoss::ComposedLoss(const std::shared_ptr& f_loss, + const std::shared_ptr& g_loss) + : f_loss_(f_loss), g_loss_(g_loss) { } void ComposedLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { f_loss_ = fuse_core::loadLossConfig(interfaces, name + ".f_loss"); g_loss_ = fuse_core::loadLossConfig(interfaces, name + ".g_loss"); } -void ComposedLoss::print(std::ostream & stream) const +void ComposedLoss::print(std::ostream& stream) const { stream << type() << "\n"; - if (f_loss_) { + if (f_loss_) + { stream << " f_loss: " << f_loss_ << "\n"; } - if (g_loss_) { + if (g_loss_) + { stream << " g_loss: " << g_loss_ << "\n"; } } -ceres::LossFunction * ComposedLoss::lossFunction() const +ceres::LossFunction* ComposedLoss::lossFunction() const { - return new ceres::ComposedLoss( - f_loss_ ? f_loss_->lossFunction() : TrivialLoss().lossFunction(), Ownership, - g_loss_ ? g_loss_->lossFunction() : TrivialLoss().lossFunction(), Ownership); + return new ceres::ComposedLoss(f_loss_ ? f_loss_->lossFunction() : TrivialLoss().lossFunction(), Ownership, + g_loss_ ? g_loss_->lossFunction() : TrivialLoss().lossFunction(), Ownership); } } // namespace fuse_loss diff --git a/fuse_loss/src/dcs_loss.cpp b/fuse_loss/src/dcs_loss.cpp index 832246091..571cbfd5f 100644 --- a/fuse_loss/src/dcs_loss.cpp +++ b/fuse_loss/src/dcs_loss.cpp @@ -43,29 +43,26 @@ namespace fuse_loss { -DCSLoss::DCSLoss(const double a) -: a_(a) +DCSLoss::DCSLoss(const double a) : a_(a) { } void DCSLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void DCSLoss::print(std::ostream & stream) const +void DCSLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * DCSLoss::lossFunction() const +ceres::LossFunction* DCSLoss::lossFunction() const { return new ceres::DCSLoss(a_); } diff --git a/fuse_loss/src/fair_loss.cpp b/fuse_loss/src/fair_loss.cpp index 19d3cc927..cc40cab7f 100644 --- a/fuse_loss/src/fair_loss.cpp +++ b/fuse_loss/src/fair_loss.cpp @@ -43,29 +43,26 @@ namespace fuse_loss { -FairLoss::FairLoss(const double a) -: a_(a) +FairLoss::FairLoss(const double a) : a_(a) { } void FairLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void FairLoss::print(std::ostream & stream) const +void FairLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * FairLoss::lossFunction() const +ceres::LossFunction* FairLoss::lossFunction() const { return new ceres::FairLoss(a_); } diff --git a/fuse_loss/src/geman_mcclure_loss.cpp b/fuse_loss/src/geman_mcclure_loss.cpp index fbe43c052..ce4ebd814 100644 --- a/fuse_loss/src/geman_mcclure_loss.cpp +++ b/fuse_loss/src/geman_mcclure_loss.cpp @@ -43,29 +43,26 @@ namespace fuse_loss { -GemanMcClureLoss::GemanMcClureLoss(const double a) -: a_(a) +GemanMcClureLoss::GemanMcClureLoss(const double a) : a_(a) { } void GemanMcClureLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void GemanMcClureLoss::print(std::ostream & stream) const +void GemanMcClureLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * GemanMcClureLoss::lossFunction() const +ceres::LossFunction* GemanMcClureLoss::lossFunction() const { return new ceres::GemanMcClureLoss(a_); } diff --git a/fuse_loss/src/huber_loss.cpp b/fuse_loss/src/huber_loss.cpp index e22cf01c8..a9b0b2d73 100644 --- a/fuse_loss/src/huber_loss.cpp +++ b/fuse_loss/src/huber_loss.cpp @@ -42,29 +42,26 @@ namespace fuse_loss { -HuberLoss::HuberLoss(const double a) -: a_(a) +HuberLoss::HuberLoss(const double a) : a_(a) { } void HuberLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void HuberLoss::print(std::ostream & stream) const +void HuberLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * HuberLoss::lossFunction() const +ceres::LossFunction* HuberLoss::lossFunction() const { return new ceres::HuberLoss(a_); } diff --git a/fuse_loss/src/loss_function.cpp b/fuse_loss/src/loss_function.cpp index 8a64004f7..3e5c9c892 100644 --- a/fuse_loss/src/loss_function.cpp +++ b/fuse_loss/src/loss_function.cpp @@ -41,7 +41,8 @@ namespace ceres void DCSLoss::Evaluate(double s, double rho[3]) const { - if (s > a_) { + if (s > a_) + { // Outlier region const double inv = 1.0 / (a_ + s); const double scale = 2.0 * a_ * inv; @@ -49,7 +50,9 @@ void DCSLoss::Evaluate(double s, double rho[3]) const rho[0] = a_ * (3.0 * s - a_) * inv; rho[1] = scale * scale; rho[2] = -2.0 * inv * rho[1]; - } else { + } + else + { // Inlier region rho[0] = s; rho[1] = 1.0; diff --git a/fuse_loss/src/scaled_loss.cpp b/fuse_loss/src/scaled_loss.cpp index abf9bb1a5..840a9c7ee 100644 --- a/fuse_loss/src/scaled_loss.cpp +++ b/fuse_loss/src/scaled_loss.cpp @@ -43,34 +43,32 @@ namespace fuse_loss { -ScaledLoss::ScaledLoss(const double a, const std::shared_ptr & loss) -: a_(a), loss_(loss) +ScaledLoss::ScaledLoss(const double a, const std::shared_ptr& loss) : a_(a), loss_(loss) { } void ScaledLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); loss_ = fuse_core::loadLossConfig(interfaces, name + ".loss"); } -void ScaledLoss::print(std::ostream & stream) const +void ScaledLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; - if (loss_) { + if (loss_) + { stream << " loss: " << loss_ << "\n"; } } -ceres::LossFunction * ScaledLoss::lossFunction() const +ceres::LossFunction* ScaledLoss::lossFunction() const { return new ceres::ScaledLoss(loss_ ? loss_->lossFunction() : nullptr, a_, Ownership); } diff --git a/fuse_loss/src/softlone_loss.cpp b/fuse_loss/src/softlone_loss.cpp index 711da1df6..3b9c87d1f 100644 --- a/fuse_loss/src/softlone_loss.cpp +++ b/fuse_loss/src/softlone_loss.cpp @@ -42,29 +42,26 @@ namespace fuse_loss { -SoftLOneLoss::SoftLOneLoss(const double a) -: a_(a) +SoftLOneLoss::SoftLOneLoss(const double a) : a_(a) { } void SoftLOneLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void SoftLOneLoss::print(std::ostream & stream) const +void SoftLOneLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * SoftLOneLoss::lossFunction() const +ceres::LossFunction* SoftLOneLoss::lossFunction() const { return new ceres::SoftLOneLoss(a_); } diff --git a/fuse_loss/src/tolerant_loss.cpp b/fuse_loss/src/tolerant_loss.cpp index 06699a7c0..c46eb4cbb 100644 --- a/fuse_loss/src/tolerant_loss.cpp +++ b/fuse_loss/src/tolerant_loss.cpp @@ -42,31 +42,28 @@ namespace fuse_loss { -TolerantLoss::TolerantLoss(const double a, const double b) -: a_(a), b_(b) +TolerantLoss::TolerantLoss(const double a, const double b) : a_(a), b_(b) { } void TolerantLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); b_ = fuse_core::getParam(interfaces, name + ".b", b_); } -void TolerantLoss::print(std::ostream & stream) const +void TolerantLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n" << " b: " << b_ << "\n"; } -ceres::LossFunction * TolerantLoss::lossFunction() const +ceres::LossFunction* TolerantLoss::lossFunction() const { return new ceres::TolerantLoss(a_, b_); } diff --git a/fuse_loss/src/trivial_loss.cpp b/fuse_loss/src/trivial_loss.cpp index 283a3acda..df344edc5 100644 --- a/fuse_loss/src/trivial_loss.cpp +++ b/fuse_loss/src/trivial_loss.cpp @@ -41,12 +41,12 @@ namespace fuse_loss { -void TrivialLoss::print(std::ostream & stream) const +void TrivialLoss::print(std::ostream& stream) const { stream << type() << "\n"; } -ceres::LossFunction * TrivialLoss::lossFunction() const +ceres::LossFunction* TrivialLoss::lossFunction() const { return new ceres::TrivialLoss(); } diff --git a/fuse_loss/src/tukey_loss.cpp b/fuse_loss/src/tukey_loss.cpp index 24ad5c657..1570e89f8 100644 --- a/fuse_loss/src/tukey_loss.cpp +++ b/fuse_loss/src/tukey_loss.cpp @@ -43,29 +43,26 @@ namespace fuse_loss { -TukeyLoss::TukeyLoss(const double a) -: a_(a) +TukeyLoss::TukeyLoss(const double a) : a_(a) { } void TukeyLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void TukeyLoss::print(std::ostream & stream) const +void TukeyLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * TukeyLoss::lossFunction() const +ceres::LossFunction* TukeyLoss::lossFunction() const { #if CERES_VERSION_AT_LEAST(2, 0, 0) return new ceres::TukeyLoss(a_); diff --git a/fuse_loss/src/welsch_loss.cpp b/fuse_loss/src/welsch_loss.cpp index fd409eb91..bc913fd86 100644 --- a/fuse_loss/src/welsch_loss.cpp +++ b/fuse_loss/src/welsch_loss.cpp @@ -43,29 +43,26 @@ namespace fuse_loss { -WelschLoss::WelschLoss(const double a) -: a_(a) +WelschLoss::WelschLoss(const double a) : a_(a) { } void WelschLoss::initialize( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& name) { a_ = fuse_core::getParam(interfaces, name + ".a", a_); } -void WelschLoss::print(std::ostream & stream) const +void WelschLoss::print(std::ostream& stream) const { stream << type() << "\n" << " a: " << a_ << "\n"; } -ceres::LossFunction * WelschLoss::lossFunction() const +ceres::LossFunction* WelschLoss::lossFunction() const { return new ceres::WelschLoss(a_); } diff --git a/fuse_loss/test/CMakeLists.txt b/fuse_loss/test/CMakeLists.txt index 0417d6b62..ba0451d71 100644 --- a/fuse_loss/test/CMakeLists.txt +++ b/fuse_loss/test/CMakeLists.txt @@ -1,4 +1,5 @@ -# CORE GTESTS ====================================================================================== +# CORE GTESTS +# ====================================================================================== ament_add_gtest(test_loss_function test_loss_function.cpp) target_link_libraries(test_loss_function ${PROJECT_NAME}) @@ -15,7 +16,10 @@ ament_add_gtest(test_scaled_loss test_scaled_loss.cpp) target_link_libraries(test_scaled_loss ${PROJECT_NAME}) # Qwt Loss Plot Tests -option(BUILD_WITH_PLOT_TESTS "Build with plot tests. These test might fail to run in headless systems." OFF) +option( + BUILD_WITH_PLOT_TESTS + "Build with plot tests. These test might fail to run in headless systems." + OFF) if(BUILD_WITH_PLOT_TESTS) find_package(Qt5 REQUIRED COMPONENTS Core Widgets) @@ -26,17 +30,11 @@ if(BUILD_WITH_PLOT_TESTS) ament_add_gtest(test_qwt_loss_plot test_qwt_loss_plot.cpp) target_link_libraries(test_qwt_loss_plot ${PROJECT_NAME}) - target_include_directories(test_qwt_loss_plot - PRIVATE - ${Qt5Core_INCLUDE_DIRS} - ${Qt5Widgets_INCLUDE_DIRS} - ${QWT_INCLUDE_DIRS} - ) - target_link_libraries(test_qwt_loss_plot - ${Qt5Core_LIBRARIES} - ${Qt5Widgets_LIBRARIES} - ${QWT_LIBRARIES} - ) + target_include_directories( + test_qwt_loss_plot PRIVATE ${Qt5Core_INCLUDE_DIRS} + ${Qt5Widgets_INCLUDE_DIRS} ${QWT_INCLUDE_DIRS}) + target_link_libraries(test_qwt_loss_plot ${Qt5Core_LIBRARIES} + ${Qt5Widgets_LIBRARIES} ${QWT_LIBRARIES}) option(BUILD_WITH_INTERACTIVE_TESTS "Build with interactive tests" OFF) if(BUILD_WITH_INTERACTIVE_TESTS) diff --git a/fuse_loss/test/test_composed_loss.cpp b/fuse_loss/test/test_composed_loss.cpp index 9ef9681a7..de607895d 100644 --- a/fuse_loss/test/test_composed_loss.cpp +++ b/fuse_loss/test/test_composed_loss.cpp @@ -59,7 +59,7 @@ TEST(ComposedLoss, Constructor) ASSERT_NE(nullptr, composed_loss_function); const double s = 1.5; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; composed_loss_function->Evaluate(s, rho); EXPECT_EQ(s, rho[0]); @@ -69,7 +69,7 @@ TEST(ComposedLoss, Constructor) // Create a loss with f_loss parameter only { - std::shared_ptr f_loss{new fuse_loss::HuberLoss}; + std::shared_ptr f_loss{ new fuse_loss::HuberLoss }; fuse_loss::ComposedLoss composed_loss(f_loss); EXPECT_NE(nullptr, composed_loss.fLoss().get()); @@ -85,10 +85,10 @@ TEST(ComposedLoss, Constructor) ASSERT_NE(nullptr, f_loss_function); const double s = 1.5; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; composed_loss_function->Evaluate(s, rho); - double f_rho[3] = {0.0}; + double f_rho[3] = { 0.0 }; f_loss_function->Evaluate(s, f_rho); // Make sure 'f(s) != s', i.e. it is not an inlier, which would be a trivial case @@ -96,14 +96,15 @@ TEST(ComposedLoss, Constructor) // Check that 'f(g(s)) == f(s)' and the same for the first and second derivatives, since g is // the TrivialLoss - for (size_t i = 0; i < 3; ++i) { + for (size_t i = 0; i < 3; ++i) + { EXPECT_EQ(f_rho[i], rho[i]); } } // Create a loss with g_loss parameter only { - std::shared_ptr g_loss{new fuse_loss::HuberLoss}; + std::shared_ptr g_loss{ new fuse_loss::HuberLoss }; fuse_loss::ComposedLoss composed_loss(nullptr, g_loss); EXPECT_EQ(nullptr, composed_loss.fLoss()); @@ -119,10 +120,10 @@ TEST(ComposedLoss, Constructor) ASSERT_NE(nullptr, g_loss_function); const double s = 1.5; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; composed_loss_function->Evaluate(s, rho); - double g_rho[3] = {0.0}; + double g_rho[3] = { 0.0 }; g_loss_function->Evaluate(s, g_rho); // Make sure 'g(s) != s', i.e. it is not an inlier, which would be a trivial case @@ -130,15 +131,16 @@ TEST(ComposedLoss, Constructor) // Check that 'f(g(s)) == g(s)' and the same for the first and second derivatives, since f is // the TrivialLoss - for (size_t i = 0; i < 3; ++i) { + for (size_t i = 0; i < 3; ++i) + { EXPECT_EQ(g_rho[i], rho[i]); } } // Create a loss with f_loss and g_loss parameters { - std::shared_ptr f_loss{new fuse_loss::HuberLoss}; - std::shared_ptr g_loss{new fuse_loss::TolerantLoss}; + std::shared_ptr f_loss{ new fuse_loss::HuberLoss }; + std::shared_ptr g_loss{ new fuse_loss::TolerantLoss }; fuse_loss::ComposedLoss composed_loss(f_loss, g_loss); EXPECT_NE(nullptr, composed_loss.fLoss().get()); @@ -158,16 +160,16 @@ TEST(ComposedLoss, Constructor) ASSERT_NE(nullptr, g_loss_function); const double s = 1.5; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; composed_loss_function->Evaluate(s, rho); - double g_rho[3] = {0.0}; + double g_rho[3] = { 0.0 }; g_loss_function->Evaluate(s, g_rho); // Make sure 'g(s) != s', i.e. it is not an inlier, which would be a trivial case ASSERT_NE(s, g_rho[0]); - double f_rho[3] = {0.0}; + double f_rho[3] = { 0.0 }; f_loss_function->Evaluate(g_rho[0], f_rho); // Make sure 'f(s) != s', i.e. it is not an inlier, which would be a trivial case @@ -183,39 +185,40 @@ TEST(ComposedLoss, Constructor) struct CostFunctor { - explicit CostFunctor(const double data) - : data(data) - {} + explicit CostFunctor(const double data) : data(data) + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(data); return true; } - double data{0.0}; + double data{ 0.0 }; }; TEST(ComposedLoss, Optimization) { // Create a simple parameter - double x{5.0}; + double x{ 5.0 }; // Create a simple inlier constraint - const double inlier{1.0}; + const double inlier{ 1.0 }; // Create a simple outlier constraint - const double outlier{10.0}; - ceres::CostFunction * cost_function_outlier = - new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); + const double outlier{ 10.0 }; + ceres::CostFunction* cost_function_outlier = + new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); // Create an 'f' loss - const double a{0.05}; - std::shared_ptr f_loss{new fuse_loss::HuberLoss(a)}; + const double a{ 0.05 }; + std::shared_ptr f_loss{ new fuse_loss::HuberLoss(a) }; // Create an 'g' loss - const double scaled_a{0.5}; - std::shared_ptr g_loss{new fuse_loss::ScaledLoss(scaled_a)}; + const double scaled_a{ 0.5 }; + std::shared_ptr g_loss{ new fuse_loss::ScaledLoss(scaled_a) }; // Create a composed loss, which illustrates the case of scaling the residuals by a factor with a // fuse_loss::ScaledLoss in the 'g' loss and applies a fuse_loss::HuberLoss loss function robust @@ -229,21 +232,19 @@ TEST(ComposedLoss, Optimization) ceres::Problem problem(problem_options); - const size_t num_inliers{1000}; - for (size_t i = 0; i < num_inliers; ++i) { - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), - composed_loss.lossFunction(), // A nullptr here would produce a slightly better solution - &x); + const size_t num_inliers{ 1000 }; + for (size_t i = 0; i < num_inliers; ++i) + { + problem.AddResidualBlock(new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), + composed_loss.lossFunction(), // A nullptr here would produce a slightly better solution + &x); } // Add outlier constraints - const size_t num_outliers{9}; - for (size_t i = 0; i < num_outliers; ++i) { - problem.AddResidualBlock( - cost_function_outlier, - composed_loss.lossFunction(), - &x); + const size_t num_outliers{ 9 }; + for (size_t i = 0; i < num_outliers; ++i) + { + problem.AddResidualBlock(cost_function_outlier, composed_loss.lossFunction(), &x); } // Run the solver @@ -272,13 +273,13 @@ TEST(ComposedLoss, Optimization) TEST(ComposedLoss, Serialization) { // Construct an 'f' loss - const double f_loss_a{0.3}; - std::shared_ptr f_loss{new fuse_loss::HuberLoss(f_loss_a)}; + const double f_loss_a{ 0.3 }; + std::shared_ptr f_loss{ new fuse_loss::HuberLoss(f_loss_a) }; // Construct a 'g' loss - const double g_loss_a{0.3}; - const double g_loss_b{0.6}; - std::shared_ptr g_loss{new fuse_loss::TolerantLoss(g_loss_a, g_loss_b)}; + const double g_loss_a{ 0.3 }; + const double g_loss_b{ 0.6 }; + std::shared_ptr g_loss{ new fuse_loss::TolerantLoss(g_loss_a, g_loss_b) }; // Construct a composed loss fuse_loss::ComposedLoss expected(f_loss, g_loss); @@ -307,13 +308,14 @@ TEST(ComposedLoss, Serialization) // Test inlier (s <= g_loss_a*g_loss_a) const double s = 0.95 * g_loss_a * g_loss_a; - double expected_rho[3] = {0.0}; + double expected_rho[3] = { 0.0 }; expected_loss_function->Evaluate(s, expected_rho); - double actual_rho[3] = {0.0}; + double actual_rho[3] = { 0.0 }; actual_loss_function->Evaluate(s, actual_rho); - for (size_t i = 0; i < 3; ++i) { + for (size_t i = 0; i < 3; ++i) + { EXPECT_EQ(expected_rho[i], actual_rho[i]); } @@ -323,7 +325,8 @@ TEST(ComposedLoss, Serialization) expected_loss_function->Evaluate(s_outlier, expected_rho); actual_loss_function->Evaluate(s_outlier, actual_rho); - for (size_t i = 0; i < 3; ++i) { + for (size_t i = 0; i < 3; ++i) + { EXPECT_EQ(expected_rho[i], actual_rho[i]); } } diff --git a/fuse_loss/test/test_huber_loss.cpp b/fuse_loss/test/test_huber_loss.cpp index 2884ef3b0..f21ac62d7 100644 --- a/fuse_loss/test/test_huber_loss.cpp +++ b/fuse_loss/test/test_huber_loss.cpp @@ -49,7 +49,7 @@ TEST(HuberLoss, Constructor) // Create a loss with a parameter { - const double a{0.3}; + const double a{ 0.3 }; fuse_loss::HuberLoss loss(a); ASSERT_EQ(a, loss.a()); } @@ -57,31 +57,32 @@ TEST(HuberLoss, Constructor) struct CostFunctor { - explicit CostFunctor(const double data) - : data(data) - {} + explicit CostFunctor(const double data) : data(data) + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(data); return true; } - double data{0.0}; + double data{ 0.0 }; }; TEST(HuberLoss, Optimization) { // Create a simple parameter - double x{5.0}; + double x{ 5.0 }; // Create a simple inlier constraint - const double inlier{1.0}; + const double inlier{ 1.0 }; // Create a simple outlier constraint - const double outlier{10.0}; - ceres::CostFunction * cost_function_outlier = - new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); + const double outlier{ 10.0 }; + ceres::CostFunction* cost_function_outlier = + new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); // Create loss fuse_loss::HuberLoss loss(0.1); @@ -92,21 +93,19 @@ TEST(HuberLoss, Optimization) ceres::Problem problem(problem_options); - const size_t num_inliers{1000}; - for (size_t i = 0; i < num_inliers; ++i) { - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), - loss.lossFunction(), // A nullptr here would produce a slightly better solution - &x); + const size_t num_inliers{ 1000 }; + for (size_t i = 0; i < num_inliers; ++i) + { + problem.AddResidualBlock(new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), + loss.lossFunction(), // A nullptr here would produce a slightly better solution + &x); } // Add outlier constraints - const size_t num_outliers{9}; - for (size_t i = 0; i < num_outliers; ++i) { - problem.AddResidualBlock( - cost_function_outlier, - loss.lossFunction(), - &x); + const size_t num_outliers{ 9 }; + for (size_t i = 0; i < num_outliers; ++i) + { + problem.AddResidualBlock(cost_function_outlier, loss.lossFunction(), &x); } // Run the solver @@ -135,7 +134,7 @@ TEST(HuberLoss, Optimization) TEST(HuberLoss, Serialization) { // Construct a loss - const double a{0.3}; + const double a{ 0.3 }; fuse_loss::HuberLoss expected(a); // Serialize the loss into an archive @@ -158,7 +157,7 @@ TEST(HuberLoss, Serialization) // Test inlier (s <= a*a) const double s = 0.95 * a * a; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; actual.lossFunction()->Evaluate(s, rho); EXPECT_EQ(s, rho[0]); diff --git a/fuse_loss/test/test_loss_function.cpp b/fuse_loss/test/test_loss_function.cpp index 7a8e02520..005177faa 100644 --- a/fuse_loss/test/test_loss_function.cpp +++ b/fuse_loss/test/test_loss_function.cpp @@ -64,7 +64,7 @@ namespace // Compares the values of rho'(s) and rho''(s) computed by the // callback with estimates obtained by symmetric finite differencing // of rho(s). -void AssertLossFunctionIsValid(const ceres::LossFunction & loss, double s) +void AssertLossFunctionIsValid(const ceres::LossFunction& loss, double s) { ASSERT_GT(s, 0); diff --git a/fuse_loss/test/test_qwt_loss_plot.cpp b/fuse_loss/test/test_qwt_loss_plot.cpp index 2a563d1b0..21d67d7b5 100644 --- a/fuse_loss/test/test_qwt_loss_plot.cpp +++ b/fuse_loss/test/test_qwt_loss_plot.cpp @@ -57,15 +57,16 @@ class QwtLossPlotTest : public testing::Test QwtLossPlotTest() { // Generate samples: - const double step{0.01}; - const size_t half_samples{1000}; + const double step{ 0.01 }; + const size_t half_samples{ 1000 }; const double x_min = -(half_samples * step); - const size_t samples{2 * half_samples + 1}; + const size_t samples{ 2 * half_samples + 1 }; residuals.reserve(samples); - for (size_t i = 0; i < samples; ++i) { + for (size_t i = 0; i < samples; ++i) + { residuals.push_back(x_min + i * step); } } @@ -76,19 +77,15 @@ class QwtLossPlotTest : public testing::Test TEST_F(QwtLossPlotTest, PlotLossQt) { // Create losses - std::vector> losses{{ // NOLINT(whitespace/braces) - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared(), - std::make_shared() - }}; + std::vector> losses{ + { // NOLINT(whitespace/braces) + std::make_shared(), std::make_shared(), + std::make_shared(), std::make_shared(), + std::make_shared(), std::make_shared(), + std::make_shared(), std::make_shared(), + std::make_shared(), std::make_shared(), + std::make_shared() } + }; // Create a Qt application: int argc = 0; @@ -102,14 +99,15 @@ TEST_F(QwtLossPlotTest, PlotLossQt) fuse_loss::HSVColormap colormap(losses.size()); fuse_loss::QwtLossPlot rho_loss_plot(residuals, colormap); - auto & plot = rho_loss_plot.plot(); + auto& plot = rho_loss_plot.plot(); plot.setTitle("rho function"); plot.setAxisTitle(QwtPlot::xBottom, "r"); plot.setAxisTitle(QwtPlot::yLeft, "rho(r)"); plot.setAxisScale(QwtPlot::yLeft, 0.0, 15.0); // Create a curve for each loss rho function: - for (const auto & loss : losses) { + for (const auto& loss : losses) + { rho_loss_plot.plotRho(loss); } @@ -118,7 +116,7 @@ TEST_F(QwtLossPlotTest, PlotLossQt) // Create a loss plot for the influence function: fuse_loss::QwtLossPlot influence_loss_plot(residuals, colormap); - auto & influence_plot = influence_loss_plot.plot(); + auto& influence_plot = influence_loss_plot.plot(); influence_plot.setTitle("influence"); influence_plot.setAxisTitle(QwtPlot::xBottom, "r"); @@ -126,7 +124,8 @@ TEST_F(QwtLossPlotTest, PlotLossQt) influence_plot.setAxisScale(QwtPlot::yLeft, -3.0, 3.0); // Create a curve for each loss rho function: - for (const auto & loss : losses) { + for (const auto& loss : losses) + { influence_loss_plot.plotInfluence(loss); } @@ -135,7 +134,7 @@ TEST_F(QwtLossPlotTest, PlotLossQt) // Create a loss plot for the weight function: fuse_loss::QwtLossPlot weight_loss_plot(residuals, colormap); - auto & weight_plot = weight_loss_plot.plot(); + auto& weight_plot = weight_loss_plot.plot(); weight_plot.setTitle("weight"); weight_plot.setAxisTitle(QwtPlot::xBottom, "r"); @@ -143,7 +142,8 @@ TEST_F(QwtLossPlotTest, PlotLossQt) weight_plot.setAxisScale(QwtPlot::yLeft, 0.0, 1.5); // Create a curve for each loss rho function: - for (const auto & loss : losses) { + for (const auto& loss : losses) + { weight_loss_plot.plotWeight(loss); } @@ -152,7 +152,7 @@ TEST_F(QwtLossPlotTest, PlotLossQt) // Create a loss plot for the second derivative function: fuse_loss::QwtLossPlot second_derivative_loss_plot(residuals, colormap); - auto & second_derivative_plot = second_derivative_loss_plot.plot(); + auto& second_derivative_plot = second_derivative_loss_plot.plot(); second_derivative_plot.setTitle("2nd derivative"); second_derivative_plot.setAxisTitle(QwtPlot::xBottom, "r"); @@ -160,7 +160,8 @@ TEST_F(QwtLossPlotTest, PlotLossQt) second_derivative_plot.setAxisScale(QwtPlot::yLeft, -0.15, 0.15); // Create a curve for each loss rho function: - for (const auto & loss : losses) { + for (const auto& loss : losses) + { second_derivative_loss_plot.plotSecondDerivative(loss); } diff --git a/fuse_loss/test/test_scaled_loss.cpp b/fuse_loss/test/test_scaled_loss.cpp index 7b87a9540..c732a7536 100644 --- a/fuse_loss/test/test_scaled_loss.cpp +++ b/fuse_loss/test/test_scaled_loss.cpp @@ -53,7 +53,7 @@ TEST(ScaledLoss, Constructor) // Create a loss with a parameter { - const double a{0.3}; + const double a{ 0.3 }; fuse_loss::ScaledLoss scaled_loss(a); EXPECT_EQ(a, scaled_loss.a()); EXPECT_EQ(nullptr, scaled_loss.loss()); @@ -61,9 +61,9 @@ TEST(ScaledLoss, Constructor) // Create a loss with a parameter and loss function to scale { - std::shared_ptr loss{new fuse_loss::HuberLoss}; + std::shared_ptr loss{ new fuse_loss::HuberLoss }; - const double a{0.3}; + const double a{ 0.3 }; fuse_loss::ScaledLoss scaled_loss(a, loss); EXPECT_EQ(a, scaled_loss.a()); EXPECT_NE(nullptr, scaled_loss.loss()); @@ -73,38 +73,39 @@ TEST(ScaledLoss, Constructor) struct CostFunctor { - explicit CostFunctor(const double data) - : data(data) - {} + explicit CostFunctor(const double data) : data(data) + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(data); return true; } - double data{0.0}; + double data{ 0.0 }; }; TEST(ScaledLoss, Optimization) { // Create a simple parameter - double x{5.0}; + double x{ 5.0 }; // Create a simple inlier constraint - const double inlier{1.0}; + const double inlier{ 1.0 }; // Create a simple outlier constraint - const double outlier{10.0}; - ceres::CostFunction * cost_function_outlier = - new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); + const double outlier{ 10.0 }; + ceres::CostFunction* cost_function_outlier = + new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); // Create loss - const double a{0.1}; - std::shared_ptr loss{new fuse_loss::HuberLoss(a)}; + const double a{ 0.1 }; + std::shared_ptr loss{ new fuse_loss::HuberLoss(a) }; // Create a scaled loss, which should not have a significant impact in this test - const double scaled_a{0.7}; + const double scaled_a{ 0.7 }; fuse_loss::ScaledLoss scaled_loss(scaled_a, loss); // Build the problem. @@ -113,21 +114,19 @@ TEST(ScaledLoss, Optimization) ceres::Problem problem(problem_options); - const size_t num_inliers{1000}; - for (size_t i = 0; i < num_inliers; ++i) { - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), - scaled_loss.lossFunction(), // A nullptr here would produce a slightly better solution - &x); + const size_t num_inliers{ 1000 }; + for (size_t i = 0; i < num_inliers; ++i) + { + problem.AddResidualBlock(new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), + scaled_loss.lossFunction(), // A nullptr here would produce a slightly better solution + &x); } // Add outlier constraints - const size_t num_outliers{9}; - for (size_t i = 0; i < num_outliers; ++i) { - problem.AddResidualBlock( - cost_function_outlier, - scaled_loss.lossFunction(), - &x); + const size_t num_outliers{ 9 }; + for (size_t i = 0; i < num_outliers; ++i) + { + problem.AddResidualBlock(cost_function_outlier, scaled_loss.lossFunction(), &x); } // Run the solver @@ -156,11 +155,11 @@ TEST(ScaledLoss, Optimization) TEST(ScaledLoss, Serialization) { // Construct a loss - const double loss_a{0.3}; - std::shared_ptr loss{new fuse_loss::HuberLoss(loss_a)}; + const double loss_a{ 0.3 }; + std::shared_ptr loss{ new fuse_loss::HuberLoss(loss_a) }; // Construct a scaled loss - const double a{0.7}; + const double a{ 0.7 }; fuse_loss::ScaledLoss expected(a, loss); // Serialize the loss into an archive @@ -184,7 +183,7 @@ TEST(ScaledLoss, Serialization) // Test inlier (s <= loss_a*loss_a) const double s = 0.95 * loss_a * loss_a; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; actual.lossFunction()->Evaluate(s, rho); EXPECT_EQ(a * s, rho[0]); diff --git a/fuse_loss/test/test_tukey_loss.cpp b/fuse_loss/test/test_tukey_loss.cpp index 39aaf6cb0..702d293a6 100644 --- a/fuse_loss/test/test_tukey_loss.cpp +++ b/fuse_loss/test/test_tukey_loss.cpp @@ -51,7 +51,7 @@ TEST(TukeyLoss, Constructor) // Create a loss with a parameter { - const double a{0.3}; + const double a{ 0.3 }; fuse_loss::TukeyLoss loss(a); ASSERT_EQ(a, loss.a()); } @@ -72,31 +72,32 @@ TEST(TukeyLoss, Evaluate) struct CostFunctor { - explicit CostFunctor(const double data) - : data(data) - {} + explicit CostFunctor(const double data) : data(data) + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(data); return true; } - double data{0.0}; + double data{ 0.0 }; }; TEST(TukeyLoss, Optimization) { // Create a simple parameter - double x{5.0}; + double x{ 5.0 }; // Create a simple inlier constraint - const double inlier{1.0}; + const double inlier{ 1.0 }; // Create a simple outlier constraint - const double outlier{10.0}; - ceres::CostFunction * cost_function_outlier = - new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); + const double outlier{ 10.0 }; + ceres::CostFunction* cost_function_outlier = + new ceres::AutoDiffCostFunction(new CostFunctor(outlier)); // Create loss with a = x, so the initial value of x is not handled in the outlier region, in // which case the optimization does not convergence and the initial solution does not change @@ -108,21 +109,19 @@ TEST(TukeyLoss, Optimization) ceres::Problem problem(problem_options); - const size_t num_inliers{1000}; - for (size_t i = 0; i < num_inliers; ++i) { - problem.AddResidualBlock( - new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), - loss.lossFunction(), // A nullptr here would produce a slightly better solution - &x); + const size_t num_inliers{ 1000 }; + for (size_t i = 0; i < num_inliers; ++i) + { + problem.AddResidualBlock(new ceres::AutoDiffCostFunction(new CostFunctor(inlier)), + loss.lossFunction(), // A nullptr here would produce a slightly better solution + &x); } // Add outlier constraints - const size_t num_outliers{9}; - for (size_t i = 0; i < num_outliers; ++i) { - problem.AddResidualBlock( - cost_function_outlier, - loss.lossFunction(), - &x); + const size_t num_outliers{ 9 }; + for (size_t i = 0; i < num_outliers; ++i) + { + problem.AddResidualBlock(cost_function_outlier, loss.lossFunction(), &x); } // Run the solver @@ -151,7 +150,7 @@ TEST(TukeyLoss, Optimization) TEST(TukeyLoss, Serialization) { // Construct a loss - const double a{0.3}; + const double a{ 0.3 }; fuse_loss::TukeyLoss expected(a); // Serialize the loss into an archive @@ -174,7 +173,7 @@ TEST(TukeyLoss, Serialization) // Test inlier (s <= a*a) const double s = 0.95 * a * a; - double rho[3] = {0.0}; + double rho[3] = { 0.0 }; actual.lossFunction()->Evaluate(s, rho); EXPECT_GT(a * a / 3.0, rho[0]); diff --git a/fuse_models/CHANGELOG.rst b/fuse_models/CHANGELOG.rst index 8c424f0c8..4adc493c2 100644 --- a/fuse_models/CHANGELOG.rst +++ b/fuse_models/CHANGELOG.rst @@ -7,7 +7,7 @@ Changelog for package fuse_models 1.1.1 (2024-05-02) ------------------ -* Required formatting changes for the lastest version of ROS 2 Rolling (`#368 `_) +* Required formatting changes for the latest version of ROS 2 Rolling (`#368 `_) * Contributors: Stephen Williams 1.1.0 (2024-04-20) @@ -90,11 +90,11 @@ Changelog for package fuse_models 0.4.1 (2021-07-13) ------------------ * Getting versions in sync -* Substract minimum twist covariance from twist covariance (`#222 `_) - * Substract min twist cov from twist cov +* Subtract minimum twist covariance from twist covariance (`#222 `_) + * Subtract min twist cov from twist cov If the twist covariance already had a minimum twist covariance added to it to prevent ill-conditioned covariance matrices, we need a way to - substract that minimum twist covariance from it before we compute the + subtract that minimum twist covariance from it before we compute the pose relative covariance. Otherwise, we cannot get the original pose relative covariance because the minimum twist covariance term is multiplies by the time delta, which could actually make the resulting diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index fe3bc9452..2d825c356 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -34,12 +34,13 @@ find_package(tf2_ros REQUIRED) find_package(Ceres REQUIRED) find_package(Eigen3 REQUIRED) -########### -## Build ## -########### +# ############################################################################## +# Build ## +# ############################################################################## -## Declare a C++ library -add_library(${PROJECT_NAME} +# Declare a C++ library +add_library( + ${PROJECT_NAME} src/acceleration_2d.cpp src/graph_ignition.cpp src/imu_2d.cpp @@ -50,13 +51,13 @@ add_library(${PROJECT_NAME} src/twist_2d.cpp src/unicycle_2d.cpp src/unicycle_2d_ignition.cpp - src/unicycle_2d_state_kinematic_constraint.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$" -) -target_link_libraries(${PROJECT_NAME} + src/unicycle_2d_state_kinematic_constraint.cpp) +target_include_directories( + ${PROJECT_NAME} + PUBLIC "$" + "$") +target_link_libraries( + ${PROJECT_NAME} Ceres::ceres fuse_constraints::fuse_constraints fuse_core::fuse_core @@ -74,47 +75,35 @@ target_link_libraries(${PROJECT_NAME} tf2::tf2 tf2_2d::tf2_2d ${tf2_geometry_msgs_TARGETS} - tf2_ros::tf2_ros -) + tf2_ros::tf2_ros) -############# -## Testing ## -############# +# ############################################################################## +# Testing ## +# ############################################################################## if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # This is because these two files have unsuitable copyrights - set(_linter_excludes - test/test_sensor_proc.cpp - test/test_unicycle_2d.cpp - ) - set(AMENT_LINT_AUTO_FILE_EXCLUDE ${_linter_excludes}) - ament_lint_auto_find_test_dependencies() add_subdirectory(test) add_subdirectory(benchmark) endif() -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## -install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-export ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) pluginlib_export_plugin_description_file(fuse_core fuse_plugins.xml) ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET) ament_export_dependencies( ament_cmake_ros - fuse_constraints fuse_core fuse_graphs @@ -132,10 +121,8 @@ ament_export_dependencies( tf2_2d tf2_geometry_msgs tf2_ros - Ceres Eigen3 - Boost -) + Boost) ament_package() diff --git a/fuse_models/benchmark/CMakeLists.txt b/fuse_models/benchmark/CMakeLists.txt index e02fe6bbc..4a0a86c90 100644 --- a/fuse_models/benchmark/CMakeLists.txt +++ b/fuse_models/benchmark/CMakeLists.txt @@ -3,19 +3,11 @@ find_package(benchmark QUIET) if(benchmark_FOUND) add_executable(benchmark_unicycle_2d_state_cost_function - benchmark_unicycle_2d_state_cost_function.cpp - ) + benchmark_unicycle_2d_state_cost_function.cpp) if(TARGET benchmark_unicycle_2d_state_cost_function) - target_link_libraries( - benchmark_unicycle_2d_state_cost_function - benchmark - Ceres::ceres - ${PROJECT_NAME} - ) + target_link_libraries(benchmark_unicycle_2d_state_cost_function benchmark + Ceres::ceres ${PROJECT_NAME}) set_target_properties(benchmark_unicycle_2d_state_cost_function - PROPERTIES - CXX_STANDARD 17 - CXX_STANDARD_REQUIRED YES - ) + PROPERTIES CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES) endif() endif() diff --git a/fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp b/fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp index f1a5b210b..ad3cc33de 100644 --- a/fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/benchmark/benchmark_unicycle_2d_state_cost_function.cpp @@ -45,35 +45,34 @@ class Unicycle2DStateCostFunction : public benchmark::Fixture public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Unicycle2DStateCostFunction() - : jacobians(num_parameter_blocks) - , J(num_parameter_blocks) + Unicycle2DStateCostFunction() : jacobians(num_parameter_blocks), J(num_parameter_blocks) { - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } } // Analytic cost function - static constexpr double dt{0.1}; + static constexpr double dt{ 0.1 }; static const fuse_core::Matrix8d sqrt_information; static const fuse_models::Unicycle2DStateCostFunction cost_function; // Parameters - static const double * parameters[]; + static const double* parameters[]; // Residuals fuse_core::Vector8d residuals; - static const std::vector & block_sizes; + static const std::vector& block_sizes; static const size_t num_parameter_blocks; static const size_t num_residuals; // Jacobians - std::vector jacobians; + std::vector jacobians; private: // Cost function process noise and covariance @@ -99,63 +98,55 @@ class Unicycle2DStateCostFunction : public benchmark::Fixture }; // Cost function process noise and covariance -const double Unicycle2DStateCostFunction::process_noise_diagonal[] = { - 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 -}; +const double Unicycle2DStateCostFunction::process_noise_diagonal[] = { 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 }; const fuse_core::Matrix8d Unicycle2DStateCostFunction::covariance = - fuse_core::Vector8d(process_noise_diagonal).asDiagonal(); + fuse_core::Vector8d(process_noise_diagonal).asDiagonal(); // Parameter blocks -const double Unicycle2DStateCostFunction::position1[] = {0.0, 0.0}; -const double Unicycle2DStateCostFunction::yaw1[] = {0.0}; -const double Unicycle2DStateCostFunction::vel_linear1[] = {1.0, 0.0}; -const double Unicycle2DStateCostFunction::vel_yaw1[] = {1.570796327}; -const double Unicycle2DStateCostFunction::acc_linear1[] = {1.0, 0.0}; - -const double Unicycle2DStateCostFunction::position2[] = {0.105, 0.0}; -const double Unicycle2DStateCostFunction::yaw2[] = {0.1570796327}; -const double Unicycle2DStateCostFunction::vel_linear2[] = {1.1, 0.0}; -const double Unicycle2DStateCostFunction::vel_yaw2[] = {1.570796327}; -const double Unicycle2DStateCostFunction::acc_linear2[] = {1.0, 0.0}; +const double Unicycle2DStateCostFunction::position1[] = { 0.0, 0.0 }; +const double Unicycle2DStateCostFunction::yaw1[] = { 0.0 }; +const double Unicycle2DStateCostFunction::vel_linear1[] = { 1.0, 0.0 }; +const double Unicycle2DStateCostFunction::vel_yaw1[] = { 1.570796327 }; +const double Unicycle2DStateCostFunction::acc_linear1[] = { 1.0, 0.0 }; + +const double Unicycle2DStateCostFunction::position2[] = { 0.105, 0.0 }; +const double Unicycle2DStateCostFunction::yaw2[] = { 0.1570796327 }; +const double Unicycle2DStateCostFunction::vel_linear2[] = { 1.1, 0.0 }; +const double Unicycle2DStateCostFunction::vel_yaw2[] = { 1.570796327 }; +const double Unicycle2DStateCostFunction::acc_linear2[] = { 1.0, 0.0 }; // Analytic cost function -const fuse_core::Matrix8d Unicycle2DStateCostFunction::sqrt_information(covariance.inverse().llt(). - matrixU()); +const fuse_core::Matrix8d Unicycle2DStateCostFunction::sqrt_information(covariance.inverse().llt().matrixU()); -const fuse_models::Unicycle2DStateCostFunction Unicycle2DStateCostFunction::cost_function{dt, - sqrt_information}; +const fuse_models::Unicycle2DStateCostFunction Unicycle2DStateCostFunction::cost_function{ dt, sqrt_information }; // Parameters -const double * Unicycle2DStateCostFunction::parameters[] = { // NOLINT(whitespace/braces) - position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, position2, yaw2, vel_linear2, vel_yaw2, - acc_linear2 +const double* Unicycle2DStateCostFunction::parameters[] = { // NOLINT(whitespace/braces) + position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 }; -const std::vector & Unicycle2DStateCostFunction::block_sizes = - cost_function.parameter_block_sizes(); +const std::vector& Unicycle2DStateCostFunction::block_sizes = cost_function.parameter_block_sizes(); const size_t Unicycle2DStateCostFunction::num_parameter_blocks = block_sizes.size(); const size_t Unicycle2DStateCostFunction::num_residuals = cost_function.num_residuals(); -BENCHMARK_F(Unicycle2DStateCostFunction, AnalyticUnicycle2DCostFunction)(benchmark::State & state) +BENCHMARK_F(Unicycle2DStateCostFunction, AnalyticUnicycle2DCostFunction)(benchmark::State& state) { - for (auto _ : state) { + for (auto _ : state) + { cost_function.Evaluate(parameters, residuals.data(), jacobians.data()); } } -BENCHMARK_F( - Unicycle2DStateCostFunction, - AutoDiffUnicycle2DStateCostFunction)(benchmark::State & state) +BENCHMARK_F(Unicycle2DStateCostFunction, AutoDiffUnicycle2DStateCostFunction)(benchmark::State& state) { // Create cost function using automatic differentiation on the cost functor - ceres::AutoDiffCostFunction< - fuse_models::Unicycle2DStateCostFunctor, 8, 2, 1, 2, 1, 2, 2, 1, 2, 1, 2 - > - cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); + ceres::AutoDiffCostFunction + cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); - for (auto _ : state) { + for (auto _ : state) + { cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians.data()); } } diff --git a/fuse_models/include/fuse_models/acceleration_2d.hpp b/fuse_models/include/fuse_models/acceleration_2d.hpp index c2b46b7d0..3236ccf4b 100644 --- a/fuse_models/include/fuse_models/acceleration_2d.hpp +++ b/fuse_models/include/fuse_models/acceleration_2d.hpp @@ -49,7 +49,6 @@ #include #include - namespace fuse_models { @@ -91,16 +90,14 @@ class Acceleration2D : public fuse_core::AsyncSensorModel /** * @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; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for acceleration messages * @param[in] msg - The acceleration message to process */ - void process(const geometry_msgs::msg::AccelWithCovarianceStamped & msg); + void process(const geometry_msgs::msg::AccelWithCovarianceStamped& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -125,17 +122,14 @@ class Acceleration2D : public fuse_core::AsyncSensorModel */ void onStop() override; - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + 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 + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; @@ -146,7 +140,7 @@ class Acceleration2D : public fuse_core::AsyncSensorModel rclcpp::Subscription::SharedPtr sub_; using AccelerationThrottledCallback = - fuse_core::ThrottledMessageCallback; + fuse_core::ThrottledMessageCallback; AccelerationThrottledCallback throttled_callback_; }; diff --git a/fuse_models/include/fuse_models/common/sensor_config.hpp b/fuse_models/include/fuse_models/common/sensor_config.hpp index b9f2b3505..b402f9dab 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.hpp +++ b/fuse_models/include/fuse_models/common/sensor_config.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -62,7 +61,7 @@ namespace common * @param[in] dimension - The erroneous dimension name * @throws runtime_error */ -inline void throwDimensionError(const std::string & dimension) +inline void throwDimensionError(const std::string& dimension) { std::string error = "Dimension " + dimension + " is not valid for this type."; RCLCPP_ERROR_STREAM(rclcpp::get_logger("fuse"), error); @@ -78,12 +77,18 @@ inline void throwDimensionError(const std::string & dimension) * @return the index of the enumerated dimension for that type * @throws runtime_error if the dimension name is invalid */ -template -std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +template +std::enable_if_t::value, size_t> toIndex(const std::string& dimension) { auto lower_dim = boost::algorithm::to_lower_copy(dimension); - if (lower_dim == "x") {return static_cast(T::X);} - if (lower_dim == "y") {return static_cast(T::Y);} + if (lower_dim == "x") + { + return static_cast(T::X); + } + if (lower_dim == "y") + { + return static_cast(T::Y); + } throwDimensionError(dimension); @@ -99,11 +104,12 @@ std::enable_if_t::value, size_t> toIndex(const std::string & dim * @return the index of the enumerated dimension for that type * @throws runtime_error if the dimension name is invalid */ -template -std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +template +std::enable_if_t::value, size_t> toIndex(const std::string& dimension) { auto lower_dim = boost::algorithm::to_lower_copy(dimension); - if (lower_dim == "yaw" || lower_dim == "z") { + if (lower_dim == "yaw" || lower_dim == "z") + { return static_cast(fuse_variables::Orientation2DStamped::YAW); } @@ -122,17 +128,13 @@ std::enable_if_t::value, size_t> toIndex(const std::string & di * @return a vector of indices that are consistent with the enumerations for that variable type * @throws runtime_error if any dimension name is invalid */ -template -std::vector getDimensionIndices(const std::vector & dimension_names) +template +std::vector getDimensionIndices(const std::vector& dimension_names) { std::vector indices; indices.reserve(dimension_names.size()); - std::transform( - dimension_names.begin(), - dimension_names.end(), - std::back_inserter(indices), - toIndex); + std::transform(dimension_names.begin(), dimension_names.end(), std::back_inserter(indices), toIndex); // Remove duplicates std::sort(indices.begin(), indices.end()); diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 56d58ff0b..d60b2df1f 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -71,24 +71,21 @@ #include - static auto sensor_proc_clock = rclcpp::Clock(); namespace tf2 { /** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs TwistWithCovarianceStamped type. -* This function is a specialization of the doTransform template defined in tf2/convert.h. -* \param t_in The twist to transform, as a timestamped TwistWithCovarianceStamped message. -* \param t_out The transformed twist, as a timestamped TwistWithCovarianceStamped message. -* \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, - const geometry_msgs::msg::TransformStamped & transform) // NOLINT + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The twist to transform, as a timestamped TwistWithCovarianceStamped message. + * \param t_out The transformed twist, as a timestamped TwistWithCovarianceStamped message. + * \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, + const geometry_msgs::msg::TransformStamped& transform) // NOLINT { tf2::Vector3 vl; fromMsg(t_in.twist.twist.linear, vl); @@ -106,17 +103,15 @@ void doTransform( } /** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs AccelWithCovarianceStamped type. -* This function is a specialization of the doTransform template defined in tf2/convert.h. -* \param t_in The acceleration to transform, as a timestamped AccelWithCovarianceStamped message. -* \param t_out The transformed acceleration, as a timestamped AccelWithCovarianceStamped message. -* \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 + * This function is a specialization of the doTransform template defined in tf2/convert.h. + * \param t_in The acceleration to transform, as a timestamped AccelWithCovarianceStamped message. + * \param t_out The transformed acceleration, as a timestamped AccelWithCovarianceStamped message. + * \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 { tf2::Vector3 al; fromMsg(t_in.accel.accel.linear, al); @@ -135,7 +130,6 @@ void doTransform( } // namespace tf2 - namespace fuse_models { @@ -149,20 +143,14 @@ namespace common * @param[in] rhs_indices - RHS vector of indices * @param[in] rhs_offset - RHS offset to be added to the RHS vector indices (defaults to 0) */ -inline std::vector mergeIndices( - const std::vector & lhs_indices, - const std::vector & rhs_indices, - const size_t rhs_offset = 0u) +inline std::vector mergeIndices(const std::vector& lhs_indices, const std::vector& rhs_indices, + const size_t rhs_offset = 0u) { - auto merged_indices = - boost::copy_range>(boost::range::join(lhs_indices, rhs_indices)); + auto merged_indices = boost::copy_range>(boost::range::join(lhs_indices, rhs_indices)); const auto rhs_it = merged_indices.begin() + lhs_indices.size(); - std::transform( - rhs_it, - merged_indices.end(), - rhs_it, - std::bind(std::plus(), std::placeholders::_1, rhs_offset)); + std::transform(rhs_it, merged_indices.end(), rhs_it, + std::bind(std::plus(), std::placeholders::_1, rhs_offset)); return merged_indices; } @@ -178,17 +166,16 @@ inline std::vector mergeIndices( * @param[in,out] mean_partial - The partial measurement mean to which we want to append * @param[in,out] covariance_partial - The partial measurement covariance to which we want to append */ -inline void populatePartialMeasurement( - const fuse_core::VectorXd & mean_full, - const fuse_core::MatrixXd & covariance_full, - const std::vector & indices, - fuse_core::VectorXd & mean_partial, - fuse_core::MatrixXd & covariance_partial) +inline void populatePartialMeasurement(const fuse_core::VectorXd& mean_full, const fuse_core::MatrixXd& covariance_full, + const std::vector& indices, fuse_core::VectorXd& mean_partial, + fuse_core::MatrixXd& covariance_partial) { - for (size_t r = 0; r < indices.size(); ++r) { + for (size_t r = 0; r < indices.size(); ++r) + { mean_partial(r) = mean_full(indices[r]); - for (size_t c = 0; c < indices.size(); ++c) { + for (size_t c = 0; c < indices.size(); ++c) + { covariance_partial(r, c) = covariance_full(indices[r], indices[c]); } } @@ -202,25 +189,25 @@ inline void populatePartialMeasurement( * @param[in] covariance_partial - The partial measurement covariance we want to validate * @param[in] precision - The precision to validate the partial measurements covariance is symmetric */ -inline void validatePartialMeasurement( - const fuse_core::VectorXd & mean_partial, - const fuse_core::MatrixXd & covariance_partial, - const double precision = Eigen::NumTraits::dummy_precision()) +inline void validatePartialMeasurement(const fuse_core::VectorXd& mean_partial, + const fuse_core::MatrixXd& covariance_partial, + const double precision = Eigen::NumTraits::dummy_precision()) { - if (!mean_partial.allFinite()) { + if (!mean_partial.allFinite()) + { throw std::runtime_error("Invalid partial mean " + fuse_core::to_string(mean_partial)); } - if (!fuse_core::isSymmetric(covariance_partial, precision)) { - throw std::runtime_error( - "Non-symmetric partial covariance matrix\n" + - fuse_core::to_string(covariance_partial, Eigen::FullPrecision)); + if (!fuse_core::isSymmetric(covariance_partial, precision)) + { + throw std::runtime_error("Non-symmetric partial covariance matrix\n" + + fuse_core::to_string(covariance_partial, Eigen::FullPrecision)); } - if (!fuse_core::isPositiveDefinite(covariance_partial)) { - throw std::runtime_error( - "Non-positive-definite partial covariance matrix\n" + - fuse_core::to_string(covariance_partial, Eigen::FullPrecision)); + if (!fuse_core::isPositiveDefinite(covariance_partial)) + { + throw std::runtime_error("Non-positive-definite partial covariance matrix\n" + + fuse_core::to_string(covariance_partial, Eigen::FullPrecision)); } } @@ -233,31 +220,30 @@ inline void validatePartialMeasurement( * @param [in] timeout - Optional. The maximum time to wait for a transform to become available. * @return true if the transform succeeded, false otherwise */ -template -bool transformMessage( - const tf2_ros::Buffer & tf_buffer, - const T & input, - T & output, - const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +template +bool transformMessage(const tf2_ros::Buffer& tf_buffer, const T& input, T& output, + const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) { - try { + try + { auto trans = geometry_msgs::msg::TransformStamped(); - if (tf_timeout.nanoseconds() == 0) { - trans = tf_buffer.lookupTransform( - output.header.frame_id, input.header.frame_id, - input.header.stamp); - } else { - trans = tf_buffer.lookupTransform( - output.header.frame_id, input.header.frame_id, - input.header.stamp, tf_timeout); + if (tf_timeout.nanoseconds() == 0) + { + trans = tf_buffer.lookupTransform(output.header.frame_id, input.header.frame_id, input.header.stamp); + } + else + { + trans = tf_buffer.lookupTransform(output.header.frame_id, input.header.frame_id, input.header.stamp, tf_timeout); } tf2::doTransform(input, output, trans); return true; - } catch (const tf2::TransformException & ex) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 5.0 * 1000, - "Could not transform message from " << input.header.frame_id << " to " - << output.header.frame_id << ". Error was " << ex.what()); + } + catch (const tf2::TransformException& ex) + { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 5.0 * 1000, + "Could not transform message from " << input.header.frame_id << " to " + << output.header.frame_id + << ". Error was " << ex.what()); } return false; @@ -284,34 +270,35 @@ bool transformMessage( * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processAbsolutePoseWithCovariance( - const std::string & source, - const fuse_core::UUID & device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const fuse_core::Loss::SharedPtr & loss, - const std::string & target_frame, - const std::vector & position_indices, - const std::vector & orientation_indices, - const tf2_ros::Buffer & tf_buffer, - const bool validate, - fuse_core::Transaction & transaction, - const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +inline bool processAbsolutePoseWithCovariance(const std::string& source, const fuse_core::UUID& device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const fuse_core::Loss::SharedPtr& loss, const std::string& target_frame, + const std::vector& position_indices, + const std::vector& orientation_indices, + const tf2_ros::Buffer& tf_buffer, const bool validate, + fuse_core::Transaction& transaction, + const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) { - if (position_indices.empty() && orientation_indices.empty()) { + if (position_indices.empty() && orientation_indices.empty()) + { return false; } geometry_msgs::msg::PoseWithCovarianceStamped transformed_message; - if (target_frame.empty()) { + if (target_frame.empty()) + { transformed_message = pose; - } else { + } + else + { transformed_message.header.frame_id = target_frame; - if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Failed to transform pose message with stamp " << rclcpp::Time( - pose.header.stamp).nanoseconds() << ". Cannot create constraint."); + if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) + { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Failed to transform pose message with stamp " + << rclcpp::Time(pose.header.stamp).nanoseconds() + << ". Cannot create constraint."); return false; } } @@ -322,8 +309,7 @@ inline bool processAbsolutePoseWithCovariance( // Create the pose variable auto position = fuse_variables::Position2DStamped::make_shared(pose.header.stamp, device_id); - auto orientation = - fuse_variables::Orientation2DStamped::make_shared(pose.header.stamp, device_id); + auto orientation = fuse_variables::Orientation2DStamped::make_shared(pose.header.stamp, device_id); position->x() = absolute_pose_2d.x(); position->y() = absolute_pose_2d.y(); orientation->yaw() = absolute_pose_2d.yaw(); @@ -334,16 +320,11 @@ inline bool processAbsolutePoseWithCovariance( // Create the covariance for the constraint fuse_core::Matrix3d pose_covariance; - pose_covariance << - transformed_message.pose.covariance[0], - transformed_message.pose.covariance[1], - transformed_message.pose.covariance[5], - transformed_message.pose.covariance[6], - transformed_message.pose.covariance[7], - transformed_message.pose.covariance[11], - transformed_message.pose.covariance[30], - transformed_message.pose.covariance[31], - transformed_message.pose.covariance[35]; + pose_covariance << transformed_message.pose.covariance[0], transformed_message.pose.covariance[1], + transformed_message.pose.covariance[5], transformed_message.pose.covariance[6], + transformed_message.pose.covariance[7], transformed_message.pose.covariance[11], + transformed_message.pose.covariance[30], transformed_message.pose.covariance[31], + transformed_message.pose.covariance[35]; // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); @@ -351,31 +332,28 @@ inline bool processAbsolutePoseWithCovariance( const auto indices = mergeIndices(position_indices, orientation_indices, position->size()); - populatePartialMeasurement( - pose_mean, pose_covariance, indices, pose_mean_partial, - pose_covariance_partial); + populatePartialMeasurement(pose_mean, pose_covariance, indices, pose_mean_partial, pose_covariance_partial); - if (validate) { - try { + if (validate) + { + try + { validatePartialMeasurement(pose_mean_partial, pose_covariance_partial); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Invalid partial absolute pose measurement from '" << source - << "' source: " << ex.what()); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial absolute pose measurement from '" << source + << "' source: " << ex.what()); return false; } } // Create an absolute pose constraint - auto constraint = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - source, - *position, - *orientation, - pose_mean_partial, - pose_covariance_partial, - position_indices, - orientation_indices); + auto constraint = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared(source, *position, *orientation, pose_mean_partial, + pose_covariance_partial, position_indices, + orientation_indices); constraint->loss(loss); @@ -400,7 +378,7 @@ inline bool processAbsolutePoseWithCovariance( * Additionally, the covariance of each pose message is rotated into the robot's base frame at the * time of pose_absolute1. They are then added in the constraint if the pose measurements are * independent. Otherwise, if the pose measurements are dependent, the covariance of pose_absolute1 - * is substracted from the covariance of pose_absolute2. A small minimum relative covariance is + * is subtracted from the covariance of pose_absolute2. A small minimum relative covariance is * added to avoid getting a zero or ill-conditioned covariance. This could happen if both covariance * matrices are the same or very similar, e.g. when pose_absolute1 == pose_absolute2, it's possible * that the covariance is the same for both poses. @@ -409,7 +387,7 @@ inline bool processAbsolutePoseWithCovariance( * @param[in] device_id - The UUID of the machine * @param[in] pose1 - The first (and temporally earlier) PoseWithCovarianceStamped message * @param[in] pose2 - The second (and temporally later) PoseWithCovarianceStamped message - * @param[in] independent - Whether the pose measurements are indepent or not + * @param[in] independent - Whether the pose measurements are independent or not * @param[in] minimum_pose_relative_covariance - The minimum pose relative covariance that is always * added to the resulting pose relative covariance * @param[in] loss - The loss function for the 2D pose constraint generated @@ -418,20 +396,18 @@ inline bool processAbsolutePoseWithCovariance( * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processDifferentialPoseWithCovariance( - const std::string & source, - const fuse_core::UUID & device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose1, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose2, - const bool independent, - const fuse_core::Matrix3d & minimum_pose_relative_covariance, - const fuse_core::Loss::SharedPtr & loss, - const std::vector & position_indices, - const std::vector & orientation_indices, - const bool validate, - fuse_core::Transaction & transaction) +inline bool processDifferentialPoseWithCovariance(const std::string& source, const fuse_core::UUID& device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose1, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose2, + const bool independent, + const fuse_core::Matrix3d& minimum_pose_relative_covariance, + const fuse_core::Loss::SharedPtr& loss, + const std::vector& position_indices, + const std::vector& orientation_indices, const bool validate, + fuse_core::Transaction& transaction) { - if (position_indices.empty() && orientation_indices.empty()) { + if (position_indices.empty() && orientation_indices.empty()) + { return false; } @@ -444,16 +420,13 @@ inline bool processDifferentialPoseWithCovariance( // Create the pose variables auto position1 = fuse_variables::Position2DStamped::make_shared(pose1.header.stamp, device_id); - auto orientation1 = - fuse_variables::Orientation2DStamped::make_shared(pose1.header.stamp, device_id); + auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(pose1.header.stamp, device_id); position1->x() = pose1_2d.x(); position1->y() = pose1_2d.y(); orientation1->yaw() = pose1_2d.yaw(); auto position2 = fuse_variables::Position2DStamped::make_shared(pose2.header.stamp, device_id); - auto orientation2 = fuse_variables::Orientation2DStamped::make_shared( - pose2.header.stamp, - device_id); + auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(pose2.header.stamp, device_id); position2->x() = pose2_2d.x(); position2->y() = pose2_2d.y(); orientation2->yaw() = pose2_2d.yaw(); @@ -464,58 +437,38 @@ inline bool processDifferentialPoseWithCovariance( double x_diff = pose2_2d.x() - pose1_2d.x(); double y_diff = pose2_2d.y() - pose1_2d.y(); fuse_core::VectorXd pose_relative_mean(3); - pose_relative_mean << - cy * x_diff - sy * y_diff, - sy * x_diff + cy * y_diff, - (pose2_2d.rotation() - pose1_2d.rotation()).getAngle(); + pose_relative_mean << cy * x_diff - sy * y_diff, sy * x_diff + cy * y_diff, + (pose2_2d.rotation() - pose1_2d.rotation()).getAngle(); // Create the covariance components for the constraint fuse_core::Matrix3d cov1; - cov1 << - pose1.pose.covariance[0], - pose1.pose.covariance[1], - pose1.pose.covariance[5], - pose1.pose.covariance[6], - pose1.pose.covariance[7], - pose1.pose.covariance[11], - pose1.pose.covariance[30], - pose1.pose.covariance[31], - pose1.pose.covariance[35]; + cov1 << pose1.pose.covariance[0], pose1.pose.covariance[1], pose1.pose.covariance[5], pose1.pose.covariance[6], + pose1.pose.covariance[7], pose1.pose.covariance[11], pose1.pose.covariance[30], pose1.pose.covariance[31], + pose1.pose.covariance[35]; fuse_core::Matrix3d cov2; - cov2 << - pose2.pose.covariance[0], - pose2.pose.covariance[1], - pose2.pose.covariance[5], - pose2.pose.covariance[6], - pose2.pose.covariance[7], - pose2.pose.covariance[11], - pose2.pose.covariance[30], - pose2.pose.covariance[31], - pose2.pose.covariance[35]; + cov2 << pose2.pose.covariance[0], pose2.pose.covariance[1], pose2.pose.covariance[5], pose2.pose.covariance[6], + pose2.pose.covariance[7], pose2.pose.covariance[11], pose2.pose.covariance[30], pose2.pose.covariance[31], + pose2.pose.covariance[35]; fuse_core::Matrix3d pose_relative_covariance; - if (independent) { + if (independent) + { // Compute Jacobians so we can rotate the covariance fuse_core::Matrix3d j_pose1; /* *INDENT-OFF* */ - j_pose1 << - -cy, sy, sy * x_diff + cy * y_diff, - -sy, -cy, -cy * x_diff + sy * y_diff, - 0, 0, -1; + j_pose1 << -cy, sy, sy * x_diff + cy * y_diff, -sy, -cy, -cy * x_diff + sy * y_diff, 0, 0, -1; /* *INDENT-ON* */ fuse_core::Matrix3d j_pose2; /* *INDENT-OFF* */ - j_pose2 << - cy, -sy, 0, - sy, cy, 0, - 0, 0, 1; + j_pose2 << cy, -sy, 0, sy, cy, 0, 0, 0, 1; /* *INDENT-ON* */ - pose_relative_covariance = j_pose1 * cov1 * j_pose1.transpose() + j_pose2 * cov2 * - j_pose2.transpose(); - } else { + pose_relative_covariance = j_pose1 * cov1 * j_pose1.transpose() + j_pose2 * cov2 * j_pose2.transpose(); + } + else + { // For dependent pose measurements p1 and p2, we assume they're computed as: // // p2 = p1 * p12 [1] @@ -663,7 +616,7 @@ inline bool processDifferentialPoseWithCovariance( // which are the j_pose1 and j_pose2 jacobians used above for the covariance propagation // expression for independent pose measurements. // - // This seems to be the approach adviced in + // This seems to be the approach advised in // https://github.com/cra-ros-pkg/robot_localization/issues/356, but after comparing the // resulting relative pose covariance C12 and the twist covariance, we can conclude that the // approach proposed here is the only one that allow us to get results that match. @@ -679,63 +632,48 @@ inline bool processDifferentialPoseWithCovariance( // added to [2]. fuse_core::Matrix3d j_pose1; /* *INDENT-OFF* */ - j_pose1 << 1, 0, sy * pose_relative_mean(0) - cy * pose_relative_mean(1), - 0, 1, cy * pose_relative_mean(0) + sy * pose_relative_mean(1), - 0, 0, 1; + j_pose1 << 1, 0, sy * pose_relative_mean(0) - cy * pose_relative_mean(1), 0, 1, + cy * pose_relative_mean(0) + sy * pose_relative_mean(1), 0, 0, 1; /* *INDENT-ON* */ fuse_core::Matrix3d j_pose12_inv; /* *INDENT-OFF* */ - j_pose12_inv << cy, -sy, 0, - sy, cy, 0, - 0, 0, 1; + j_pose12_inv << cy, -sy, 0, sy, cy, 0, 0, 0, 1; /* *INDENT-ON* */ - pose_relative_covariance = j_pose12_inv * (cov2 - j_pose1 * cov1 * j_pose1.transpose()) * - j_pose12_inv.transpose() + - minimum_pose_relative_covariance; + pose_relative_covariance = j_pose12_inv * (cov2 - j_pose1 * cov1 * j_pose1.transpose()) * j_pose12_inv.transpose() + + minimum_pose_relative_covariance; } // Build the sub-vector and sub-matrices based on the requested indices - fuse_core::VectorXd pose_relative_mean_partial( - position_indices.size() + orientation_indices.size()); + fuse_core::VectorXd pose_relative_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_relative_covariance_partial(pose_relative_mean_partial.rows(), - pose_relative_mean_partial.rows()); + pose_relative_mean_partial.rows()); const auto indices = mergeIndices(position_indices, orientation_indices, position1->size()); - populatePartialMeasurement( - pose_relative_mean, - pose_relative_covariance, - indices, - pose_relative_mean_partial, - pose_relative_covariance_partial); - - if (validate) { - try { - validatePartialMeasurement( - pose_relative_mean_partial, pose_relative_covariance_partial, - 1e-6); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Invalid partial differential pose measurement from '" - << source << "' source: " << ex.what()); + populatePartialMeasurement(pose_relative_mean, pose_relative_covariance, indices, pose_relative_mean_partial, + pose_relative_covariance_partial); + + if (validate) + { + try + { + validatePartialMeasurement(pose_relative_mean_partial, pose_relative_covariance_partial, 1e-6); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement from '" << source + << "' source: " << ex.what()); return false; } } // Create a relative pose constraint. auto constraint = fuse_constraints::RelativePose2DStampedConstraint::make_shared( - source, - *position1, - *orientation1, - *position2, - *orientation2, - pose_relative_mean_partial, - pose_relative_covariance_partial, - position_indices, - orientation_indices); + source, *position1, *orientation1, *position2, *orientation2, pose_relative_mean_partial, + pose_relative_covariance_partial, position_indices, orientation_indices); constraint->loss(loss); @@ -774,7 +712,7 @@ inline bool processDifferentialPoseWithCovariance( * @param[in] minimum_pose_relative_covariance - The minimum pose relative covariance that is always * added to the resulting pose relative covariance * @param[in] twist_covariance_offset - The twist covariance offset that was added to the twist - * covariance and must be substracted from it before computing + * covariance and must be subtracted from it before computing * the pose relative covariance from it * @param[in] loss - The loss function for the 2D pose constraint generated * @param[in] validate - Whether to validate the measurements or not. If the validation fails no @@ -782,21 +720,19 @@ inline bool processDifferentialPoseWithCovariance( * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processDifferentialPoseWithTwistCovariance( - const std::string & source, - const fuse_core::UUID & device_id, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose1, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose2, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const fuse_core::Matrix3d & minimum_pose_relative_covariance, - const fuse_core::Matrix3d & twist_covariance_offset, - const fuse_core::Loss::SharedPtr & loss, - const std::vector & position_indices, - const std::vector & orientation_indices, - const bool validate, - fuse_core::Transaction & transaction) +inline bool processDifferentialPoseWithTwistCovariance(const std::string& source, const fuse_core::UUID& device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose1, + const geometry_msgs::msg::PoseWithCovarianceStamped& pose2, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, + const fuse_core::Matrix3d& minimum_pose_relative_covariance, + const fuse_core::Matrix3d& twist_covariance_offset, + const fuse_core::Loss::SharedPtr& loss, + const std::vector& position_indices, + const std::vector& orientation_indices, + const bool validate, fuse_core::Transaction& transaction) { - if (position_indices.empty() && orientation_indices.empty()) { + if (position_indices.empty() && orientation_indices.empty()) + { return false; } @@ -809,16 +745,13 @@ inline bool processDifferentialPoseWithTwistCovariance( // Create the pose variables auto position1 = fuse_variables::Position2DStamped::make_shared(pose1.header.stamp, device_id); - auto orientation1 = - fuse_variables::Orientation2DStamped::make_shared(pose1.header.stamp, device_id); + auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(pose1.header.stamp, device_id); position1->x() = pose1_2d.x(); position1->y() = pose1_2d.y(); orientation1->yaw() = pose1_2d.yaw(); auto position2 = fuse_variables::Position2DStamped::make_shared(pose2.header.stamp, device_id); - auto orientation2 = fuse_variables::Orientation2DStamped::make_shared( - pose2.header.stamp, - device_id); + auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(pose2.header.stamp, device_id); position2->x() = pose2_2d.x(); position2->y() = pose2_2d.y(); orientation2->yaw() = pose2_2d.yaw(); @@ -830,16 +763,9 @@ inline bool processDifferentialPoseWithTwistCovariance( // Create the covariance components for the constraint fuse_core::Matrix3d cov; - cov << - twist.twist.covariance[0], - twist.twist.covariance[1], - twist.twist.covariance[5], - twist.twist.covariance[6], - twist.twist.covariance[7], - twist.twist.covariance[11], - twist.twist.covariance[30], - twist.twist.covariance[31], - twist.twist.covariance[35]; + cov << twist.twist.covariance[0], twist.twist.covariance[1], twist.twist.covariance[5], twist.twist.covariance[6], + twist.twist.covariance[7], twist.twist.covariance[11], twist.twist.covariance[30], twist.twist.covariance[31], + twist.twist.covariance[35]; // For dependent pose measurements p1 and p2, we assume they're computed as: // @@ -878,10 +804,10 @@ inline bool processDifferentialPoseWithTwistCovariance( // covariance offset added to it by the publisher, so we have to remove it before using it. const auto dt = (rclcpp::Time(pose2.header.stamp) - rclcpp::Time(pose1.header.stamp)).seconds(); - if (dt < 1e-6) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Very small time difference " << dt << "s from '" << source << "' source."); + if (dt < 1e-6) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Very small time difference " << dt << "s from '" << source << "' source."); return false; } @@ -890,49 +816,37 @@ inline bool processDifferentialPoseWithTwistCovariance( j_twist *= dt; fuse_core::Matrix3d pose_relative_covariance = - j_twist * (cov - twist_covariance_offset) * j_twist.transpose() + - minimum_pose_relative_covariance; + j_twist * (cov - twist_covariance_offset) * j_twist.transpose() + minimum_pose_relative_covariance; // Build the sub-vector and sub-matrices based on the requested indices - fuse_core::VectorXd pose_relative_mean_partial( - position_indices.size() + orientation_indices.size()); + fuse_core::VectorXd pose_relative_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_relative_covariance_partial(pose_relative_mean_partial.rows(), - pose_relative_mean_partial.rows()); + pose_relative_mean_partial.rows()); const auto indices = mergeIndices(position_indices, orientation_indices, position1->size()); - populatePartialMeasurement( - pose_relative_mean, - pose_relative_covariance, - indices, - pose_relative_mean_partial, - pose_relative_covariance_partial); - - if (validate) { - try { - validatePartialMeasurement( - pose_relative_mean_partial, pose_relative_covariance_partial, - 1e-6); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Invalid partial differential pose measurement using the twist covariance from '" - << source << "' source: " << ex.what()); + populatePartialMeasurement(pose_relative_mean, pose_relative_covariance, indices, pose_relative_mean_partial, + pose_relative_covariance_partial); + + if (validate) + { + try + { + validatePartialMeasurement(pose_relative_mean_partial, pose_relative_covariance_partial, 1e-6); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement using the twist covariance from '" + << source << "' source: " << ex.what()); return false; } } // Create a relative pose constraint. auto constraint = fuse_constraints::RelativePose2DStampedConstraint::make_shared( - source, - *position1, - *orientation1, - *position2, - *orientation2, - pose_relative_mean_partial, - pose_relative_covariance_partial, - position_indices, - orientation_indices); + source, *position1, *orientation1, *position2, *orientation2, pose_relative_mean_partial, + pose_relative_covariance_partial, position_indices, orientation_indices); constraint->loss(loss); @@ -972,36 +886,36 @@ inline bool processDifferentialPoseWithTwistCovariance( * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processTwistWithCovariance( - const std::string & source, - const fuse_core::UUID & device_id, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const fuse_core::Loss::SharedPtr & linear_velocity_loss, - const fuse_core::Loss::SharedPtr & angular_velocity_loss, - const std::string & target_frame, - const std::vector & linear_indices, - const std::vector & angular_indices, - const tf2_ros::Buffer & tf_buffer, - const bool validate, - fuse_core::Transaction & transaction, - const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +inline bool processTwistWithCovariance(const std::string& source, const fuse_core::UUID& device_id, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, + const fuse_core::Loss::SharedPtr& linear_velocity_loss, + const fuse_core::Loss::SharedPtr& angular_velocity_loss, + const std::string& target_frame, const std::vector& linear_indices, + const std::vector& angular_indices, const tf2_ros::Buffer& tf_buffer, + const bool validate, fuse_core::Transaction& transaction, + const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) { // Make sure we actually have work to do - if (linear_indices.empty() && angular_indices.empty()) { + if (linear_indices.empty() && angular_indices.empty()) + { return false; } geometry_msgs::msg::TwistWithCovarianceStamped transformed_message; - if (target_frame.empty()) { + if (target_frame.empty()) + { transformed_message = twist; - } else { + } + else + { transformed_message.header.frame_id = target_frame; - if (!transformMessage(tf_buffer, twist, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Failed to transform twist message with stamp " << rclcpp::Time( - twist.header.stamp).nanoseconds() << ". Cannot create constraint."); + if (!transformMessage(tf_buffer, twist, transformed_message, tf_timeout)) + { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Failed to transform twist message with stamp " + << rclcpp::Time(twist.header.stamp).nanoseconds() + << ". Cannot create constraint."); return false; } } @@ -1009,56 +923,49 @@ inline bool processTwistWithCovariance( bool constraints_added = false; // Create two absolute constraints - if (!linear_indices.empty()) { - auto velocity_linear = - fuse_variables::VelocityLinear2DStamped::make_shared(twist.header.stamp, device_id); + if (!linear_indices.empty()) + { + auto velocity_linear = fuse_variables::VelocityLinear2DStamped::make_shared(twist.header.stamp, device_id); velocity_linear->x() = transformed_message.twist.twist.linear.x; velocity_linear->y() = transformed_message.twist.twist.linear.y; // Create the mean twist vectors for the constraints fuse_core::VectorXd linear_vel_mean(2); - linear_vel_mean << transformed_message.twist.twist.linear.x, - transformed_message.twist.twist.linear.y; + linear_vel_mean << transformed_message.twist.twist.linear.x, transformed_message.twist.twist.linear.y; // Create the covariances for the constraints fuse_core::MatrixXd linear_vel_covariance(2, 2); - linear_vel_covariance << - transformed_message.twist.covariance[0], - transformed_message.twist.covariance[1], - transformed_message.twist.covariance[6], - transformed_message.twist.covariance[7]; + linear_vel_covariance << transformed_message.twist.covariance[0], transformed_message.twist.covariance[1], + transformed_message.twist.covariance[6], transformed_message.twist.covariance[7]; // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd linear_vel_mean_partial(linear_indices.size()); - fuse_core::MatrixXd linear_vel_covariance_partial(linear_vel_mean_partial.rows(), - linear_vel_mean_partial.rows()); + fuse_core::MatrixXd linear_vel_covariance_partial(linear_vel_mean_partial.rows(), linear_vel_mean_partial.rows()); - populatePartialMeasurement( - linear_vel_mean, - linear_vel_covariance, - linear_indices, - linear_vel_mean_partial, - linear_vel_covariance_partial); + populatePartialMeasurement(linear_vel_mean, linear_vel_covariance, linear_indices, linear_vel_mean_partial, + linear_vel_covariance_partial); bool add_constraint = true; - if (validate) { - try { + if (validate) + { + try + { validatePartialMeasurement(linear_vel_mean_partial, linear_vel_covariance_partial); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Invalid partial linear velocity measurement from '" - << source << "' source: " << ex.what()); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial linear velocity measurement from '" << source + << "' source: " << ex.what()); add_constraint = false; } } - if (add_constraint) { - auto linear_vel_constraint = - fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared( - source, *velocity_linear, linear_vel_mean_partial, linear_vel_covariance_partial, - linear_indices); + if (add_constraint) + { + auto linear_vel_constraint = fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared( + source, *velocity_linear, linear_vel_mean_partial, linear_vel_covariance_partial, linear_indices); linear_vel_constraint->loss(linear_velocity_loss); @@ -1068,10 +975,10 @@ inline bool processTwistWithCovariance( } } - if (!angular_indices.empty()) { + if (!angular_indices.empty()) + { // Create the twist variables - auto velocity_angular = - fuse_variables::VelocityAngular2DStamped::make_shared(twist.header.stamp, device_id); + auto velocity_angular = fuse_variables::VelocityAngular2DStamped::make_shared(twist.header.stamp, device_id); velocity_angular->yaw() = transformed_message.twist.twist.angular.z; fuse_core::VectorXd angular_vel_vector(1); @@ -1082,22 +989,25 @@ inline bool processTwistWithCovariance( bool add_constraint = true; - if (validate) { - try { + if (validate) + { + try + { validatePartialMeasurement(angular_vel_vector, angular_vel_covariance); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, - "Invalid partial angular velocity measurement from '" - << source << "' source: " << ex.what()); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + "Invalid partial angular velocity measurement from '" + << source << "' source: " << ex.what()); add_constraint = false; } } - if (add_constraint) { - auto angular_vel_constraint = - fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint::make_shared( - source, *velocity_angular, angular_vel_vector, angular_vel_covariance, angular_indices); + if (add_constraint) + { + auto angular_vel_constraint = fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint::make_shared( + source, *velocity_angular, angular_vel_vector, angular_vel_covariance, angular_indices); angular_vel_constraint->loss(angular_velocity_loss); @@ -1107,7 +1017,8 @@ inline bool processTwistWithCovariance( } } - if (constraints_added) { + if (constraints_added) + { transaction.addInvolvedStamp(twist.header.stamp); } @@ -1136,42 +1047,41 @@ inline bool processTwistWithCovariance( * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processAccelWithCovariance( - const std::string & source, - const fuse_core::UUID & device_id, - const geometry_msgs::msg::AccelWithCovarianceStamped & acceleration, - const fuse_core::Loss::SharedPtr & loss, - const std::string & target_frame, - const std::vector & indices, - const tf2_ros::Buffer & tf_buffer, - const bool validate, - fuse_core::Transaction & transaction, - const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +inline bool processAccelWithCovariance(const std::string& source, const fuse_core::UUID& device_id, + const geometry_msgs::msg::AccelWithCovarianceStamped& acceleration, + const fuse_core::Loss::SharedPtr& loss, const std::string& target_frame, + const std::vector& indices, const tf2_ros::Buffer& tf_buffer, + const bool validate, fuse_core::Transaction& transaction, + const rclcpp::Duration& tf_timeout = rclcpp::Duration(0, 0)) { // Make sure we actually have work to do - if (indices.empty()) { + if (indices.empty()) + { return false; } geometry_msgs::msg::AccelWithCovarianceStamped transformed_message; - if (target_frame.empty()) { + if (target_frame.empty()) + { transformed_message = acceleration; - } else { + } + else + { transformed_message.header.frame_id = target_frame; - if (!transformMessage(tf_buffer, acceleration, transformed_message, tf_timeout)) { - RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, - "Failed to transform acceleration message with stamp " << - rclcpp::Time(acceleration.header.stamp).nanoseconds() - << ". Cannot create constraint."); + if (!transformMessage(tf_buffer, acceleration, transformed_message, tf_timeout)) + { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + "Failed to transform acceleration message with stamp " + << rclcpp::Time(acceleration.header.stamp).nanoseconds() + << ". Cannot create constraint."); return false; } } // Create the acceleration variables auto acceleration_linear = - fuse_variables::AccelerationLinear2DStamped::make_shared(acceleration.header.stamp, device_id); + fuse_variables::AccelerationLinear2DStamped::make_shared(acceleration.header.stamp, device_id); acceleration_linear->x() = transformed_message.accel.accel.linear.x; acceleration_linear->y() = transformed_message.accel.accel.linear.y; @@ -1180,41 +1090,33 @@ inline bool processAccelWithCovariance( accel_mean << transformed_message.accel.accel.linear.x, transformed_message.accel.accel.linear.y; fuse_core::MatrixXd accel_covariance(2, 2); - accel_covariance << - transformed_message.accel.covariance[0], - transformed_message.accel.covariance[1], - transformed_message.accel.covariance[6], - transformed_message.accel.covariance[7]; + accel_covariance << transformed_message.accel.covariance[0], transformed_message.accel.covariance[1], + transformed_message.accel.covariance[6], transformed_message.accel.covariance[7]; // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd accel_mean_partial(indices.size()); - fuse_core::MatrixXd accel_covariance_partial(accel_mean_partial.rows(), - accel_mean_partial.rows()); + fuse_core::MatrixXd accel_covariance_partial(accel_mean_partial.rows(), accel_mean_partial.rows()); - populatePartialMeasurement( - accel_mean, accel_covariance, indices, accel_mean_partial, - accel_covariance_partial); + populatePartialMeasurement(accel_mean, accel_covariance, indices, accel_mean_partial, accel_covariance_partial); - if (validate) { - try { + if (validate) + { + try + { validatePartialMeasurement(accel_mean_partial, accel_covariance_partial); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, - "Invalid partial linear acceleration measurement from '" - << source << "' source: " << ex.what()); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial linear acceleration measurement from '" + << source << "' source: " << ex.what()); return false; } } // Create the constraint - auto linear_accel_constraint = - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( - source, - *acceleration_linear, - accel_mean_partial, - accel_covariance_partial, - indices); + auto linear_accel_constraint = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( + source, *acceleration_linear, accel_mean_partial, accel_covariance_partial, indices); linear_accel_constraint->loss(loss); @@ -1235,10 +1137,9 @@ inline bool processAccelWithCovariance( * @param[in] velocity_yaw - The yaw velocity * @param[in] velocity_norm_min - The minimum velocity norm */ -inline void scaleProcessNoiseCovariance( - fuse_core::Matrix8d & process_noise_covariance, - const tf2_2d::Vector2 & velocity_linear, const double velocity_yaw, - const double velocity_norm_min) +inline void scaleProcessNoiseCovariance(fuse_core::Matrix8d& process_noise_covariance, + const tf2_2d::Vector2& velocity_linear, const double velocity_yaw, + const double velocity_norm_min) { // A more principled approach would be to get the current velocity from the state, make a diagonal // matrix from it, and then rotate it to be in the world frame (i.e., the same frame as the pose @@ -1258,12 +1159,10 @@ inline void scaleProcessNoiseCovariance( fuse_core::Matrix3d velocity; velocity.setIdentity(); velocity.diagonal() *= - std::max( - velocity_norm_min, - fuse_core::Vector3d(velocity_linear.x(), velocity_linear.y(), velocity_yaw).norm()); + std::max(velocity_norm_min, fuse_core::Vector3d(velocity_linear.x(), velocity_linear.y(), velocity_yaw).norm()); process_noise_covariance.topLeftCorner<3, 3>() = - velocity * process_noise_covariance.topLeftCorner<3, 3>() * velocity.transpose(); + velocity * process_noise_covariance.topLeftCorner<3, 3>() * velocity.transpose(); } } // namespace common diff --git a/fuse_models/include/fuse_models/common/variable_traits.hpp b/fuse_models/include/fuse_models/common/variable_traits.hpp index b5a77de98..10006456f 100644 --- a/fuse_models/include/fuse_models/common/variable_traits.hpp +++ b/fuse_models/include/fuse_models/common/variable_traits.hpp @@ -40,50 +40,49 @@ #include #include - namespace fuse_models { namespace common { -template +template struct is_linear_2d { static const bool value = false; }; -template<> +template <> struct is_linear_2d { static const bool value = true; }; -template<> +template <> struct is_linear_2d { static const bool value = true; }; -template<> +template <> struct is_linear_2d { static const bool value = true; }; -template +template struct is_angular_2d { static const bool value = false; }; -template<> +template <> struct is_angular_2d { static const bool value = true; }; -template<> +template <> struct is_angular_2d { static const bool value = true; diff --git a/fuse_models/include/fuse_models/graph_ignition.hpp b/fuse_models/include/fuse_models/graph_ignition.hpp index 0f27da202..6d105ba99 100644 --- a/fuse_models/include/fuse_models/graph_ignition.hpp +++ b/fuse_models/include/fuse_models/graph_ignition.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -96,10 +95,8 @@ class GraphIgnition : public fuse_core::AsyncSensorModel /** * @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; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Subscribe to the input topic to start sending transactions to the optimizer @@ -127,15 +124,14 @@ class GraphIgnition : public fuse_core::AsyncSensorModel /** * @brief Triggers the publication of a new transaction equivalent to the supplied graph */ - void subscriberCallback(const fuse_msgs::msg::SerializedGraph & msg); + void subscriberCallback(const fuse_msgs::msg::SerializedGraph& msg); /** * @brief Triggers the publication of a new transaction equivalent to the supplied graph */ - bool setGraphServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetGraph::Request::SharedPtr req); + bool setGraphServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetGraph::Request::SharedPtr req); /** * @brief Perform any required initialization for the kinematic ignition sensor @@ -150,8 +146,7 @@ class GraphIgnition : public fuse_core::AsyncSensorModel * * @param[in] msg - The graph message */ - void process( - const fuse_msgs::msg::SerializedGraph & msg, std::function post_process = nullptr); + void process(const fuse_msgs::msg::SerializedGraph& msg, std::function post_process = nullptr); /** * @brief Create and send a transaction equivalent to the supplied graph @@ -159,20 +154,16 @@ class GraphIgnition : public fuse_core::AsyncSensorModel * @param[in] graph - The graph * @param[in] stamp - The graph stamp */ - void sendGraph(const fuse_core::Graph & graph, const rclcpp::Time & stamp); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Graph, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Services, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void sendGraph(const fuse_core::Graph& graph, const rclcpp::Time& stamp); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ std::atomic_bool started_; //!< Flag indicating the sensor has been started - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; //!< Object containing all of the configuration parameters diff --git a/fuse_models/include/fuse_models/imu_2d.hpp b/fuse_models/include/fuse_models/imu_2d.hpp index 82db3b734..c95a36349 100644 --- a/fuse_models/include/fuse_models/imu_2d.hpp +++ b/fuse_models/include/fuse_models/imu_2d.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -115,16 +114,14 @@ class Imu2D : public fuse_core::AsyncSensorModel /** * @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; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The IMU message to process */ - void process(const sensor_msgs::msg::Imu & msg); + void process(const sensor_msgs::msg::Imu& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -158,22 +155,18 @@ class Imu2D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @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, - fuse_core::Transaction & transaction); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction); + + 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 + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; diff --git a/fuse_models/include/fuse_models/odometry_2d.hpp b/fuse_models/include/fuse_models/odometry_2d.hpp index f921bcec9..8edbc5d20 100644 --- a/fuse_models/include/fuse_models/odometry_2d.hpp +++ b/fuse_models/include/fuse_models/odometry_2d.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -106,16 +105,14 @@ class Odometry2D : public fuse_core::AsyncSensorModel /** * @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; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The pose message to process */ - void process(const nav_msgs::msg::Odometry & msg); + void process(const nav_msgs::msg::Odometry& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -149,22 +146,18 @@ class Odometry2D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @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, - fuse_core::Transaction & transaction); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction); + + 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 + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; diff --git a/fuse_models/include/fuse_models/odometry_2d_publisher.hpp b/fuse_models/include/fuse_models/odometry_2d_publisher.hpp index dc51b0713..48e128bd0 100644 --- a/fuse_models/include/fuse_models/odometry_2d_publisher.hpp +++ b/fuse_models/include/fuse_models/odometry_2d_publisher.hpp @@ -56,7 +56,6 @@ #include #include - namespace fuse_models { @@ -123,9 +122,8 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher /** * @brief Shadowing extension to the AsyncPublisher::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; protected: /** @@ -142,9 +140,8 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) override; + void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; /** * @brief Perform any required operations before the first call to notify() occurs @@ -176,17 +173,11 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher * structure * @return true if the checks pass, false otherwise */ - bool getState( - const fuse_core::Graph & graph, - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id, - fuse_core::UUID & position_uuid, - fuse_core::UUID & orientation_uuid, - fuse_core::UUID & velocity_linear_uuid, - fuse_core::UUID & velocity_angular_uuid, - fuse_core::UUID & acceleration_linear_uuid, - nav_msgs::msg::Odometry & odometry, - geometry_msgs::msg::AccelWithCovarianceStamped & acceleration); + bool getState(const fuse_core::Graph& graph, const rclcpp::Time& stamp, const fuse_core::UUID& device_id, + fuse_core::UUID& position_uuid, fuse_core::UUID& orientation_uuid, + fuse_core::UUID& velocity_linear_uuid, fuse_core::UUID& velocity_angular_uuid, + fuse_core::UUID& acceleration_linear_uuid, nav_msgs::msg::Odometry& odometry, + geometry_msgs::msg::AccelWithCovarianceStamped& acceleration); /** * @brief Timer callback method for the filtered state publication and tf broadcasting @@ -198,31 +189,24 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher * @brief Object that searches for the most recent common timestamp for a set of variables */ using Synchronizer = fuse_publishers::StampedVariableSynchronizer< - fuse_variables::Orientation2DStamped, - fuse_variables::Position2DStamped, - fuse_variables::VelocityLinear2DStamped, - fuse_variables::VelocityAngular2DStamped, - fuse_variables::AccelerationLinear2DStamped>; - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Timers, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncPublisher interfaces_ - - fuse_core::UUID device_id_; //!< The UUID of this device + fuse_variables::Orientation2DStamped, fuse_variables::Position2DStamped, fuse_variables::VelocityLinear2DStamped, + fuse_variables::VelocityAngular2DStamped, fuse_variables::AccelerationLinear2DStamped>; + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncPublisher interfaces_ + + fuse_core::UUID device_id_; //!< The UUID of this device rclcpp::Clock::SharedPtr clock_; //!< The publisher's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The publisher's logger + rclcpp::Logger logger_; //!< The publisher's logger ParameterType params_; rclcpp::Time latest_stamp_; rclcpp::Time latest_covariance_stamp_; - bool latest_covariance_valid_{false}; //!< Whether the latest covariance computed is valid or not + bool latest_covariance_valid_{ false }; //!< Whether the latest covariance computed is valid or not nav_msgs::msg::Odometry odom_output_; geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output_; @@ -237,10 +221,10 @@ class Odometry2DPublisher : public fuse_core::AsyncPublisher std::shared_ptr tf_broadcaster_ = nullptr; std::unique_ptr tf_listener_; - fuse_core::DelayedThrottleFilter delayed_throttle_filter_{10.0}; //!< A ros::console filter to - //!< print delayed throttle - //!< messages, that can be reset - //!< on start + fuse_core::DelayedThrottleFilter delayed_throttle_filter_{ 10.0 }; //!< A ros::console filter to + //!< print delayed throttle + //!< messages, that can be reset + //!< on start rclcpp::TimerBase::SharedPtr publish_timer_; diff --git a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h index c47431f98..f427cd491 100644 --- a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h +++ b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__PARAMETERS__ACCELERATION_2D_PARAMS_H_ #define FUSE_MODELS__PARAMETERS__ACCELERATION_2D_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_models/parameters/acceleration_2d_params.hpp instead +#warning This header is obsolete, please include fuse_models/parameters/acceleration_2d_params.hpp instead #include diff --git a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp index 3b5fc1ead..fdf6b4f64 100644 --- a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.hpp @@ -42,7 +42,6 @@ #include #include - namespace fuse_models { @@ -62,65 +61,37 @@ struct Acceleration2DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "dimensions")); + interfaces, fuse_core::joinParameterName(ns, "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::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::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); - fuse_core::getParamRequired( - interfaces, fuse_core::joinParameterName( - ns, - "target_frame"), - target_frame); + fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame); loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "loss")); } - bool disable_checks {false}; - int queue_size {10}; - 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::string topic {}; - std::string target_frame {}; + bool disable_checks{ false }; + int queue_size{ 10 }; + 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::string topic{}; + std::string target_frame{}; std::vector indices; fuse_core::Loss::SharedPtr loss; }; diff --git a/fuse_models/include/fuse_models/parameters/graph_ignition_params.h b/fuse_models/include/fuse_models/parameters/graph_ignition_params.h index ee24e5ec9..7739b4fd8 100644 --- a/fuse_models/include/fuse_models/parameters/graph_ignition_params.h +++ b/fuse_models/include/fuse_models/parameters/graph_ignition_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__PARAMETERS__GRAPH_IGNITION_PARAMS_H_ #define FUSE_MODELS__PARAMETERS__GRAPH_IGNITION_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_models/parameters/graph_ignition_params.hpp instead +#warning This header is obsolete, please include fuse_models/parameters/graph_ignition_params.hpp instead #include diff --git a/fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp index 1452a5b58..b22826135 100644 --- a/fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/graph_ignition_params.hpp @@ -39,7 +39,6 @@ #include - namespace fuse_models { @@ -59,51 +58,37 @@ struct GraphIgnitionParams : public fuse_models::parameters::ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - reset_service = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "reset_service"), - reset_service); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + reset_service = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "reset_service"), reset_service); set_graph_service = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "set_graph_service"), - set_graph_service); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "set_graph_service"), set_graph_service); topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); } /** * @brief The size of the subscriber queue for the topic */ - int queue_size{10}; + int queue_size{ 10 }; /** * @brief The name of the reset service to call before sending transactions to the optimizer */ - std::string reset_service{"~/reset"}; + std::string reset_service{ "~/reset" }; /** * @brief The name of the set_graph service to advertise */ - std::string set_graph_service{"set_graph"}; + std::string set_graph_service{ "set_graph" }; /** * @brief The topic name for received SerializedGraph messages */ - std::string topic{"graph"}; + std::string topic{ "graph" }; }; } // namespace parameters diff --git a/fuse_models/include/fuse_models/parameters/imu_2d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_2d_params.hpp index 6abe06384..f8cf24fe8 100644 --- a/fuse_models/include/fuse_models/parameters/imu_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_2d_params.hpp @@ -45,7 +45,6 @@ #include #include - namespace fuse_models { @@ -65,140 +64,81 @@ struct Imu2DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - angular_velocity_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_dimensions")); - linear_acceleration_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_acceleration_dimensions")); + angular_velocity_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "angular_velocity_dimensions")); + linear_acceleration_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "linear_acceleration_dimensions")); orientation_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_dimensions")); - - differential = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "differential"), - differential); + interfaces, fuse_core::joinParameterName(ns, "orientation_dimensions")); + + differential = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "differential"), differential); 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::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, "tf_timeout", tf_timeout, false); fuse_core::getPositiveParam(interfaces, "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); - - remove_gravitational_acceleration = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, "remove_gravitational_acceleration"), remove_gravitational_acceleration); - gravitational_acceleration = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "gravitational_acceleration"), - gravitational_acceleration); + throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), + throttle_use_wall_time); + + remove_gravitational_acceleration = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "remove_gravitational_acceleration"), + remove_gravitational_acceleration); + gravitational_acceleration = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "gravitational_acceleration"), gravitational_acceleration); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); - if (differential) { - independent = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "independent"), - independent); - use_twist_covariance = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "use_twist_covariance"), - use_twist_covariance); - - minimum_pose_relative_covariance = - fuse_core::getCovarianceDiagonalParam<3>( - interfaces, - fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); - twist_covariance_offset = - fuse_core::getCovarianceDiagonalParam<3>( - interfaces, - fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + if (differential) + { + independent = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "independent"), independent); + use_twist_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_twist_covariance"), + use_twist_covariance); + + minimum_pose_relative_covariance = fuse_core::getCovarianceDiagonalParam<3>( + interfaces, fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = fuse_core::getCovarianceDiagonalParam<3>( + interfaces, fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); } - acceleration_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "acceleration_target_frame"), - acceleration_target_frame); - orientation_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_target_frame"), - orientation_target_frame); + acceleration_target_frame = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "acceleration_target_frame"), acceleration_target_frame); + orientation_target_frame = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "orientation_target_frame"), orientation_target_frame); twist_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "twist_target_frame"), - twist_target_frame); - - pose_loss = - fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "twist_target_frame"), twist_target_frame); + + pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); angular_velocity_loss = - fuse_core::loadLossConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "angular_velocity_loss")); linear_acceleration_loss = - fuse_core::loadLossConfig( - interfaces, - fuse_core::joinParameterName(ns, "linear_acceleration_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "linear_acceleration_loss")); } - bool differential {false}; - bool disable_checks {false}; - bool independent {true}; - bool use_twist_covariance {true}; + bool differential{ false }; + bool disable_checks{ false }; + bool independent{ true }; + bool use_twist_covariance{ true }; fuse_core::Matrix3d minimum_pose_relative_covariance; //!< Minimum pose relative covariance //!< matrix - fuse_core::Matrix3d twist_covariance_offset; //!< Offset already added to the twist covariance - //!< matrix, that will be substracted in order to - //!< recover the raw values - bool remove_gravitational_acceleration {false}; - int queue_size {10}; - 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 - double gravitational_acceleration {9.80665}; - std::string acceleration_target_frame {}; - std::string orientation_target_frame {}; - std::string topic {}; - std::string twist_target_frame {}; + fuse_core::Matrix3d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be subtracted in order to + //!< recover the raw values + bool remove_gravitational_acceleration{ false }; + int queue_size{ 10 }; + 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 + double gravitational_acceleration{ 9.80665 }; + std::string acceleration_target_frame{}; + std::string orientation_target_frame{}; + std::string topic{}; + std::string twist_target_frame{}; std::vector angular_velocity_indices; std::vector linear_acceleration_indices; std::vector orientation_indices; diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_params.h b/fuse_models/include/fuse_models/parameters/odometry_2d_params.h index 8103cad0b..1925cc5f4 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_params.h +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PARAMS_H_ #define FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_models/parameters/odometry_2d_params.hpp instead +#warning This header is obsolete, please include fuse_models/parameters/odometry_2d_params.hpp instead #include diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_2d_params.hpp index 8adf9e32a..f178d8bda 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_params.hpp @@ -46,7 +46,6 @@ #include #include - namespace fuse_models { @@ -66,135 +65,73 @@ struct Odometry2DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { position_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "position_dimensions")); + interfaces, fuse_core::joinParameterName(ns, "position_dimensions")); orientation_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_dimensions")); - linear_velocity_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_velocity_dimensions")); - angular_velocity_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_dimensions")); - - differential = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "differential"), - differential); + interfaces, fuse_core::joinParameterName(ns, "orientation_dimensions")); + linear_velocity_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "linear_velocity_dimensions")); + angular_velocity_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "angular_velocity_dimensions")); + + differential = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "differential"), differential); 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::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); twist_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "twist_target_frame"), - twist_target_frame); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "twist_target_frame"), twist_target_frame); pose_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "pose_target_frame"), - pose_target_frame); - - if (differential) { - independent = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "independent"), - independent); - use_twist_covariance = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "use_twist_covariance"), - use_twist_covariance); - - minimum_pose_relative_covariance = - fuse_core::getCovarianceDiagonalParam<3>( - interfaces, - fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); - twist_covariance_offset = - fuse_core::getCovarianceDiagonalParam<3>( - interfaces, - fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_target_frame"), pose_target_frame); + + if (differential) + { + independent = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "independent"), independent); + use_twist_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_twist_covariance"), + use_twist_covariance); + + minimum_pose_relative_covariance = fuse_core::getCovarianceDiagonalParam<3>( + interfaces, fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = fuse_core::getCovarianceDiagonalParam<3>( + interfaces, fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); } - pose_loss = - fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); linear_velocity_loss = - fuse_core::loadLossConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_velocity_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "linear_velocity_loss")); angular_velocity_loss = - fuse_core::loadLossConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "angular_velocity_loss")); } - bool differential {false}; - bool disable_checks {false}; - bool independent {true}; - bool use_twist_covariance {true}; + bool differential{ false }; + bool disable_checks{ false }; + bool independent{ true }; + bool use_twist_covariance{ true }; fuse_core::Matrix3d minimum_pose_relative_covariance; //!< Minimum pose relative covariance //!< matrix - fuse_core::Matrix3d twist_covariance_offset; //!< Offset already added to the twist covariance - //!< matrix, that will be substracted in order to - //!< recover the raw values - int queue_size {10}; - 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::string topic {}; - std::string pose_target_frame {}; - std::string twist_target_frame {}; + fuse_core::Matrix3d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be subtracted in order to + //!< recover the raw values + int queue_size{ 10 }; + 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::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; diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h index 080693470..7bec22d88 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PUBLISHER_PARAMS_H_ #define FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PUBLISHER_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_models/parameters/odometry_2d_publisher_params.hpp \ +#warning This header is obsolete, please include fuse_models/parameters/odometry_2d_publisher_params.hpp \ instead #include diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.hpp index 488a38aaa..c65f9c361 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.hpp @@ -49,7 +49,6 @@ #include - namespace fuse_models { @@ -71,168 +70,93 @@ struct Odometry2DPublisherParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - publish_tf = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "publish_tf"), - publish_tf); - invert_tf = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "invert_tf"), - invert_tf); - predict_to_current_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "predict_to_current_time"), - predict_to_current_time); - predict_with_acceleration = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "predict_with_acceleration"), - predict_with_acceleration); + publish_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_tf"), publish_tf); + invert_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "invert_tf"), invert_tf); + predict_to_current_time = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "predict_to_current_time"), predict_to_current_time); + predict_with_acceleration = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "predict_with_acceleration"), predict_with_acceleration); publish_frequency = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "publish_frequency"), - publish_frequency); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_frequency"), publish_frequency); process_noise_covariance = fuse_core::getCovarianceDiagonalParam<8>( - interfaces, fuse_core::joinParameterName( - ns, - "process_noise_diagonal"), 0.0); + interfaces, fuse_core::joinParameterName(ns, "process_noise_diagonal"), 0.0); scale_process_noise = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "scale_process_noise"), - scale_process_noise); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "scale_process_noise"), scale_process_noise); velocity_norm_min = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "velocity_norm_min"), - velocity_norm_min); - - fuse_core::getPositiveParam( - interfaces, - fuse_core::joinParameterName( - ns, - "covariance_throttle_period"), covariance_throttle_period, - false); - - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_cache_time"), tf_cache_time, - false); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_timeout"), tf_timeout, - false); - - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - - map_frame_id = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "map_frame_id"), - map_frame_id); - odom_frame_id = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "odom_frame_id"), - odom_frame_id); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "velocity_norm_min"), velocity_norm_min); + + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "covariance_throttle_period"), + covariance_throttle_period, false); + + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_cache_time"), tf_cache_time, false); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); + + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + + map_frame_id = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "map_frame_id"), map_frame_id); + odom_frame_id = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "odom_frame_id"), odom_frame_id); base_link_frame_id = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "base_link_frame_id"), - base_link_frame_id); - base_link_output_frame_id = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "base_link_output_frame_id"), - base_link_output_frame_id); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "base_link_frame_id"), base_link_frame_id); + base_link_output_frame_id = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "base_link_output_frame_id"), base_link_output_frame_id); world_frame_id = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "world_frame_id"), - world_frame_id); - - const bool frames_valid = - map_frame_id != odom_frame_id && - map_frame_id != base_link_frame_id && - map_frame_id != base_link_output_frame_id && - odom_frame_id != base_link_frame_id && - odom_frame_id != base_link_output_frame_id && - (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); - - if (!frames_valid) { - RCLCPP_FATAL_STREAM( - interfaces.get_node_logging_interface()->get_logger(), - "Invalid frame configuration! Please note:\n" - << " - The values for map_frame_id, odom_frame_id, and base_link_frame_id must be " - << "unique\n" - << " - The values for map_frame_id, odom_frame_id, and base_link_output_frame_id must be " - << "unique\n" - << " - The world_frame_id must be the same as the map_frame_id or odom_frame_id\n"); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "world_frame_id"), world_frame_id); + + const bool frames_valid = map_frame_id != odom_frame_id && map_frame_id != base_link_frame_id && + map_frame_id != base_link_output_frame_id && odom_frame_id != base_link_frame_id && + odom_frame_id != base_link_output_frame_id && + (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); + + if (!frames_valid) + { + RCLCPP_FATAL_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "Invalid frame configuration! Please note:\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_frame_id must be " + << "unique\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_output_frame_id must be " + << "unique\n" + << " - The world_frame_id must be the same as the map_frame_id or odom_frame_id\n"); assert(frames_valid); } topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); acceleration_topic = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "acceleration_topic"), - acceleration_topic); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "acceleration_topic"), acceleration_topic); fuse_core::loadCovarianceOptionsFromROS(interfaces, covariance_options, "covariance_options"); } - bool publish_tf {true}; //!< Whether to publish/broadcast the TF transform or not - bool invert_tf{false}; //!< Whether to broadcast the inverse of the TF transform or not. When - //!< the inverse is broadcasted, the transform is inverted and the - //!< header.frame_id and child_frame_id are swapped, i.e. the odometry - //!< output header.frame_id is set to the base_link_output_frame_id and - //!< the child_frame_id to the world_frame_id - bool predict_to_current_time {false}; - bool predict_with_acceleration {false}; - double publish_frequency {10.0}; - fuse_core::Matrix8d process_noise_covariance; //!< Process noise covariance matrix - bool scale_process_noise{false}; - double velocity_norm_min{1e-3}; - rclcpp::Duration covariance_throttle_period {0, 0}; //!< The throttle period duration in seconds - //!< to compute the covariance - rclcpp::Duration tf_cache_time {10, 0}; - rclcpp::Duration tf_timeout {0, static_cast(RCUTILS_S_TO_NS(0.1))}; - int queue_size {1}; - std::string map_frame_id {"map"}; - std::string odom_frame_id {"odom"}; - std::string base_link_frame_id {"base_link"}; - std::string base_link_output_frame_id {base_link_frame_id}; - std::string world_frame_id {odom_frame_id}; - std::string topic {"odometry/filtered"}; - std::string acceleration_topic {"acceleration/filtered"}; + bool publish_tf{ true }; //!< Whether to publish/broadcast the TF transform or not + bool invert_tf{ false }; //!< Whether to broadcast the inverse of the TF transform or not. When + //!< the inverse is broadcasted, the transform is inverted and the + //!< header.frame_id and child_frame_id are swapped, i.e. the odometry + //!< output header.frame_id is set to the base_link_output_frame_id and + //!< the child_frame_id to the world_frame_id + bool predict_to_current_time{ false }; + bool predict_with_acceleration{ false }; + double publish_frequency{ 10.0 }; + fuse_core::Matrix8d process_noise_covariance; //!< Process noise covariance matrix + bool scale_process_noise{ false }; + double velocity_norm_min{ 1e-3 }; + rclcpp::Duration covariance_throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + //!< to compute the covariance + rclcpp::Duration tf_cache_time{ 10, 0 }; + rclcpp::Duration tf_timeout{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; + int queue_size{ 1 }; + std::string map_frame_id{ "map" }; + std::string odom_frame_id{ "odom" }; + std::string base_link_frame_id{ "base_link" }; + std::string base_link_output_frame_id{ base_link_frame_id }; + std::string world_frame_id{ odom_frame_id }; + std::string topic{ "odometry/filtered" }; + std::string acceleration_topic{ "acceleration/filtered" }; ceres::Covariance::Options covariance_options; }; diff --git a/fuse_models/include/fuse_models/parameters/parameter_base.hpp b/fuse_models/include/fuse_models/parameters/parameter_base.hpp index def42a29e..0508d9041 100644 --- a/fuse_models/include/fuse_models/parameters/parameter_base.hpp +++ b/fuse_models/include/fuse_models/parameters/parameter_base.hpp @@ -41,7 +41,6 @@ #include #include - namespace fuse_models { @@ -60,12 +59,10 @@ struct ParameterBase * @param[in] ns - The parameter namespace to use */ virtual void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) = 0; + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) = 0; }; /** @@ -77,14 +74,15 @@ struct ParameterBase * @param[in] name - The ROS parameter name for the sensor configuration parameter * @return A vector with the dimension indices, that would be empty if the parameter does not exist */ -template -inline std::vector loadSensorConfig( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) +template +inline std::vector +loadSensorConfig(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) { std::vector dimensions; dimensions = fuse_core::getParam(interfaces, name, dimensions); - if (!dimensions.empty()) { + if (!dimensions.empty()) + { return common::getDimensionIndices(dimensions); } diff --git a/fuse_models/include/fuse_models/parameters/pose_2d_params.hpp b/fuse_models/include/fuse_models/parameters/pose_2d_params.hpp index d68b36bc3..82bbe0cc0 100644 --- a/fuse_models/include/fuse_models/parameters/pose_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/pose_2d_params.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_models { @@ -64,93 +63,55 @@ struct Pose2DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { position_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "position_dimensions")); + interfaces, fuse_core::joinParameterName(ns, "position_dimensions")); orientation_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_dimensions")); - - differential = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "differential"), - differential); + interfaces, fuse_core::joinParameterName(ns, "orientation_dimensions")); + + differential = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "differential"), differential); 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::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); - - if (differential) { - independent = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "independent"), - independent); - - if (!independent) { - minimum_pose_relative_covariance = - fuse_core::getCovarianceDiagonalParam<3>( - interfaces, - fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + target_frame = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame); + + if (differential) + { + independent = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "independent"), independent); + + if (!independent) + { + minimum_pose_relative_covariance = fuse_core::getCovarianceDiagonalParam<3>( + interfaces, fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); } } loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "loss")); } - bool differential {false}; - bool disable_checks {false}; - bool independent {true}; + bool differential{ false }; + bool disable_checks{ false }; + bool independent{ true }; fuse_core::Matrix3d minimum_pose_relative_covariance; //!< Minimum pose relative covariance //!< matrix - int queue_size {10}; - 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::string topic {}; - std::string target_frame {}; + int queue_size{ 10 }; + 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::string topic{}; + std::string target_frame{}; std::vector position_indices; std::vector orientation_indices; fuse_core::Loss::SharedPtr loss; diff --git a/fuse_models/include/fuse_models/parameters/transaction_params.h b/fuse_models/include/fuse_models/parameters/transaction_params.h index 9ed5a7583..014e45d5b 100644 --- a/fuse_models/include/fuse_models/parameters/transaction_params.h +++ b/fuse_models/include/fuse_models/parameters/transaction_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__PARAMETERS__TRANSACTION_PARAMS_H_ #define FUSE_MODELS__PARAMETERS__TRANSACTION_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_models/parameters/transaction_params.hpp instead +#warning This header is obsolete, please include fuse_models/parameters/transaction_params.hpp instead #include diff --git a/fuse_models/include/fuse_models/parameters/transaction_params.hpp b/fuse_models/include/fuse_models/parameters/transaction_params.hpp index b025bcffc..95669e555 100644 --- a/fuse_models/include/fuse_models/parameters/transaction_params.hpp +++ b/fuse_models/include/fuse_models/parameters/transaction_params.hpp @@ -41,7 +41,6 @@ #include - namespace fuse_models { @@ -61,22 +60,16 @@ struct TransactionParams : public fuse_models::parameters::ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); } - int queue_size{10}; + int queue_size{ 10 }; std::string topic{}; }; diff --git a/fuse_models/include/fuse_models/parameters/twist_2d_params.hpp b/fuse_models/include/fuse_models/parameters/twist_2d_params.hpp index dc0129dd1..62061d1b7 100644 --- a/fuse_models/include/fuse_models/parameters/twist_2d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/twist_2d_params.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_models { @@ -64,72 +63,40 @@ struct Twist2DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { linear_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_dimensions")); + interfaces, fuse_core::joinParameterName(ns, "linear_dimensions")); angular_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_dimensions")); + interfaces, fuse_core::joinParameterName(ns, "angular_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::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::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); - fuse_core::getParamRequired( - interfaces, fuse_core::joinParameterName( - ns, - "target_frame"), - target_frame); + fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "target_frame"), target_frame); - linear_loss = - fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "linear_loss")); - angular_loss = - fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "angular_loss")); + linear_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "linear_loss")); + angular_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "angular_loss")); } - bool disable_checks {false}; - int queue_size {10}; - 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::string topic {}; - std::string target_frame {}; + bool disable_checks{ false }; + int queue_size{ 10 }; + 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::string topic{}; + std::string target_frame{}; std::vector linear_indices; std::vector angular_indices; fuse_core::Loss::SharedPtr linear_loss; diff --git a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h index 0c4b4746f..8cba776a3 100644 --- a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h +++ b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__PARAMETERS__UNICYCLE_2D_IGNITION_PARAMS_H_ #define FUSE_MODELS__PARAMETERS__UNICYCLE_2D_IGNITION_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_models/parameters/unicycle_2d_ignition_params.hpp \ +#warning This header is obsolete, please include fuse_models/parameters/unicycle_2d_ignition_params.hpp \ instead #include diff --git a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp index 8a3ca5d4f..d55023858 100644 --- a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_models { @@ -64,88 +63,54 @@ struct Unicycle2DIgnitionParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { publish_on_startup = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "publish_on_startup"), - publish_on_startup); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - reset_service = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "reset_service"), - reset_service); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_on_startup"), publish_on_startup); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + reset_service = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "reset_service"), reset_service); set_pose_service = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "set_pose_service"), - set_pose_service); - set_pose_deprecated_service = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "set_pose_deprecated_service"), - set_pose_deprecated_service); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "set_pose_service"), set_pose_service); + set_pose_deprecated_service = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "set_pose_deprecated_service"), set_pose_deprecated_service); topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); std::vector sigma_vector; - sigma_vector = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "initial_sigma"), - sigma_vector); - if (!sigma_vector.empty()) { - if (sigma_vector.size() != 8) { - throw std::invalid_argument( - "The supplied initial_sigma parameter must be length 8, but " - "is actually length " + - std::to_string(sigma_vector.size())); + sigma_vector = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "initial_sigma"), sigma_vector); + if (!sigma_vector.empty()) + { + if (sigma_vector.size() != 8) + { + throw std::invalid_argument("The supplied initial_sigma parameter must be length 8, but " + "is actually length " + + std::to_string(sigma_vector.size())); } - auto is_sigma_valid = [](const double sigma) - { - return std::isfinite(sigma) && (sigma > 0); - }; - if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { - throw std::invalid_argument( - "The supplied initial_sigma parameter must contain valid floating point values. " - "NaN, Inf, and values <= 0 are not acceptable."); + auto is_sigma_valid = [](const double sigma) { return std::isfinite(sigma) && (sigma > 0); }; + if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) + { + throw std::invalid_argument("The supplied initial_sigma parameter must contain valid floating point values. " + "NaN, Inf, and values <= 0 are not acceptable."); } initial_sigma.swap(sigma_vector); } std::vector state_vector; - state_vector = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "initial_state"), - state_vector); - if (!state_vector.empty()) { - if (state_vector.size() != 8) { - throw std::invalid_argument( - "The supplied initial_state parameter must be length 8, but is actually length " + - std::to_string(state_vector.size())); + state_vector = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "initial_state"), state_vector); + if (!state_vector.empty()) + { + if (state_vector.size() != 8) + { + throw std::invalid_argument("The supplied initial_state parameter must be length 8, but is actually length " + + std::to_string(state_vector.size())); } - auto is_state_valid = [](const double state) - { - return std::isfinite(state); - }; - if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { - throw std::invalid_argument( - "The supplied initial_state parameter must contain valid floating point values. " - "NaN, Inf, etc are not acceptable."); + auto is_state_valid = [](const double state) { return std::isfinite(state); }; + if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) + { + throw std::invalid_argument("The supplied initial_state parameter must contain valid floating point values. " + "NaN, Inf, etc are not acceptable."); } initial_state.swap(state_vector); } @@ -153,37 +118,36 @@ struct Unicycle2DIgnitionParams : public ParameterBase loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "loss")); } - /** * @brief Flag indicating if an initial state transaction should be sent on startup, or only in * response to a set_pose service call or topic message. */ - bool publish_on_startup {true}; + bool publish_on_startup{ true }; /** * @brief The size of the subscriber queue for the set_pose topic */ - int queue_size {10}; + int queue_size{ 10 }; /** * @brief The name of the reset service to call before sending transactions to the optimizer */ - std::string reset_service {"~/reset"}; + std::string reset_service{ "~/reset" }; /** * @brief The name of the set_pose service to advertise */ - std::string set_pose_service {"set_pose"}; + std::string set_pose_service{ "set_pose" }; /** * @brief The name of the deprecated set_pose service without return codes */ - std::string set_pose_deprecated_service {"set_pose_deprecated"}; + std::string set_pose_deprecated_service{ "set_pose_deprecated" }; /** * @brief The topic name for received PoseWithCovarianceStamped messages */ - std::string topic {"set_pose"}; + std::string topic{ "set_pose" }; /** * @brief The uncertainty of the initial state value @@ -193,14 +157,13 @@ struct Unicycle2DIgnitionParams : public ParameterBase * The covariance matrix is created placing the squared standard deviations along the diagonal of * an 8x8 matrix. */ - std::vector initial_sigma {1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, - 1.0e-9}; + std::vector initial_sigma{ 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9 }; /** * @brief The initial value of the 8-dimension state vector (x, y, yaw, x_vel, y_vel, yaw_vel, * x_acc, y_acc) */ - std::vector initial_state {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::vector initial_state{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; /** * @brief Loss function diff --git a/fuse_models/include/fuse_models/pose_2d.hpp b/fuse_models/include/fuse_models/pose_2d.hpp index 51d361276..154102025 100644 --- a/fuse_models/include/fuse_models/pose_2d.hpp +++ b/fuse_models/include/fuse_models/pose_2d.hpp @@ -49,7 +49,6 @@ #include #include - namespace fuse_models { @@ -96,16 +95,14 @@ class Pose2D : public fuse_core::AsyncSensorModel /** * @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; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The pose message to process */ - void process(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); + void process(const geometry_msgs::msg::PoseWithCovarianceStamped& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -137,21 +134,17 @@ 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, - fuse_core::Transaction & transaction); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, const bool validate, + fuse_core::Transaction& transaction); + + 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 + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; @@ -163,8 +156,7 @@ class Pose2D : public fuse_core::AsyncSensorModel rclcpp::Subscription::SharedPtr sub_; - using PoseThrottledCallback = - fuse_core::ThrottledMessageCallback; + using PoseThrottledCallback = fuse_core::ThrottledMessageCallback; PoseThrottledCallback throttled_callback_; }; diff --git a/fuse_models/include/fuse_models/transaction.hpp b/fuse_models/include/fuse_models/transaction.hpp index febcf7ecf..ba2517069 100644 --- a/fuse_models/include/fuse_models/transaction.hpp +++ b/fuse_models/include/fuse_models/transaction.hpp @@ -45,7 +45,6 @@ #include #include - namespace fuse_models { @@ -83,10 +82,8 @@ class Transaction : public fuse_core::AsyncSensorModel /** * @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; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; protected: /** @@ -108,15 +105,12 @@ class Transaction : public fuse_core::AsyncSensorModel * @brief Callback for transaction messages * @param[in] msg - The transaction message to process */ - void process(const fuse_msgs::msg::SerializedTransaction & msg); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void process(const fuse_msgs::msg::SerializedTransaction& msg); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ ParameterType params_; //!< Object containing all of the configuration parameters diff --git a/fuse_models/include/fuse_models/twist_2d.hpp b/fuse_models/include/fuse_models/twist_2d.hpp index 5e8004d4f..de3494c59 100644 --- a/fuse_models/include/fuse_models/twist_2d.hpp +++ b/fuse_models/include/fuse_models/twist_2d.hpp @@ -49,7 +49,6 @@ #include #include - namespace fuse_models { @@ -91,16 +90,14 @@ class Twist2D : public fuse_core::AsyncSensorModel /** * @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; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for twist messages * @param[in] msg - The twist message to process */ - void process(const geometry_msgs::msg::TwistWithCovarianceStamped & msg); + void process(const geometry_msgs::msg::TwistWithCovarianceStamped& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -120,17 +117,14 @@ class Twist2D : public fuse_core::AsyncSensorModel */ void onStop() override; - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + 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 + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; @@ -140,8 +134,7 @@ class Twist2D : public fuse_core::AsyncSensorModel rclcpp::Subscription::SharedPtr sub_; - using TwistThrottledCallback = - fuse_core::ThrottledMessageCallback; + using TwistThrottledCallback = fuse_core::ThrottledMessageCallback; TwistThrottledCallback throttled_callback_; }; diff --git a/fuse_models/include/fuse_models/unicycle_2d.hpp b/fuse_models/include/fuse_models/unicycle_2d.hpp index 2a4766aa2..808d9865e 100644 --- a/fuse_models/include/fuse_models/unicycle_2d.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -92,11 +91,10 @@ class Unicycle2D : public fuse_core::AsyncMotionModel /** * @brief Shadowing extension to the AsyncMotionModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; - void print(std::ostream & stream = std::cout) const; + void print(std::ostream& stream = std::cout) const; protected: /** @@ -104,19 +102,19 @@ class Unicycle2D : public fuse_core::AsyncMotionModel */ struct StateHistoryElement { - fuse_core::UUID position_uuid; //!< The uuid of the associated position variable - fuse_core::UUID yaw_uuid; //!< The uuid of the associated orientation variable - fuse_core::UUID vel_linear_uuid; //!< The uuid of the associated linear velocity variable - fuse_core::UUID vel_yaw_uuid; //!< The uuid of the associated angular velocity variable - fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration - //!< variable + fuse_core::UUID position_uuid; //!< The uuid of the associated position variable + fuse_core::UUID yaw_uuid; //!< The uuid of the associated orientation variable + fuse_core::UUID vel_linear_uuid; //!< The uuid of the associated linear velocity variable + fuse_core::UUID vel_yaw_uuid; //!< The uuid of the associated angular velocity variable + fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration + //!< variable tf2_2d::Transform pose; //!< Map-frame pose tf2_2d::Vector2 velocity_linear; //!< Body-frame linear velocity - double velocity_yaw{0.0}; //!< Body-frame yaw velocity + double velocity_yaw{ 0.0 }; //!< Body-frame yaw velocity tf2_2d::Vector2 acceleration_linear; //!< Body-frame linear acceleration - void print(std::ostream & stream = std::cout) const; + void print(std::ostream& stream = std::cout) const; /** * @brief Validate the state components: pose, linear velocity, yaw velocity and linear @@ -138,7 +136,7 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * constraints * @return True if the motion models were generated successfully, false otherwise */ - bool applyCallback(fuse_core::Transaction & transaction) override; + bool applyCallback(fuse_core::Transaction& transaction) override; /** * @brief Generate a single motion model segment between the specified timestamps. @@ -158,11 +156,9 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * ending_stamp. The variables should include initial values for the * optimizer. */ - void generateMotionModel( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables); + void generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables); /** * @brief Callback fired in the local callback queue thread(s) whenever a new Graph is received @@ -189,10 +185,8 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * @param[in] state_history The state history object to be updated * @param[in] buffer_length States older than this in the history will be pruned */ - static void updateStateHistoryEstimates( - const fuse_core::Graph & graph, - StateHistory & state_history, - const rclcpp::Duration & buffer_length); + static void updateStateHistoryEstimates(const fuse_core::Graph& graph, StateHistory& state_history, + const rclcpp::Duration& buffer_length); /** * @brief Validate the motion model state #1, state #2 and process noise covariance @@ -205,39 +199,35 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * @param[in] process_noise_covariance The process noise covariance, after it is scaled and * multiplied by dt */ - static void validateMotionModel( - const StateHistoryElement & state1, const StateHistoryElement & state2, - const fuse_core::Matrix8d & process_noise_covariance); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + static void validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, + const fuse_core::Matrix8d& process_noise_covariance); + + 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 + rclcpp::Logger logger_; //!< The sensor model's logger rclcpp::Duration buffer_length_; //!< The length of the state history fuse_core::UUID device_id_; //!< The UUID of the device to be published fuse_core::TimestampManager timestamp_manager_; //!< Tracks timestamps and previously created //!< motion model segments fuse_core::Matrix8d process_noise_covariance_; //!< Process noise covariance matrix - bool scale_process_noise_{false}; //!< Whether to scale the process noise + bool scale_process_noise_{ false }; //!< Whether to scale the process noise //!< covariance pose by the norm of the current //!< state twist - double velocity_norm_min_{1e-3}; //!< The minimum velocity/twist norm allowed when + double velocity_norm_min_{ 1e-3 }; //!< The minimum velocity/twist norm allowed when //!< scaling the process noise covariance - bool disable_checks_{false}; //!< Whether to disable the validation checks for the current and - //!< predicted state, including the process noise covariance after - //!< it is scaled and multiplied by dt - StateHistory state_history_; //!< History of optimized graph pose estimates + bool disable_checks_{ false }; //!< Whether to disable the validation checks for the current and + //!< predicted state, including the process noise covariance after + //!< it is scaled and multiplied by dt + StateHistory state_history_; //!< History of optimized graph pose estimates }; -std::ostream & operator<<(std::ostream & stream, const Unicycle2D & unicycle_2d); +std::ostream& operator<<(std::ostream& stream, const Unicycle2D& unicycle_2d); } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/unicycle_2d_ignition.hpp b/fuse_models/include/fuse_models/unicycle_2d_ignition.hpp index 28b12bafe..832d25239 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_ignition.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_ignition.hpp @@ -49,7 +49,6 @@ #include #include - namespace fuse_models { @@ -108,10 +107,8 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel /** * @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; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Subscribe to the input topic to start sending transactions to the optimizer @@ -138,23 +135,20 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); + void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg); /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - bool setPoseServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr, - const fuse_msgs::srv::SetPose::Request::SharedPtr req); + bool setPoseServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr, const fuse_msgs::srv::SetPose::Request::SharedPtr req); /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - bool setPoseDeprecatedServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req); + bool setPoseDeprecatedServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req); protected: /** @@ -170,9 +164,7 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, yaw) */ - void process( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - std::function post_process = nullptr); + void process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, std::function post_process = nullptr); /** * @brief Create and send a prior transaction based on the supplied pose @@ -183,24 +175,20 @@ class Unicycle2DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, yaw) */ - void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Graph, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Services, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ - - std::atomic_bool started_; //!< Flag indicating the sensor has been started - bool initial_transaction_sent_; //!< Flag indicating an initial transaction has been sent already - fuse_core::UUID device_id_; //!< The UUID of this device + void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + std::atomic_bool started_; //!< Flag indicating the sensor has been started + bool initial_transaction_sent_; //!< Flag indicating an initial transaction has been sent already + fuse_core::UUID device_id_; //!< The UUID of this device rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; //!< Object containing all of the configuration parameters diff --git a/fuse_models/include/fuse_models/unicycle_2d_predict.hpp b/fuse_models/include/fuse_models/unicycle_2d_predict.hpp index 3d90b51cd..a7d4b1cdb 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_predict.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_predict.hpp @@ -42,7 +42,6 @@ #include #include - namespace fuse_models { @@ -66,25 +65,11 @@ namespace fuse_models * @param[out] acc_linear2_x - Second X acceleration * @param[out] acc_linear2_y - Second Y acceleration */ -template -inline void predict( - const T position1_x, - const T position1_y, - const T yaw1, - const T vel_linear1_x, - const T vel_linear1_y, - const T vel_yaw1, - const T acc_linear1_x, - const T acc_linear1_y, - const T dt, - T & position2_x, - T & position2_y, - T & yaw2, - T & vel_linear2_x, - T & vel_linear2_y, - T & vel_yaw2, - T & acc_linear2_x, - T & acc_linear2_y) +template +inline void predict(const T position1_x, const T position1_y, const T yaw1, const T vel_linear1_x, + const T vel_linear1_y, const T vel_yaw1, const T acc_linear1_x, const T acc_linear1_y, const T dt, + T& position2_x, T& position2_y, T& yaw2, T& vel_linear2_x, T& vel_linear2_y, T& vel_yaw2, + T& acc_linear2_x, T& acc_linear2_y) { // There are better models for this projection, but this matches the one used by r_l. T sy = ceres::sin(yaw1); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses this model @@ -125,25 +110,11 @@ inline void predict( * @param[out] acc_linear2_y - Second Y acceleration * @param[out] jacobians - Jacobians wrt the state */ -inline void predict( - const double position1_x, - const double position1_y, - const double yaw1, - const double vel_linear1_x, - const double vel_linear1_y, - const double vel_yaw1, - const double acc_linear1_x, - const double acc_linear1_y, - const double dt, - double & position2_x, - double & position2_y, - double & yaw2, - double & vel_linear2_x, - double & vel_linear2_y, - double & vel_yaw2, - double & acc_linear2_x, - double & acc_linear2_y, - double ** jacobians) +inline void predict(const double position1_x, const double position1_y, const double yaw1, const double vel_linear1_x, + const double vel_linear1_y, const double vel_yaw1, const double acc_linear1_x, + const double acc_linear1_y, const double dt, double& position2_x, double& position2_y, double& yaw2, + double& vel_linear2_x, double& vel_linear2_y, double& vel_yaw2, double& acc_linear2_x, + double& acc_linear2_y, double** jacobians) { // There are better models for this projection, but this matches the one used by r_l. const double sy = ceres::sin(yaw1); // Should probably be sin((yaw1 + yaw2) / 2), but r_l uses @@ -168,62 +139,47 @@ inline void predict( fuse_core::wrapAngle2D(yaw2); - if (jacobians) { + if (jacobians) + { // Jacobian wrt position1 - if (jacobians[0]) { + if (jacobians[0]) + { Eigen::Map> jacobian(jacobians[0]); - jacobian << 1, 0, - 0, 1, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0; + jacobian << 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; } // Jacobian wrt yaw1 - if (jacobians[1]) { + if (jacobians[1]) + { Eigen::Map jacobian(jacobians[1]); jacobian << -delta_y_rot, delta_x_rot, 1, 0, 0, 0, 0, 0; } // Jacobian wrt vel_linear1 - if (jacobians[2]) { + if (jacobians[2]) + { const double cy_dt = cy * dt; const double sy_dt = sy * dt; Eigen::Map> jacobian(jacobians[2]); - jacobian << cy_dt, -sy_dt, - sy_dt, cy_dt, - 0, 0, - 1, 0, - 0, 1, - 0, 0, - 0, 0, - 0, 0; + jacobian << cy_dt, -sy_dt, sy_dt, cy_dt, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0; } // Jacobian wrt vel_yaw1 - if (jacobians[3]) { + if (jacobians[3]) + { Eigen::Map jacobian(jacobians[3]); jacobian << 0, 0, dt, 0, 0, 1, 0, 0; } // Jacobian wrt acc_linear1 - if (jacobians[4]) { + if (jacobians[4]) + { const double cy_half_dt2 = cy * half_dt2; const double sy_half_dt2 = sy * half_dt2; Eigen::Map> jacobian(jacobians[4]); - jacobian << cy_half_dt2, -sy_half_dt2, - sy_half_dt2, cy_half_dt2, - 0, 0, - dt, 0, - 0, dt, - 0, 0, - 1, 0, - 0, 1; + jacobian << cy_half_dt2, -sy_half_dt2, sy_half_dt2, cy_half_dt2, 0, 0, dt, 0, 0, dt, 0, 0, 1, 0, 0, 1; } } } @@ -242,38 +198,14 @@ inline void predict( * @param[out] vel_yaw2 - Second yaw velocity * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1) */ -template -inline void predict( - const T * const position1, - const T * const yaw1, - const T * const vel_linear1, - const T * const vel_yaw1, - const T * const acc_linear1, - const T dt, - T * const position2, - T * const yaw2, - T * const vel_linear2, - T * const vel_yaw2, - T * const acc_linear2) +template +inline void predict(const T* const position1, const T* const yaw1, const T* const vel_linear1, const T* const vel_yaw1, + const T* const acc_linear1, const T dt, T* const position2, T* const yaw2, T* const vel_linear2, + T* const vel_yaw2, T* const acc_linear2) { - predict( - position1[0], - position1[1], - *yaw1, - vel_linear1[0], - vel_linear1[1], - *vel_yaw1, - acc_linear1[0], - acc_linear1[1], - dt, - position2[0], - position2[1], - *yaw2, - vel_linear2[0], - vel_linear2[1], - *vel_yaw2, - acc_linear2[0], - acc_linear2[1]); + predict(position1[0], position1[1], *yaw1, vel_linear1[0], vel_linear1[1], *vel_yaw1, acc_linear1[0], acc_linear1[1], + dt, position2[0], position2[1], *yaw2, vel_linear2[0], vel_linear2[1], *vel_yaw2, acc_linear2[0], + acc_linear2[1]); } /** @@ -289,62 +221,40 @@ inline void predict( * @param[in] acc_linear2 - The second linear acceleration * @param[in] jacobian - The jacobian wrt the state */ -inline void predict( - const tf2_2d::Transform & pose1, - const tf2_2d::Vector2 & vel_linear1, - const double vel_yaw1, - const tf2_2d::Vector2 & acc_linear1, - const double dt, - tf2_2d::Transform & pose2, - tf2_2d::Vector2 & vel_linear2, - double & vel_yaw2, - tf2_2d::Vector2 & acc_linear2, - fuse_core::Matrix8d & jacobian) +inline void predict(const tf2_2d::Transform& pose1, const tf2_2d::Vector2& vel_linear1, const double vel_yaw1, + const tf2_2d::Vector2& acc_linear1, const double dt, tf2_2d::Transform& pose2, + tf2_2d::Vector2& vel_linear2, double& vel_yaw2, tf2_2d::Vector2& acc_linear2, + fuse_core::Matrix8d& jacobian) { - double x_pred {}; - double y_pred {}; - double yaw_pred {}; - double vel_linear_x_pred {}; - double vel_linear_y_pred {}; - double acc_linear_x_pred {}; - double acc_linear_y_pred {}; + double x_pred{}; + double y_pred{}; + double yaw_pred{}; + double vel_linear_x_pred{}; + double vel_linear_y_pred{}; + double acc_linear_x_pred{}; + double acc_linear_y_pred{}; // fuse_core::Matrix8d is Eigen::RowMajor, so we cannot use pointers to the columns where each // parameter block starts. Instead, we need to create a vector of Eigen::RowMajor matrices per // parameter block and later reconstruct the fuse_core::Matrix8d with the full jacobian. The // parameter blocks have the following sizes: {position1: 2, yaw1: 1, vel_linear1: 2, vel_yaw1: 1, // acc_linear1: 2} - static constexpr size_t num_residuals{8}; - static constexpr size_t num_parameter_blocks{5}; - static const std::array block_sizes = {2, 1, 2, 1, 2}; + static constexpr size_t num_residuals{ 8 }; + static constexpr size_t num_parameter_blocks{ 5 }; + static const std::array block_sizes = { 2, 1, 2, 1, 2 }; std::array J; - std::array jacobians; + std::array jacobians; - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } - predict( - pose1.x(), - pose1.y(), - pose1.yaw(), - vel_linear1.x(), - vel_linear1.y(), - vel_yaw1, - acc_linear1.x(), - acc_linear1.y(), - dt, - x_pred, - y_pred, - yaw_pred, - vel_linear_x_pred, - vel_linear_y_pred, - vel_yaw2, - acc_linear_x_pred, - acc_linear_y_pred, - jacobians.data()); + predict(pose1.x(), pose1.y(), pose1.yaw(), vel_linear1.x(), vel_linear1.y(), vel_yaw1, acc_linear1.x(), + acc_linear1.y(), dt, x_pred, y_pred, yaw_pred, vel_linear_x_pred, vel_linear_y_pred, vel_yaw2, + acc_linear_x_pred, acc_linear_y_pred, jacobians.data()); jacobian << J[0], J[1], J[2], J[3], J[4]; @@ -369,43 +279,21 @@ inline void predict( * @param[in] vel_yaw2 - The second yaw velocity * @param[in] acc_linear2 - The second linear acceleration */ -inline void predict( - const tf2_2d::Transform & pose1, - const tf2_2d::Vector2 & vel_linear1, - const double vel_yaw1, - const tf2_2d::Vector2 & acc_linear1, - const double dt, - tf2_2d::Transform & pose2, - tf2_2d::Vector2 & vel_linear2, - double & vel_yaw2, - tf2_2d::Vector2 & acc_linear2) +inline void predict(const tf2_2d::Transform& pose1, const tf2_2d::Vector2& vel_linear1, const double vel_yaw1, + const tf2_2d::Vector2& acc_linear1, const double dt, tf2_2d::Transform& pose2, + tf2_2d::Vector2& vel_linear2, double& vel_yaw2, tf2_2d::Vector2& acc_linear2) { - double x_pred {}; - double y_pred {}; - double yaw_pred {}; - double vel_linear_x_pred {}; - double vel_linear_y_pred {}; - double acc_linear_x_pred {}; - double acc_linear_y_pred {}; - - predict( - pose1.x(), - pose1.y(), - pose1.yaw(), - vel_linear1.x(), - vel_linear1.y(), - vel_yaw1, - acc_linear1.x(), - acc_linear1.y(), - dt, - x_pred, - y_pred, - yaw_pred, - vel_linear_x_pred, - vel_linear_y_pred, - vel_yaw2, - acc_linear_x_pred, - acc_linear_y_pred); + double x_pred{}; + double y_pred{}; + double yaw_pred{}; + double vel_linear_x_pred{}; + double vel_linear_y_pred{}; + double acc_linear_x_pred{}; + double acc_linear_y_pred{}; + + predict(pose1.x(), pose1.y(), pose1.yaw(), vel_linear1.x(), vel_linear1.y(), vel_yaw1, acc_linear1.x(), + acc_linear1.y(), dt, x_pred, y_pred, yaw_pred, vel_linear_x_pred, vel_linear_y_pred, vel_yaw2, + acc_linear_x_pred, acc_linear_y_pred); pose2.setX(x_pred); pose2.setY(y_pred); diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h index 23cb9f95f..e67ff4e1b 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h +++ b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTION_H_ #define FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTION_H_ -#warning \ - This header is obsolete, please include fuse_models/unicycle_2d_state_cost_function.hpp instead +#warning This header is obsolete, please include fuse_models/unicycle_2d_state_cost_function.hpp instead #include diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.hpp b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.hpp index 69f7fcbad..cdae9ce35 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.hpp @@ -42,7 +42,6 @@ #include #include - namespace fuse_models { @@ -94,7 +93,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, * @param[in] A The residual weighting matrix, most likely the square root information matrix in * order (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc) */ - Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d & A); + Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -120,10 +119,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, * @return The return value indicates whether the computation of the residuals and/or jacobians * was successful or not. */ - bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const override + bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override { double position_pred_x; double position_pred_y; @@ -133,25 +129,16 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, double vel_yaw_pred; double acc_linear_pred_x; double acc_linear_pred_y; - predict( - parameters[0][0], // position1_x - parameters[0][1], // position1_y - parameters[1][0], // yaw1 - parameters[2][0], // vel_linear1_x - parameters[2][1], // vel_linear1_y - parameters[3][0], // vel_yaw1 - parameters[4][0], // acc_linear1_x - parameters[4][1], // acc_linear1_y - dt_, - position_pred_x, - position_pred_y, - yaw_pred, - vel_linear_pred_x, - vel_linear_pred_y, - vel_yaw_pred, - acc_linear_pred_x, - acc_linear_pred_y, - jacobians); + predict(parameters[0][0], // position1_x + parameters[0][1], // position1_y + parameters[1][0], // yaw1 + parameters[2][0], // vel_linear1_x + parameters[2][1], // vel_linear1_y + parameters[3][0], // vel_yaw1 + parameters[4][0], // acc_linear1_x + parameters[4][1], // acc_linear1_y + dt_, position_pred_x, position_pred_y, yaw_pred, vel_linear_pred_x, vel_linear_pred_y, vel_yaw_pred, + acc_linear_pred_x, acc_linear_pred_y, jacobians); residuals[0] = parameters[5][0] - position_pred_x; residuals[1] = parameters[5][1] - position_pred_y; @@ -169,7 +156,8 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, Eigen::Map residuals_map(residuals); residuals_map.applyOnTheLeft(A_); - if (jacobians) { + if (jacobians) + { // It might be possible to simplify the code below implementing something like this but using // compile-time template recursion. // @@ -185,31 +173,36 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, // } // Update jacobian wrt position1 - if (jacobians[0]) { + if (jacobians[0]) + { Eigen::Map> jacobian(jacobians[0]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt yaw1 - if (jacobians[1]) { + if (jacobians[1]) + { Eigen::Map jacobian(jacobians[1]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt vel_linear1 - if (jacobians[2]) { + if (jacobians[2]) + { Eigen::Map> jacobian(jacobians[2]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt vel_yaw1 - if (jacobians[3]) { + if (jacobians[3]) + { Eigen::Map jacobian(jacobians[3]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt acc_linear1 - if (jacobians[4]) { + if (jacobians[4]) + { Eigen::Map> jacobian(jacobians[4]); jacobian.applyOnTheLeft(-A_); } @@ -232,31 +225,36 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, // } // Jacobian wrt position2 - if (jacobians[5]) { + if (jacobians[5]) + { Eigen::Map> jacobian(jacobians[5]); jacobian = A_.block<8, 2>(0, 0); } // Jacobian wrt yaw2 - if (jacobians[6]) { + if (jacobians[6]) + { Eigen::Map jacobian(jacobians[6]); jacobian = A_.col(2); } // Jacobian wrt vel_linear2 - if (jacobians[7]) { + if (jacobians[7]) + { Eigen::Map> jacobian(jacobians[7]); jacobian = A_.block<8, 2>(0, 3); } // Jacobian wrt vel_yaw2 - if (jacobians[8]) { + if (jacobians[8]) + { Eigen::Map jacobian(jacobians[8]); jacobian = A_.col(5); } // Jacobian wrt acc_linear2 - if (jacobians[9]) { + if (jacobians[9]) + { Eigen::Map> jacobian(jacobians[9]); jacobian = A_.block<8, 2>(0, 6); } @@ -271,11 +269,7 @@ class Unicycle2DStateCostFunction : public ceres::SizedCostFunction<8, 2, 1, 2, //!< information matrix }; -Unicycle2DStateCostFunction::Unicycle2DStateCostFunction( - const double dt, - const fuse_core::Matrix8d & A) -: dt_(dt), - A_(A) +Unicycle2DStateCostFunction::Unicycle2DStateCostFunction(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) { } diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h b/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h index 68457fcc3..c7a330df2 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h +++ b/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTOR_H_ #define FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTOR_H_ -#warning \ - This header is obsolete, please include fuse_models/unicycle_2d_state_cost_functor.hpp instead +#warning This header is obsolete, please include fuse_models/unicycle_2d_state_cost_functor.hpp instead #include diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.hpp b/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.hpp index b77458715..a37f49d69 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.hpp @@ -40,7 +40,6 @@ #include #include - namespace fuse_models { @@ -92,7 +91,7 @@ class Unicycle2DStateCostFunctor * @param[in] A The residual weighting matrix, most likely the square root information matrix in * order (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc) */ - Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d & A); + Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. @@ -108,19 +107,10 @@ class Unicycle2DStateCostFunctor * @param[in] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1) * @param[out] residual - The computed residual (error) */ - template - bool operator()( - const T * const position1, - const T * const yaw1, - const T * const vel_linear1, - const T * const vel_yaw1, - const T * const acc_linear1, - const T * const position2, - const T * const yaw2, - const T * const vel_linear2, - const T * const vel_yaw2, - const T * const acc_linear2, - T * residual) const; + template + bool operator()(const T* const position1, const T* const yaw1, const T* const vel_linear1, const T* const vel_yaw1, + const T* const acc_linear1, const T* const position2, const T* const yaw2, const T* const vel_linear2, + const T* const vel_yaw2, const T* const acc_linear2, T* residual) const; private: double dt_; @@ -128,45 +118,23 @@ class Unicycle2DStateCostFunctor //!< information matrix }; -Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor( - const double dt, - const fuse_core::Matrix8d & A) -: dt_(dt), - A_(A) +Unicycle2DStateCostFunctor::Unicycle2DStateCostFunctor(const double dt, const fuse_core::Matrix8d& A) : dt_(dt), A_(A) { } -template -bool Unicycle2DStateCostFunctor::operator()( - const T * const position1, - const T * const yaw1, - const T * const vel_linear1, - const T * const vel_yaw1, - const T * const acc_linear1, - const T * const position2, - const T * const yaw2, - const T * const vel_linear2, - const T * const vel_yaw2, - const T * const acc_linear2, - T * residual) const +template +bool Unicycle2DStateCostFunctor::operator()(const T* const position1, const T* const yaw1, const T* const vel_linear1, + const T* const vel_yaw1, const T* const acc_linear1, + const T* const position2, const T* const yaw2, const T* const vel_linear2, + const T* const vel_yaw2, const T* const acc_linear2, T* residual) const { T position_pred[2]; T yaw_pred[1]; T vel_linear_pred[2]; T vel_yaw_pred[1]; T acc_linear_pred[2]; - predict( - position1, - yaw1, - vel_linear1, - vel_yaw1, - acc_linear1, - T(dt_), - position_pred, - yaw_pred, - vel_linear_pred, - vel_yaw_pred, - acc_linear_pred); + predict(position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, T(dt_), position_pred, yaw_pred, vel_linear_pred, + vel_yaw_pred, acc_linear_pred); Eigen::Map> residuals_map(residual); residuals_map(0) = position2[0] - position_pred[0]; diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.h b/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.h index 4500aca39..b714233a7 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.h +++ b/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.h @@ -35,8 +35,7 @@ #ifndef FUSE_MODELS__UNICYCLE_2D_STATE_KINEMATIC_CONSTRAINT_H_ #define FUSE_MODELS__UNICYCLE_2D_STATE_KINEMATIC_CONSTRAINT_H_ -#warning \ - This header is obsolete, please include fuse_models/unicycle_2d_state_kinematic_constraint.hpp \ +#warning This header is obsolete, please include fuse_models/unicycle_2d_state_kinematic_constraint.hpp \ instead #include diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.hpp index 3f7667787..ae86596ca 100644 --- a/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.hpp +++ b/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.hpp @@ -53,7 +53,6 @@ #include #include - namespace fuse_models { @@ -93,19 +92,17 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * @param[in] covariance - The covariance matrix used to weight the constraint. Order is (x, y, * yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc) */ - Unicycle2DStateKinematicConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & position1, - const fuse_variables::Orientation2DStamped & yaw1, - const fuse_variables::VelocityLinear2DStamped & linear_velocity1, - const fuse_variables::VelocityAngular2DStamped & yaw_velocity1, - const fuse_variables::AccelerationLinear2DStamped & linear_acceleration1, - const fuse_variables::Position2DStamped & position2, - const fuse_variables::Orientation2DStamped & yaw2, - const fuse_variables::VelocityLinear2DStamped & linear_velocity2, - const fuse_variables::VelocityAngular2DStamped & yaw_velocity2, - const fuse_variables::AccelerationLinear2DStamped & linear_acceleration2, - const fuse_core::Matrix8d & covariance); + Unicycle2DStateKinematicConstraint(const std::string& source, const fuse_variables::Position2DStamped& position1, + const fuse_variables::Orientation2DStamped& yaw1, + const fuse_variables::VelocityLinear2DStamped& linear_velocity1, + const fuse_variables::VelocityAngular2DStamped& yaw_velocity1, + const fuse_variables::AccelerationLinear2DStamped& linear_acceleration1, + const fuse_variables::Position2DStamped& position2, + const fuse_variables::Orientation2DStamped& yaw2, + const fuse_variables::VelocityLinear2DStamped& linear_velocity2, + const fuse_variables::VelocityAngular2DStamped& yaw_velocity2, + const fuse_variables::AccelerationLinear2DStamped& linear_acceleration2, + const fuse_core::Matrix8d& covariance); /** * @brief Destructor @@ -117,14 +114,20 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * the position1 and position2 variables in the constructor) */ - double dt() const {return dt_;} + double dt() const + { + return dt_; + } /** * @brief Read-only access to the square root information matrix. * * Order is (x, y, yaw, x_vel, y_vel, yaw_vel, x_acc, y_acc) */ - const fuse_core::Matrix8d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix8d& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -141,7 +144,7 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -153,10 +156,10 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - double dt_; //!< The time delta for the constraint + double dt_; //!< The time delta for the constraint fuse_core::Matrix8d sqrt_information_; //!< The square root information matrix private: @@ -170,12 +173,12 @@ class Unicycle2DStateKinematicConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & dt_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& dt_; + archive& sqrt_information_; } }; diff --git a/fuse_models/package.xml b/fuse_models/package.xml index 28d6bf95c..d90eb3c39 100644 --- a/fuse_models/package.xml +++ b/fuse_models/package.xml @@ -56,8 +56,6 @@ benchmark ament_cmake_gtest ament_cmake_gmock - ament_lint_auto - ament_lint_common ament_cmake diff --git a/fuse_models/src/acceleration_2d.cpp b/fuse_models/src/acceleration_2d.cpp index 509981c82..e720ec010 100644 --- a/fuse_models/src/acceleration_2d.cpp +++ b/fuse_models/src/acceleration_2d.cpp @@ -46,17 +46,15 @@ namespace fuse_models { Acceleration2D::Acceleration2D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Acceleration2D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Acceleration2D::process, this, std::placeholders::_1)) { } -void Acceleration2D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Acceleration2D::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); @@ -74,45 +72,36 @@ void Acceleration2D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } - if (params_.indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " - << fuse_core::joinTopicName(name_, params_.topic) << " will be ignored."); + if (params_.indices.empty()) + { + RCLCPP_WARN_STREAM(logger_, "No dimensions were specified. Data from topic " + << fuse_core::joinTopicName(name_, params_.topic) << " will 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() - ); + 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 Acceleration2D::onStart() { - if (!params_.indices.empty()) { + if (!params_.indices.empty()) + { rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &AccelerationThrottledCallback::callback< - const geometry_msgs::msg::AccelWithCovarianceStamped &>, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&AccelerationThrottledCallback::callback, + &throttled_callback_, std::placeholders::_1), + sub_options); } } @@ -121,23 +110,14 @@ void Acceleration2D::onStop() sub_.reset(); } -void Acceleration2D::process(const geometry_msgs::msg::AccelWithCovarianceStamped & msg) +void Acceleration2D::process(const geometry_msgs::msg::AccelWithCovarianceStamped& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); transaction->stamp(msg.header.stamp); - common::processAccelWithCovariance( - name(), - device_id_, - msg, - params_.loss, - params_.target_frame, - params_.indices, - *tf_buffer_, - !params_.disable_checks, - *transaction, - params_.tf_timeout); + common::processAccelWithCovariance(name(), device_id_, msg, params_.loss, params_.target_frame, params_.indices, + *tf_buffer_, !params_.disable_checks, *transaction, params_.tf_timeout); // Send the transaction object to the plugin's parent sendTransaction(transaction); diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index 2132999b4..db74fb258 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -46,16 +46,12 @@ namespace fuse_models { GraphIgnition::GraphIgnition() -: fuse_core::AsyncSensorModel(1), - started_(false), - logger_(rclcpp::get_logger("uninitialized")) + : fuse_core::AsyncSensorModel(1), started_(false), logger_(rclcpp::get_logger("uninitialized")) { } -void GraphIgnition::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void GraphIgnition::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); @@ -69,40 +65,26 @@ void GraphIgnition::onInit() params_.loadFromROS(interfaces_, name_); // Connect to the reset service - if (!params_.reset_service.empty()) { + if (!params_.reset_service.empty()) + { reset_client_ = rclcpp::create_client( - interfaces_.get_node_base_interface(), - interfaces_.get_node_graph_interface(), - interfaces_.get_node_services_interface(), - params_.reset_service, - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_graph_interface(), + interfaces_.get_node_services_interface(), params_.reset_service, rclcpp::ServicesQoS(), cb_group_); } // Advertise rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - fuse_core::joinTopicName(name_, params_.topic), - params_.queue_size, - std::bind(&GraphIgnition::subscriberCallback, this, std::placeholders::_1), - sub_options - ); + interfaces_, fuse_core::joinTopicName(name_, params_.topic), params_.queue_size, + std::bind(&GraphIgnition::subscriberCallback, this, std::placeholders::_1), sub_options); set_graph_service_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.set_graph_service), - std::bind( - &GraphIgnition::setGraphServiceCallback, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3), - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_graph_service), + std::bind(&GraphIgnition::setGraphServiceCallback, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + rclcpp::ServicesQoS(), cb_group_); } void GraphIgnition::start() @@ -115,29 +97,32 @@ void GraphIgnition::stop() started_ = false; } -void GraphIgnition::subscriberCallback(const fuse_msgs::msg::SerializedGraph & msg) +void GraphIgnition::subscriberCallback(const fuse_msgs::msg::SerializedGraph& msg) { - try { + try + { process(msg); - } catch (const std::exception & e) { + } + catch (const std::exception& e) + { RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); } } -bool GraphIgnition::setGraphServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetGraph::Request::SharedPtr req) +bool GraphIgnition::setGraphServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetGraph::Request::SharedPtr req) { - try { - process( - req->graph, - [service, request_id]() { - fuse_msgs::srv::SetGraph::Response response; - response.success = true; - service->send_response(*request_id, response); - }); - } catch (const std::exception & e) { + try + { + process(req->graph, [service, request_id]() { + fuse_msgs::srv::SetGraph::Response response; + response.success = true; + service->send_response(*request_id, response); + }); + } + catch (const std::exception& e) + { fuse_msgs::srv::SetGraph::Response response; response.success = false; response.message = e.what(); @@ -147,11 +132,11 @@ bool GraphIgnition::setGraphServiceCallback( return true; } -void GraphIgnition::process( - const fuse_msgs::msg::SerializedGraph & msg, std::function post_process) +void GraphIgnition::process(const fuse_msgs::msg::SerializedGraph& msg, std::function post_process) { // Verify we are in the correct state to process set graph requests - if (!started_) { + if (!started_) + { throw std::runtime_error("Attempting to set the graph while the sensor is stopped."); } @@ -159,90 +144,96 @@ void GraphIgnition::process( // NOTE(methylDragon): We convert the Graph::UniquePtr to a shared pointer so it can be passed as // a copyable object to the deferred service call's std::function<> arg to // satisfy the requirement that std::function<> arguments are copyable. - const auto graph = - std::shared_ptr(std::move(graph_deserializer_.deserialize(msg))); + const auto graph = std::shared_ptr(std::move(graph_deserializer_.deserialize(msg))); // Validate the requested graph before we do anything - if (boost::empty(graph->getConstraints())) { + if (boost::empty(graph->getConstraints())) + { throw std::runtime_error("Attempting to set a graph with no constraints."); } - if (boost::empty(graph->getVariables())) { + if (boost::empty(graph->getVariables())) + { throw std::runtime_error("Attempting to set a graph with no variables."); } // Tell the optimizer to reset before providing the initial state - if (!params_.reset_service.empty()) { + if (!params_.reset_service.empty()) + { // Wait for the reset service while (!reset_client_->wait_for_service(std::chrono::seconds(10)) && - interfaces_.get_node_base_interface()->get_context()->is_valid()) + interfaces_.get_node_base_interface()->get_context()->is_valid()) { - RCLCPP_WARN_STREAM( - logger_, - "Waiting for '" << reset_client_->wait_for_service() << "' service to become avaiable."); + RCLCPP_WARN_STREAM(logger_, + "Waiting for '" << reset_client_->wait_for_service() << "' service to become available."); } auto srv = std::make_shared(); // Don't block the executor. // It needs to be free to handle the response to this service call. // Have a callback do the rest of the work when a response comes. - auto result_future = reset_client_->async_send_request( - srv, - [this, post_process, captured_graph = std::move(graph), - msg](rclcpp::Client::SharedFuture result) { - (void)result; - // Now that the optimizer has been reset, actually send the initial state constraints to the - // optimizer - sendGraph(*captured_graph, msg.header.stamp); - if (post_process) { - post_process(); - } - }); - } else { + auto result_future = + reset_client_->async_send_request(srv, [this, post_process, captured_graph = std::move(graph), + msg](rclcpp::Client::SharedFuture result) { + (void)result; + // Now that the optimizer has been reset, actually send the initial state constraints to the + // optimizer + sendGraph(*captured_graph, msg.header.stamp); + if (post_process) + { + post_process(); + } + }); + } + else + { sendGraph(*graph, msg.header.stamp); - if (post_process) { + if (post_process) + { post_process(); } } } -void GraphIgnition::sendGraph(const fuse_core::Graph & graph, const rclcpp::Time & stamp) +void GraphIgnition::sendGraph(const fuse_core::Graph& graph, const rclcpp::Time& stamp) { // Create a transaction equivalent to the graph auto transaction = fuse_core::Transaction::make_shared(); transaction->stamp(stamp); // Add variables - for (const auto & variable : graph.getVariables()) { + for (const auto& variable : graph.getVariables()) + { transaction->addVariable(variable.clone()); // If the variable is a fuse_variables::Stamped variable, set the involved stamp - const auto stamped_variable = dynamic_cast(&variable); - if (stamped_variable) { + const auto stamped_variable = dynamic_cast(&variable); + if (stamped_variable) + { transaction->addInvolvedStamp(stamped_variable->stamp()); } } // If the transaction ended up with no involved stamps, we use a single involved stamped equal to // the transaction/graph stamp - if (boost::empty(transaction->involvedStamps())) { + if (boost::empty(transaction->involvedStamps())) + { transaction->addInvolvedStamp(stamp); } // Add constraints - for (const auto & constraint : graph.getConstraints()) { + for (const auto& constraint : graph.getConstraints()) + { transaction->addConstraint(constraint.clone()); } // Send the transaction to the optimizer. sendTransaction(transaction); - RCLCPP_INFO_STREAM( - logger_, - "Received a set_graph request (stamp: " - << transaction->stamp().nanoseconds() << ", constraints: " - << boost::size(transaction->addedConstraints()) << ", variables: " - << boost::size(transaction->addedVariables()) << ")"); + RCLCPP_INFO_STREAM(logger_, "Received a set_graph request (stamp: " + << transaction->stamp().nanoseconds() + << ", constraints: " << boost::size(transaction->addedConstraints()) + << ", variables: " << boost::size(transaction->addedVariables()) << ")"); } } // namespace fuse_models diff --git a/fuse_models/src/imu_2d.cpp b/fuse_models/src/imu_2d.cpp index 1a619f660..2521f5dfb 100644 --- a/fuse_models/src/imu_2d.cpp +++ b/fuse_models/src/imu_2d.cpp @@ -45,7 +45,6 @@ #include #include - // Register this sensor model with ROS as a plugin. PLUGINLIB_EXPORT_CLASS(fuse_models::Imu2D, fuse_core::SensorModel) @@ -53,17 +52,15 @@ namespace fuse_models { Imu2D::Imu2D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Imu2D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Imu2D::process, this, std::placeholders::_1)) { } -void Imu2D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Imu2D::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); @@ -81,35 +78,29 @@ void Imu2D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } - if (params_.orientation_indices.empty() && - params_.linear_acceleration_indices.empty() && - params_.angular_velocity_indices.empty()) + if (params_.orientation_indices.empty() && params_.linear_acceleration_indices.empty() && + params_.angular_velocity_indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " << params_.topic - << " will be ignored."); + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << params_.topic << " will 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() - ); + 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 Imu2D::onStart() { - if (!params_.orientation_indices.empty() || - !params_.linear_acceleration_indices.empty() || - !params_.angular_velocity_indices.empty()) + if (!params_.orientation_indices.empty() || !params_.linear_acceleration_indices.empty() || + !params_.angular_velocity_indices.empty()) { previous_pose_.reset(); @@ -117,16 +108,10 @@ void Imu2D::onStart() sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &ImuThrottledCallback::callback, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&ImuThrottledCallback::callback, &throttled_callback_, + std::placeholders::_1), + sub_options); } } @@ -135,7 +120,7 @@ void Imu2D::onStop() sub_.reset(); } -void Imu2D::process(const sensor_msgs::msg::Imu & msg) +void Imu2D::process(const sensor_msgs::msg::Imu& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -170,37 +155,21 @@ void Imu2D::process(const sensor_msgs::msg::Imu & msg) const bool validate = !params_.disable_checks; - if (params_.differential) { + if (params_.differential) + { processDifferential(*pose, twist, validate, *transaction); - } else { - common::processAbsolutePoseWithCovariance( - name(), - device_id_, - *pose, - params_.pose_loss, - params_.orientation_target_frame, - {}, - params_.orientation_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + } + else + { + common::processAbsolutePoseWithCovariance(name(), device_id_, *pose, params_.pose_loss, + params_.orientation_target_frame, {}, params_.orientation_indices, + *tf_buffer_, validate, *transaction, params_.tf_timeout); } // Handle the twist data (only include indices for angular velocity) - common::processTwistWithCovariance( - name(), - device_id_, - twist, - nullptr, - params_.angular_velocity_loss, - params_.twist_target_frame, - {}, - params_.angular_velocity_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + common::processTwistWithCovariance(name(), device_id_, twist, nullptr, params_.angular_velocity_loss, + params_.twist_target_frame, {}, params_.angular_velocity_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); // Handle the acceleration data geometry_msgs::msg::AccelWithCovarianceStamped accel; @@ -217,7 +186,8 @@ void Imu2D::process(const sensor_msgs::msg::Imu & msg) accel.accel.covariance[14] = msg.linear_acceleration_covariance[8]; // Optionally remove the acceleration due to gravity - if (params_.remove_gravitational_acceleration) { + if (params_.remove_gravitational_acceleration) + { geometry_msgs::msg::Vector3 accel_gravity; accel_gravity.z = params_.gravitational_acceleration; geometry_msgs::msg::TransformStamped orientation_trans; @@ -230,87 +200,64 @@ void Imu2D::process(const sensor_msgs::msg::Imu & msg) accel.accel.accel.linear.z -= accel_gravity.z; } - common::processAccelWithCovariance( - name(), - device_id_, - accel, - params_.linear_acceleration_loss, - params_.acceleration_target_frame, - params_.linear_acceleration_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + common::processAccelWithCovariance(name(), device_id_, accel, params_.linear_acceleration_loss, + params_.acceleration_target_frame, params_.linear_acceleration_indices, + *tf_buffer_, validate, *transaction, params_.tf_timeout); // Send the transaction object to the plugin's parent sendTransaction(transaction); } -void Imu2D::processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, const bool validate, - fuse_core::Transaction & transaction) +void Imu2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); transformed_pose->header.frame_id = - params_.orientation_target_frame.empty() ? pose.header.frame_id : params_. - orientation_target_frame; - - if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform pose message with stamp " << rclcpp::Time( - pose.header.stamp).nanoseconds() - << " to orientation target frame " << - params_.orientation_target_frame); + params_.orientation_target_frame.empty() ? pose.header.frame_id : params_.orientation_target_frame; + + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time(pose.header.stamp).nanoseconds() << " to orientation target frame " + << params_.orientation_target_frame); return; } - if (!previous_pose_) { + if (!previous_pose_) + { previous_pose_ = std::move(transformed_pose); return; } - if (params_.use_twist_covariance) { + if (params_.use_twist_covariance) + { geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; transformed_twist.header.frame_id = - params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - - if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform twist message with stamp " << rclcpp::Time( - twist.header.stamp).nanoseconds() - << " to twist target frame " << - params_.twist_target_frame); - } else { - common::processDifferentialPoseWithTwistCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - twist, - params_.minimum_pose_relative_covariance, - params_.twist_covariance_offset, - params_.pose_loss, - {}, - params_.orientation_indices, - validate, - transaction); + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " + << rclcpp::Time(twist.header.stamp).nanoseconds() << " to twist target frame " + << params_.twist_target_frame); } - } else { - common::processDifferentialPoseWithCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - params_.independent, - params_.minimum_pose_relative_covariance, - params_.pose_loss, - {}, - params_.orientation_indices, - validate, - transaction); + else + { + common::processDifferentialPoseWithTwistCovariance(name(), device_id_, *previous_pose_, *transformed_pose, twist, + params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, params_.pose_loss, {}, + params_.orientation_indices, validate, transaction); + } + } + else + { + common::processDifferentialPoseWithCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + params_.independent, params_.minimum_pose_relative_covariance, + params_.pose_loss, {}, params_.orientation_indices, validate, + transaction); } previous_pose_ = std::move(transformed_pose); diff --git a/fuse_models/src/odometry_2d.cpp b/fuse_models/src/odometry_2d.cpp index 192d56b89..e9f5d43b5 100644 --- a/fuse_models/src/odometry_2d.cpp +++ b/fuse_models/src/odometry_2d.cpp @@ -51,17 +51,15 @@ namespace fuse_models { Odometry2D::Odometry2D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Odometry2D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Odometry2D::process, this, std::placeholders::_1)) { } -void Odometry2D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Odometry2D::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); @@ -79,37 +77,29 @@ void Odometry2D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } - if (params_.position_indices.empty() && - params_.orientation_indices.empty() && - params_.linear_velocity_indices.empty() && - params_.angular_velocity_indices.empty()) + if (params_.position_indices.empty() && params_.orientation_indices.empty() && + params_.linear_velocity_indices.empty() && params_.angular_velocity_indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " << params_.topic - << " will be ignored."); + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << params_.topic << " will 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() - ); + 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 Odometry2D::onStart() { - if (!params_.position_indices.empty() || - !params_.orientation_indices.empty() || - !params_.linear_velocity_indices.empty() || - !params_.angular_velocity_indices.empty()) + if (!params_.position_indices.empty() || !params_.orientation_indices.empty() || + !params_.linear_velocity_indices.empty() || !params_.angular_velocity_indices.empty()) { previous_pose_.reset(); @@ -117,17 +107,10 @@ void Odometry2D::onStart() sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &OdometryThrottledCallback::callback< - const nav_msgs::msg::Odometry &>, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&OdometryThrottledCallback::callback, &throttled_callback_, + std::placeholders::_1), + sub_options); } } @@ -136,7 +119,7 @@ void Odometry2D::onStop() sub_.reset(); } -void Odometry2D::process(const nav_msgs::msg::Odometry & msg) +void Odometry2D::process(const nav_msgs::msg::Odometry& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -154,105 +137,78 @@ void Odometry2D::process(const nav_msgs::msg::Odometry & msg) const bool validate = !params_.disable_checks; - if (params_.differential) { + if (params_.differential) + { processDifferential(*pose, twist, validate, *transaction); - } else { - common::processAbsolutePoseWithCovariance( - name(), - device_id_, - *pose, - params_.pose_loss, - params_.pose_target_frame, - params_.position_indices, - params_.orientation_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + } + else + { + common::processAbsolutePoseWithCovariance(name(), device_id_, *pose, params_.pose_loss, params_.pose_target_frame, + params_.position_indices, params_.orientation_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); } // Handle the twist data - common::processTwistWithCovariance( - name(), - device_id_, - twist, - params_.linear_velocity_loss, - params_.angular_velocity_loss, - params_.twist_target_frame, - params_.linear_velocity_indices, - params_.angular_velocity_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + common::processTwistWithCovariance(name(), device_id_, twist, params_.linear_velocity_loss, + params_.angular_velocity_loss, params_.twist_target_frame, + params_.linear_velocity_indices, params_.angular_velocity_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); // Send the transaction object to the plugin's parent sendTransaction(transaction); } -void Odometry2D::processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, const bool validate, - fuse_core::Transaction & transaction) +void Odometry2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); transformed_pose->header.frame_id = - params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; + params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; - if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform pose message with stamp " - << rclcpp::Time( - pose.header.stamp).nanoseconds() << " to pose target frame " << params_.pose_target_frame); + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time(pose.header.stamp).nanoseconds() << " to pose target frame " + << params_.pose_target_frame); return; } - if (!previous_pose_) { + if (!previous_pose_) + { previous_pose_ = std::move(transformed_pose); return; } - if (params_.use_twist_covariance) { + if (params_.use_twist_covariance) + { geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; transformed_twist.header.frame_id = - params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - - if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform twist message with stamp " << rclcpp::Time( - twist.header.stamp).nanoseconds() - << " to twist target frame " << - params_.twist_target_frame); - } else { - common::processDifferentialPoseWithTwistCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - transformed_twist, - params_.minimum_pose_relative_covariance, - params_.twist_covariance_offset, - params_.pose_loss, - params_.position_indices, - params_.orientation_indices, - validate, - transaction); + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " + << rclcpp::Time(twist.header.stamp).nanoseconds() << " to twist target frame " + << params_.twist_target_frame); + } + else + { + common::processDifferentialPoseWithTwistCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + transformed_twist, params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, params_.pose_loss, + params_.position_indices, params_.orientation_indices, + validate, transaction); } - } else { - common::processDifferentialPoseWithCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - params_.independent, - params_.minimum_pose_relative_covariance, - params_.pose_loss, - params_.position_indices, - params_.orientation_indices, - validate, - transaction); + } + else + { + common::processDifferentialPoseWithCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + params_.independent, params_.minimum_pose_relative_covariance, + params_.pose_loss, params_.position_indices, + params_.orientation_indices, validate, transaction); } previous_pose_ = std::move(transformed_pose); diff --git a/fuse_models/src/odometry_2d_publisher.cpp b/fuse_models/src/odometry_2d_publisher.cpp index a4f108e4a..e8a67f04e 100644 --- a/fuse_models/src/odometry_2d_publisher.cpp +++ b/fuse_models/src/odometry_2d_publisher.cpp @@ -61,17 +61,16 @@ namespace fuse_models { Odometry2DPublisher::Odometry2DPublisher() -: fuse_core::AsyncPublisher(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - latest_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)), - latest_covariance_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) + : fuse_core::AsyncPublisher(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , latest_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) + , latest_covariance_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { } void Odometry2DPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -89,54 +88,45 @@ void Odometry2DPublisher::onInit() params_.loadFromROS(interfaces_, name_); - if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) + { tf_buffer_ = std::make_unique( - clock_, - params_.tf_cache_time.to_chrono() - // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h - // TODO(methylDragon): See above ^ + clock_, params_.tf_cache_time.to_chrono() + // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h + // TODO(methylDragon): See above ^ ); - 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()); + 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()); } // Advertise the topics rclcpp::PublisherOptions pub_options; pub_options.callback_group = cb_group_; - odom_pub_ = rclcpp::create_publisher( - interfaces_, - params_.topic, - params_.queue_size, - pub_options); + odom_pub_ = + rclcpp::create_publisher(interfaces_, params_.topic, params_.queue_size, pub_options); acceleration_pub_ = rclcpp::create_publisher( - interfaces_, - params_.acceleration_topic, - params_.queue_size, - pub_options); + interfaces_, params_.acceleration_topic, params_.queue_size, pub_options); } -void Odometry2DPublisher::notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) +void Odometry2DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) { // Find the most recent common timestamp const auto latest_stamp = synchronizer_.findLatestCommonStamp(*transaction, *graph); - if (0u == latest_stamp.nanoseconds()) { + if (0u == latest_stamp.nanoseconds()) + { { std::lock_guard lock(mutex_); latest_stamp_ = latest_stamp; } - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a matching set of state variables with device id '" - << device_id_ << "'."); + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a matching set of state variables with device id '" << device_id_ + << "'."); return; } @@ -149,17 +139,8 @@ void Odometry2DPublisher::notifyCallback( nav_msgs::msg::Odometry odom_output; geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; - if (!getState( - *graph, - latest_stamp, - device_id_, - position_uuid, - orientation_uuid, - velocity_linear_uuid, - velocity_angular_uuid, - acceleration_linear_uuid, - odom_output, - acceleration_output)) + if (!getState(*graph, latest_stamp, device_id_, position_uuid, orientation_uuid, velocity_linear_uuid, + velocity_angular_uuid, acceleration_linear_uuid, odom_output, acceleration_output)) { std::lock_guard lock(mutex_); latest_stamp_ = latest_stamp; @@ -176,14 +157,16 @@ void Odometry2DPublisher::notifyCallback( // Don't waste CPU computing the covariance if nobody is listening rclcpp::Time latest_covariance_stamp = latest_covariance_stamp_; bool latest_covariance_valid = latest_covariance_valid_; - if (odom_pub_->get_subscription_count() > 0 || acceleration_pub_->get_subscription_count() > 0) { + if (odom_pub_->get_subscription_count() > 0 || acceleration_pub_->get_subscription_count() > 0) + { // Throttle covariance computation if (params_.covariance_throttle_period.nanoseconds() == 0 || - latest_stamp - latest_covariance_stamp > params_.covariance_throttle_period) + latest_stamp - latest_covariance_stamp > params_.covariance_throttle_period) { latest_covariance_stamp = latest_stamp; - try { + try + { std::vector> covariance_requests; covariance_requests.emplace_back(position_uuid, position_uuid); covariance_requests.emplace_back(position_uuid, orientation_uuid); @@ -222,22 +205,21 @@ void Odometry2DPublisher::notifyCallback( acceleration_output.accel.covariance[7] = covariance_matrices[6][3]; latest_covariance_valid = true; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM( - logger_, - "An error occurred computing the covariance information for " - << latest_stamp.nanoseconds() - << ". The covariance will be set to zero.\n" - << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM(logger_, "An error occurred computing the covariance information for " + << latest_stamp.nanoseconds() << ". The covariance will be set to zero.\n" + << e.what()); std::fill(odom_output.pose.covariance.begin(), odom_output.pose.covariance.end(), 0.0); std::fill(odom_output.twist.covariance.begin(), odom_output.twist.covariance.end(), 0.0); - std::fill( - acceleration_output.accel.covariance.begin(), - acceleration_output.accel.covariance.end(), 0.0); + std::fill(acceleration_output.accel.covariance.begin(), acceleration_output.accel.covariance.end(), 0.0); latest_covariance_valid = false; } - } else { + } + else + { // This covariance computation cycle has been skipped, so simply take the last covariance // computed // @@ -269,13 +251,9 @@ void Odometry2DPublisher::onStart() acceleration_output_ = geometry_msgs::msg::AccelWithCovarianceStamped(); // TODO(CH3): Add this to a separate callback group for async behavior - publish_timer_ = rclcpp::create_timer( - interfaces_, - clock_, - std::chrono::duration(1.0 / params_.publish_frequency), - std::move(std::bind(&Odometry2DPublisher::publishTimerCallback, this)), - cb_group_ - ); + publish_timer_ = + rclcpp::create_timer(interfaces_, clock_, std::chrono::duration(1.0 / params_.publish_frequency), + std::move(std::bind(&Odometry2DPublisher::publishTimerCallback, this)), cb_group_); delayed_throttle_filter_.reset(); } @@ -285,39 +263,33 @@ void Odometry2DPublisher::onStop() publish_timer_->cancel(); } -bool Odometry2DPublisher::getState( - const fuse_core::Graph & graph, - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id, - fuse_core::UUID & position_uuid, - fuse_core::UUID & orientation_uuid, - fuse_core::UUID & velocity_linear_uuid, - fuse_core::UUID & velocity_angular_uuid, - fuse_core::UUID & acceleration_linear_uuid, - nav_msgs::msg::Odometry & odometry, - geometry_msgs::msg::AccelWithCovarianceStamped & acceleration) +bool Odometry2DPublisher::getState(const fuse_core::Graph& graph, const rclcpp::Time& stamp, + const fuse_core::UUID& device_id, fuse_core::UUID& position_uuid, + fuse_core::UUID& orientation_uuid, fuse_core::UUID& velocity_linear_uuid, + fuse_core::UUID& velocity_angular_uuid, fuse_core::UUID& acceleration_linear_uuid, + nav_msgs::msg::Odometry& odometry, + geometry_msgs::msg::AccelWithCovarianceStamped& acceleration) { - try { + try + { position_uuid = fuse_variables::Position2DStamped(stamp, device_id).uuid(); - auto position_variable = dynamic_cast( - graph.getVariable(position_uuid)); + auto position_variable = dynamic_cast(graph.getVariable(position_uuid)); orientation_uuid = fuse_variables::Orientation2DStamped(stamp, device_id).uuid(); - auto orientation_variable = dynamic_cast( - graph.getVariable(orientation_uuid)); + auto orientation_variable = + dynamic_cast(graph.getVariable(orientation_uuid)); velocity_linear_uuid = fuse_variables::VelocityLinear2DStamped(stamp, device_id).uuid(); - auto velocity_linear_variable = dynamic_cast( - graph.getVariable(velocity_linear_uuid)); + auto velocity_linear_variable = + dynamic_cast(graph.getVariable(velocity_linear_uuid)); velocity_angular_uuid = fuse_variables::VelocityAngular2DStamped(stamp, device_id).uuid(); - auto velocity_angular_variable = dynamic_cast( - graph.getVariable(velocity_angular_uuid)); + auto velocity_angular_variable = + dynamic_cast(graph.getVariable(velocity_angular_uuid)); acceleration_linear_uuid = fuse_variables::AccelerationLinear2DStamped(stamp, device_id).uuid(); auto acceleration_linear_variable = - dynamic_cast( - graph.getVariable(acceleration_linear_uuid)); + dynamic_cast(graph.getVariable(acceleration_linear_uuid)); odometry.pose.pose.position.x = position_variable.x(); odometry.pose.pose.position.y = position_variable.y(); @@ -336,15 +308,17 @@ bool Odometry2DPublisher::getState( acceleration.accel.accel.angular.x = 0.0; acceleration.accel.accel.angular.y = 0.0; acceleration.accel.accel.angular.z = 0.0; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); return false; - } catch (...) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a state at time " << stamp.nanoseconds() << ". Error: unknown"); + } + catch (...) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: unknown"); return false; } @@ -368,10 +342,10 @@ void Odometry2DPublisher::publishTimerCallback() acceleration_output = acceleration_output_; } - if (0u == latest_stamp.nanoseconds()) { - RCLCPP_WARN_STREAM_EXPRESSION( - logger_, delayed_throttle_filter_.isEnabled(), - "No valid state data yet. Delaying tf broadcast."); + if (0u == latest_stamp.nanoseconds()) + { + RCLCPP_WARN_STREAM_EXPRESSION(logger_, delayed_throttle_filter_.isEnabled(), + "No valid state data yet. Delaying tf broadcast."); return; } @@ -379,7 +353,8 @@ void Odometry2DPublisher::publishTimerCallback() tf2::fromMsg(odom_output.pose.pose, pose); // If requested, we need to project our state forward in time using the 2D kinematic model - if (params_.predict_to_current_time) { + if (params_.predict_to_current_time) + { rclcpp::Time timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); tf2_2d::Vector2 velocity_linear; tf2::fromMsg(odom_output.twist.twist.linear, velocity_linear); @@ -389,23 +364,15 @@ void Odometry2DPublisher::publishTimerCallback() fuse_core::Matrix8d jacobian; tf2_2d::Vector2 acceleration_linear; - if (params_.predict_with_acceleration) { + if (params_.predict_with_acceleration) + { tf2::fromMsg(acceleration_output.accel.accel.linear, acceleration_linear); } double yaw_vel; - predict( - pose, - velocity_linear, - odom_output.twist.twist.angular.z, - acceleration_linear, - dt, - pose, - velocity_linear, - yaw_vel, - acceleration_linear, - jacobian); + predict(pose, velocity_linear, odom_output.twist.twist.angular.z, acceleration_linear, dt, pose, velocity_linear, + yaw_vel, acceleration_linear, jacobian); odom_output.pose.pose.position.x = pose.getX(); odom_output.pose.pose.position.y = pose.getY(); @@ -415,7 +382,8 @@ void Odometry2DPublisher::publishTimerCallback() odom_output.twist.twist.linear.y = velocity_linear.y(); odom_output.twist.twist.angular.z = yaw_vel; - if (params_.predict_with_acceleration) { + if (params_.predict_with_acceleration) + { acceleration_output.accel.accel.linear.x = acceleration_linear.x(); acceleration_output.accel.accel.linear.y = acceleration_linear.y(); } @@ -425,7 +393,8 @@ void Odometry2DPublisher::publishTimerCallback() // Either the last covariance computation was skipped because there was no subscriber, // or it failed - if (latest_covariance_valid) { + if (latest_covariance_valid) + { fuse_core::Matrix8d covariance; covariance(0, 0) = odom_output.pose.covariance[0]; covariance(0, 1) = odom_output.pose.covariance[1]; @@ -463,10 +432,10 @@ void Odometry2DPublisher::publishTimerCallback() covariance = jacobian * covariance * jacobian.transpose(); auto process_noise_covariance = params_.process_noise_covariance; - if (params_.scale_process_noise) { - common::scaleProcessNoiseCovariance( - process_noise_covariance, velocity_linear, - odom_output.twist.twist.angular.z, params_.velocity_norm_min); + if (params_.scale_process_noise) + { + common::scaleProcessNoiseCovariance(process_noise_covariance, velocity_linear, + odom_output.twist.twist.angular.z, params_.velocity_norm_min); } covariance.noalias() += dt * process_noise_covariance; @@ -501,11 +470,13 @@ void Odometry2DPublisher::publishTimerCallback() odom_pub_->publish(odom_output); acceleration_pub_->publish(acceleration_output); - if (params_.publish_tf) { + if (params_.publish_tf) + { auto frame_id = odom_output.header.frame_id; auto child_frame_id = odom_output.child_frame_id; - if (params_.invert_tf) { + if (params_.invert_tf) + { pose = pose.inverse(); std::swap(frame_id, child_frame_id); } @@ -519,23 +490,24 @@ void Odometry2DPublisher::publishTimerCallback() trans.transform.translation.z = odom_output.pose.pose.position.z; trans.transform.rotation = tf2::toMsg(pose.getRotation()); - if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { - try { - auto base_to_odom = tf_buffer_->lookupTransform( - params_.base_link_frame_id, - params_.odom_frame_id, - trans.header.stamp, - params_.tf_timeout); + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) + { + try + { + auto base_to_odom = tf_buffer_->lookupTransform(params_.base_link_frame_id, params_.odom_frame_id, + trans.header.stamp, params_.tf_timeout); geometry_msgs::msg::TransformStamped map_to_odom; tf2::doTransform(base_to_odom, map_to_odom, trans); map_to_odom.child_frame_id = params_.odom_frame_id; trans = map_to_odom; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Could not lookup the " << params_.base_link_frame_id << "->" - << params_.odom_frame_id << " transform. Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Could not lookup the " << params_.base_link_frame_id << "->" + << params_.odom_frame_id + << " transform. Error: " << e.what()); return; } diff --git a/fuse_models/src/pose_2d.cpp b/fuse_models/src/pose_2d.cpp index 0066ee7b5..53ae1ce51 100644 --- a/fuse_models/src/pose_2d.cpp +++ b/fuse_models/src/pose_2d.cpp @@ -49,17 +49,15 @@ namespace fuse_models { Pose2D::Pose2D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Pose2D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Pose2D::process, this, std::placeholders::_1)) { } -void Pose2D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Pose2D::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); @@ -77,49 +75,36 @@ void Pose2D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } - if (params_.position_indices.empty() && - params_.orientation_indices.empty()) + if (params_.position_indices.empty() && params_.orientation_indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " << params_.topic - << " will be ignored."); + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << params_.topic << " will 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() - ); + 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 Pose2D::onStart() { - if (!params_.position_indices.empty() || - !params_.orientation_indices.empty()) + if (!params_.position_indices.empty() || !params_.orientation_indices.empty()) { rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &PoseThrottledCallback::callback< - const geometry_msgs::msg::PoseWithCovarianceStamped &>, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&PoseThrottledCallback::callback, + &throttled_callback_, std::placeholders::_1), + sub_options); } } @@ -128,7 +113,7 @@ void Pose2D::onStop() sub_.reset(); } -void Pose2D::process(const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +void Pose2D::process(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -136,56 +121,42 @@ void Pose2D::process(const geometry_msgs::msg::PoseWithCovarianceStamped & msg) const bool validate = !params_.disable_checks; - if (params_.differential) { + if (params_.differential) + { processDifferential(msg, validate, *transaction); - } else { - common::processAbsolutePoseWithCovariance( - name(), - device_id_, - msg, - params_.loss, - params_.target_frame, - params_.position_indices, - params_.orientation_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + } + else + { + common::processAbsolutePoseWithCovariance(name(), device_id_, msg, params_.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); } -void Pose2D::processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const bool validate, - fuse_core::Transaction & transaction) +void Pose2D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, const bool validate, + fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); - transformed_pose->header.frame_id = - params_.target_frame.empty() ? pose.header.frame_id : params_.target_frame; - - if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform pose message with stamp " << rclcpp::Time(pose.header.stamp).nanoseconds() - << " to target frame " << params_.target_frame); + transformed_pose->header.frame_id = params_.target_frame.empty() ? pose.header.frame_id : params_.target_frame; + + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time(pose.header.stamp).nanoseconds() << " to target frame " + << params_.target_frame); return; } - if (previous_pose_msg_) { - common::processDifferentialPoseWithCovariance( - name(), - device_id_, - *previous_pose_msg_, - *transformed_pose, - params_.independent, - params_.minimum_pose_relative_covariance, - params_.loss, - params_.position_indices, - params_.orientation_indices, - validate, - transaction); + if (previous_pose_msg_) + { + common::processDifferentialPoseWithCovariance(name(), device_id_, *previous_pose_msg_, *transformed_pose, + params_.independent, params_.minimum_pose_relative_covariance, + params_.loss, params_.position_indices, params_.orientation_indices, + validate, transaction); } previous_pose_msg_ = std::move(transformed_pose); diff --git a/fuse_models/src/transaction.cpp b/fuse_models/src/transaction.cpp index 635c6baca..dc3392315 100644 --- a/fuse_models/src/transaction.cpp +++ b/fuse_models/src/transaction.cpp @@ -42,15 +42,12 @@ PLUGINLIB_EXPORT_CLASS(fuse_models::Transaction, fuse_core::SensorModel) namespace fuse_models { -Transaction::Transaction() -: fuse_core::AsyncSensorModel(1) +Transaction::Transaction() : fuse_core::AsyncSensorModel(1) { } -void Transaction::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Transaction::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); @@ -68,12 +65,8 @@ void Transaction::onStart() sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - fuse_core::joinTopicName(name_, params_.topic), - params_.queue_size, - std::bind(&Transaction::process, this, std::placeholders::_1), - sub_options - ); + interfaces_, fuse_core::joinTopicName(name_, params_.topic), params_.queue_size, + std::bind(&Transaction::process, this, std::placeholders::_1), sub_options); } void Transaction::onStop() @@ -81,7 +74,7 @@ void Transaction::onStop() sub_.reset(); } -void Transaction::process(const fuse_msgs::msg::SerializedTransaction & msg) +void Transaction::process(const fuse_msgs::msg::SerializedTransaction& msg) { // Deserialize and send the transaction to the plugin's parent sendTransaction(transaction_deserializer_.deserialize(msg)->clone()); diff --git a/fuse_models/src/twist_2d.cpp b/fuse_models/src/twist_2d.cpp index b244971e5..83bf129de 100644 --- a/fuse_models/src/twist_2d.cpp +++ b/fuse_models/src/twist_2d.cpp @@ -46,17 +46,15 @@ namespace fuse_models { Twist2D::Twist2D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Twist2D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Twist2D::process, this, std::placeholders::_1)) { } -void Twist2D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Twist2D::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); @@ -74,49 +72,36 @@ void Twist2D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } - if (params_.linear_indices.empty() && - params_.angular_indices.empty()) + if (params_.linear_indices.empty() && params_.angular_indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " << params_.topic - << " will be ignored."); + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << params_.topic << " will 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() - ); + 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 Twist2D::onStart() { - if (!params_.linear_indices.empty() || - !params_.angular_indices.empty()) + if (!params_.linear_indices.empty() || !params_.angular_indices.empty()) { rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &TwistThrottledCallback::callback< - const geometry_msgs::msg::TwistWithCovarianceStamped &>, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&TwistThrottledCallback::callback, + &throttled_callback_, std::placeholders::_1), + sub_options); } } @@ -125,25 +110,15 @@ void Twist2D::onStop() sub_.reset(); } -void Twist2D::process(const geometry_msgs::msg::TwistWithCovarianceStamped & msg) +void Twist2D::process(const geometry_msgs::msg::TwistWithCovarianceStamped& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); transaction->stamp(msg.header.stamp); - common::processTwistWithCovariance( - name(), - device_id_, - msg, - params_.linear_loss, - params_.angular_loss, - params_.target_frame, - params_.linear_indices, - params_.angular_indices, - *tf_buffer_, - !params_.disable_checks, - *transaction, - params_.tf_timeout); + common::processTwistWithCovariance(name(), device_id_, msg, params_.linear_loss, params_.angular_loss, + params_.target_frame, params_.linear_indices, params_.angular_indices, *tf_buffer_, + !params_.disable_checks, *transaction, params_.tf_timeout); // Send the transaction object to the plugin's parent sendTransaction(transaction); diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 00f71e18a..9930b921a 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -65,25 +65,24 @@ PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle2D, fuse_core::MotionModel) namespace std { -inline bool isfinite(const tf2_2d::Vector2 & vector) +inline bool isfinite(const tf2_2d::Vector2& vector) { return std::isfinite(vector.x()) && std::isfinite(vector.y()); } -inline bool isfinite(const tf2_2d::Transform & transform) +inline bool isfinite(const tf2_2d::Transform& transform) { - return std::isfinite(transform.x()) && std::isfinite(transform.y()) && std::isfinite( - transform.yaw()); + return std::isfinite(transform.x()) && std::isfinite(transform.y()) && std::isfinite(transform.yaw()); } -std::string to_string(const tf2_2d::Vector2 & vector) +std::string to_string(const tf2_2d::Vector2& vector) { std::ostringstream oss; oss << vector; return oss.str(); } -std::string to_string(const tf2_2d::Transform & transform) +std::string to_string(const tf2_2d::Transform& transform) { std::ostringstream oss; oss << transform; @@ -95,21 +94,20 @@ std::string to_string(const tf2_2d::Transform & transform) namespace fuse_core { -template -inline void validateCovariance( - const Eigen::DenseBase & covariance, - const double precision = Eigen::NumTraits::dummy_precision()) +template +inline void validateCovariance(const Eigen::DenseBase& covariance, + const double precision = Eigen::NumTraits::dummy_precision()) { - if (!fuse_core::isSymmetric(covariance, precision)) { - throw std::runtime_error( - "Non-symmetric partial covariance matrix\n" + - fuse_core::to_string(covariance, Eigen::FullPrecision)); + if (!fuse_core::isSymmetric(covariance, precision)) + { + throw std::runtime_error("Non-symmetric partial covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); } - if (!fuse_core::isPositiveDefinite(covariance)) { - throw std::runtime_error( - "Non-positive-definite partial covariance matrix\n" + - fuse_core::to_string(covariance, Eigen::FullPrecision)); + if (!fuse_core::isPositiveDefinite(covariance)) + { + throw std::runtime_error("Non-positive-definite partial covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); } } @@ -119,24 +117,25 @@ namespace fuse_models { Unicycle2D::Unicycle2D() -: fuse_core::AsyncMotionModel(1), - logger_(rclcpp::get_logger("uninitialized")), - buffer_length_(rclcpp::Duration::max()), - device_id_(fuse_core::uuid::NIL), - timestamp_manager_(&Unicycle2D::generateMotionModel, this, rclcpp::Duration::max()) + : fuse_core::AsyncMotionModel(1) + , logger_(rclcpp::get_logger("uninitialized")) + , buffer_length_(rclcpp::Duration::max()) + , device_id_(fuse_core::uuid::NIL) + , timestamp_manager_(&Unicycle2D::generateMotionModel, this, rclcpp::Duration::max()) { } -void Unicycle2D::print(std::ostream & stream) const +void Unicycle2D::print(std::ostream& stream) const { stream << "state history:\n"; - for (const auto & state : state_history_) { + for (const auto& state : state_history_) + { stream << "- stamp: " << state.first.nanoseconds() << "\n"; state.second.print(stream); } } -void Unicycle2D::StateHistoryElement::print(std::ostream & stream) const +void Unicycle2D::StateHistoryElement::print(std::ostream& stream) const { stream << " position uuid: " << position_uuid << "\n" << " yaw uuid: " << yaw_uuid << "\n" @@ -151,34 +150,40 @@ void Unicycle2D::StateHistoryElement::print(std::ostream & stream) const void Unicycle2D::StateHistoryElement::validate() const { - if (!std::isfinite(pose)) { + if (!std::isfinite(pose)) + { throw std::runtime_error("Invalid pose " + std::to_string(pose)); } - if (!std::isfinite(velocity_linear)) { + if (!std::isfinite(velocity_linear)) + { throw std::runtime_error("Invalid linear velocity " + std::to_string(velocity_linear)); } - if (!std::isfinite(velocity_yaw)) { + if (!std::isfinite(velocity_yaw)) + { throw std::runtime_error("Invalid yaw velocity " + std::to_string(velocity_yaw)); } - if (!std::isfinite(acceleration_linear)) { + if (!std::isfinite(acceleration_linear)) + { throw std::runtime_error("Invalid linear acceleration " + std::to_string(acceleration_linear)); } } -bool Unicycle2D::applyCallback(fuse_core::Transaction & transaction) +bool Unicycle2D::applyCallback(fuse_core::Transaction& transaction) { // Use the timestamp manager to generate just the required motion model segments. The timestamp // manager, in turn, makes calls to the generateMotionModel() function. - try { + try + { // Now actually generate the motion model segments timestamp_manager_.query(transaction, true); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "An error occurred while completing the motion model query. Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "An error occurred while completing the motion model query. Error: " << e.what()); return false; } return true; @@ -189,9 +194,8 @@ void Unicycle2D::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) updateStateHistoryEstimates(*graph, state_history_, buffer_length_); } -void Unicycle2D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void Unicycle2D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncMotionModel::initialize(interfaces, name); @@ -203,55 +207,33 @@ void Unicycle2D::onInit() clock_ = interfaces_.get_node_clock_interface()->get_clock(); std::vector process_noise_diagonal; - process_noise_diagonal = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "process_noise_diagonal"), - process_noise_diagonal); - - if (process_noise_diagonal.size() != 8) { + process_noise_diagonal = fuse_core::getParam( + interfaces_, fuse_core::joinParameterName(name_, "process_noise_diagonal"), process_noise_diagonal); + + if (process_noise_diagonal.size() != 8) + { throw std::runtime_error("Process noise diagonal must be of length 8!"); } process_noise_covariance_ = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal(); - scale_process_noise_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "scale_process_noise"), - scale_process_noise_); + scale_process_noise_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "scale_process_noise"), + scale_process_noise_); velocity_norm_min_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "velocity_norm_min"), - velocity_norm_min_); + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "velocity_norm_min"), velocity_norm_min_); disable_checks_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "disable_checks"), - disable_checks_); + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "disable_checks"), disable_checks_); double buffer_length = 3.0; - buffer_length = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "buffer_length"), - buffer_length); - - if (buffer_length < 0.0) { - throw std::runtime_error( - "Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); + buffer_length = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "buffer_length"), buffer_length); + + if (buffer_length < 0.0) + { + throw std::runtime_error("Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); } - buffer_length_ = - (buffer_length == - 0.0) ? rclcpp::Duration::max() : rclcpp::Duration::from_seconds(buffer_length); + buffer_length_ = (buffer_length == 0.0) ? rclcpp::Duration::max() : rclcpp::Duration::from_seconds(buffer_length); timestamp_manager_.bufferLength(buffer_length_); device_id_ = fuse_variables::loadDeviceId(interfaces_); @@ -263,29 +245,27 @@ void Unicycle2D::onStart() state_history_.clear(); } -void Unicycle2D::generateMotionModel( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables) +void Unicycle2D::generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables) { - assert( - beginning_stamp < ending_stamp || - (beginning_stamp == ending_stamp && state_history_.empty())); + assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history_.empty())); StateHistoryElement base_state; - rclcpp::Time base_time{0, 0, RCL_ROS_TIME}; + rclcpp::Time base_time{ 0, 0, RCL_ROS_TIME }; // Find an entry that is > beginning_stamp // The entry that is <= will be the one before it auto base_state_pair_it = state_history_.upper_bound(beginning_stamp); - if (base_state_pair_it == state_history_.begin()) { - RCLCPP_WARN_STREAM_EXPRESSION( - logger_, !state_history_.empty(), - "Unable to locate a state in this history with stamp <= " - << beginning_stamp.nanoseconds() << ". Variables will all be initialized to 0."); + if (base_state_pair_it == state_history_.begin()) + { + RCLCPP_WARN_STREAM_EXPRESSION(logger_, !state_history_.empty(), + "Unable to locate a state in this history with stamp <= " + << beginning_stamp.nanoseconds() << ". Variables will all be initialized to 0."); base_time = beginning_stamp; - } else { + } + else + { --base_state_pair_it; base_time = base_state_pair_it->first; base_state = base_state_pair_it->second; @@ -295,33 +275,27 @@ void Unicycle2D::generateMotionModel( // If the nearest state we had was before the beginning stamp, we need to project that state to // the beginning stamp - if (base_time != beginning_stamp) { - predict( - base_state.pose, - base_state.velocity_linear, - base_state.velocity_yaw, - base_state.acceleration_linear, - (beginning_stamp - base_time).seconds(), - state1.pose, - state1.velocity_linear, - state1.velocity_yaw, - state1.acceleration_linear); - } else { + if (base_time != beginning_stamp) + { + predict(base_state.pose, base_state.velocity_linear, base_state.velocity_yaw, base_state.acceleration_linear, + (beginning_stamp - base_time).seconds(), state1.pose, state1.velocity_linear, state1.velocity_yaw, + state1.acceleration_linear); + } + else + { state1 = base_state; } // If dt is zero, we only need to update the state history: const double dt = (ending_stamp - beginning_stamp).seconds(); - if (dt == 0.0) { + if (dt == 0.0) + { state1.position_uuid = fuse_variables::Position2DStamped(beginning_stamp, device_id_).uuid(); state1.yaw_uuid = fuse_variables::Orientation2DStamped(beginning_stamp, device_id_).uuid(); - state1.vel_linear_uuid = - fuse_variables::VelocityLinear2DStamped(beginning_stamp, device_id_).uuid(); - state1.vel_yaw_uuid = - fuse_variables::VelocityAngular2DStamped(beginning_stamp, device_id_).uuid(); - state1.acc_linear_uuid = - fuse_variables::AccelerationLinear2DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_linear_uuid = fuse_variables::VelocityLinear2DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_yaw_uuid = fuse_variables::VelocityAngular2DStamped(beginning_stamp, device_id_).uuid(); + state1.acc_linear_uuid = fuse_variables::AccelerationLinear2DStamped(beginning_stamp, device_id_).uuid(); state_history_.emplace(beginning_stamp, std::move(state1)); @@ -330,39 +304,20 @@ void Unicycle2D::generateMotionModel( // Now predict to get an initial guess for the state at the ending stamp StateHistoryElement state2; - predict( - state1.pose, - state1.velocity_linear, - state1.velocity_yaw, - state1.acceleration_linear, - dt, - state2.pose, - state2.velocity_linear, - state2.velocity_yaw, - state2.acceleration_linear); + predict(state1.pose, state1.velocity_linear, state1.velocity_yaw, state1.acceleration_linear, dt, state2.pose, + state2.velocity_linear, state2.velocity_yaw, state2.acceleration_linear); // Define the fuse variables required for this constraint auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id_); auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(beginning_stamp, device_id_); - auto velocity_linear1 = fuse_variables::VelocityLinear2DStamped::make_shared( - beginning_stamp, - device_id_); - auto velocity_yaw1 = fuse_variables::VelocityAngular2DStamped::make_shared( - beginning_stamp, - device_id_); - auto acceleration_linear1 = fuse_variables::AccelerationLinear2DStamped::make_shared( - beginning_stamp, device_id_); + auto velocity_linear1 = fuse_variables::VelocityLinear2DStamped::make_shared(beginning_stamp, device_id_); + auto velocity_yaw1 = fuse_variables::VelocityAngular2DStamped::make_shared(beginning_stamp, device_id_); + auto acceleration_linear1 = fuse_variables::AccelerationLinear2DStamped::make_shared(beginning_stamp, device_id_); auto position2 = fuse_variables::Position2DStamped::make_shared(ending_stamp, device_id_); auto yaw2 = fuse_variables::Orientation2DStamped::make_shared(ending_stamp, device_id_); - auto velocity_linear2 = fuse_variables::VelocityLinear2DStamped::make_shared( - ending_stamp, - device_id_); - auto velocity_yaw2 = fuse_variables::VelocityAngular2DStamped::make_shared( - ending_stamp, - device_id_); - auto acceleration_linear2 = fuse_variables::AccelerationLinear2DStamped::make_shared( - ending_stamp, - device_id_); + auto velocity_linear2 = fuse_variables::VelocityLinear2DStamped::make_shared(ending_stamp, device_id_); + auto velocity_yaw2 = fuse_variables::VelocityAngular2DStamped::make_shared(ending_stamp, device_id_); + auto acceleration_linear2 = fuse_variables::AccelerationLinear2DStamped::make_shared(ending_stamp, device_id_); position1->data()[fuse_variables::Position2DStamped::X] = state1.pose.x(); position1->data()[fuse_variables::Position2DStamped::Y] = state1.pose.y(); @@ -370,20 +325,16 @@ void Unicycle2D::generateMotionModel( velocity_linear1->data()[fuse_variables::VelocityLinear2DStamped::X] = state1.velocity_linear.x(); velocity_linear1->data()[fuse_variables::VelocityLinear2DStamped::Y] = state1.velocity_linear.y(); velocity_yaw1->data()[fuse_variables::VelocityAngular2DStamped::YAW] = state1.velocity_yaw; - acceleration_linear1->data()[fuse_variables::AccelerationLinear2DStamped::X] = - state1.acceleration_linear.x(); - acceleration_linear1->data()[fuse_variables::AccelerationLinear2DStamped::Y] = - state1.acceleration_linear.y(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear2DStamped::X] = state1.acceleration_linear.x(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear2DStamped::Y] = state1.acceleration_linear.y(); position2->data()[fuse_variables::Position2DStamped::X] = state2.pose.x(); position2->data()[fuse_variables::Position2DStamped::Y] = state2.pose.y(); yaw2->data()[fuse_variables::Orientation2DStamped::YAW] = state2.pose.yaw(); velocity_linear2->data()[fuse_variables::VelocityLinear2DStamped::X] = state2.velocity_linear.x(); velocity_linear2->data()[fuse_variables::VelocityLinear2DStamped::Y] = state2.velocity_linear.y(); velocity_yaw2->data()[fuse_variables::VelocityAngular2DStamped::YAW] = state2.velocity_yaw; - acceleration_linear2->data()[fuse_variables::AccelerationLinear2DStamped::X] = - state2.acceleration_linear.x(); - acceleration_linear2->data()[fuse_variables::AccelerationLinear2DStamped::Y] = - state2.acceleration_linear.y(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear2DStamped::X] = state2.acceleration_linear.x(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear2DStamped::Y] = state2.acceleration_linear.y(); state1.position_uuid = position1->uuid(); state1.yaw_uuid = yaw1->uuid(); @@ -401,40 +352,33 @@ void Unicycle2D::generateMotionModel( // Scale process noise covariance pose by the norm of the current state twist auto process_noise_covariance = process_noise_covariance_; - if (scale_process_noise_) { - common::scaleProcessNoiseCovariance( - process_noise_covariance, state1.velocity_linear, state1.velocity_yaw, - velocity_norm_min_); + if (scale_process_noise_) + { + common::scaleProcessNoiseCovariance(process_noise_covariance, state1.velocity_linear, state1.velocity_yaw, + velocity_norm_min_); } // Validate process_noise_covariance *= dt; - if (!disable_checks_) { - try { + if (!disable_checks_) + { + try + { validateMotionModel(state1, state2, process_noise_covariance); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Invalid '" << name_ << "' motion model: " << ex.what()); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Invalid '" << name_ << "' motion model: " << ex.what()); return; } } // Create the constraints for this motion model segment auto constraint = fuse_models::Unicycle2DStateKinematicConstraint::make_shared( - name(), - *position1, - *yaw1, - *velocity_linear1, - *velocity_yaw1, - *acceleration_linear1, - *position2, - *yaw2, - *velocity_linear2, - *velocity_yaw2, - *acceleration_linear2, - process_noise_covariance); + name(), *position1, *yaw1, *velocity_linear1, *velocity_yaw1, *acceleration_linear1, *position2, *yaw2, + *velocity_linear2, *velocity_yaw2, *acceleration_linear2, process_noise_covariance); // Update the output variables constraints.push_back(constraint); @@ -450,22 +394,24 @@ void Unicycle2D::generateMotionModel( variables.push_back(acceleration_linear2); } -void Unicycle2D::updateStateHistoryEstimates( - const fuse_core::Graph & graph, - StateHistory & state_history, - const rclcpp::Duration & buffer_length) +void Unicycle2D::updateStateHistoryEstimates(const fuse_core::Graph& graph, StateHistory& state_history, + const rclcpp::Duration& buffer_length) { - if (state_history.empty()) { + if (state_history.empty()) + { return; } // Compute the expiration time carefully, as ROS can't handle negative times - const auto & ending_stamp = state_history.rbegin()->first; + const auto& ending_stamp = state_history.rbegin()->first; rclcpp::Time expiration_time; - if (ending_stamp.seconds() > buffer_length.seconds()) { + if (ending_stamp.seconds() > buffer_length.seconds()) + { expiration_time = ending_stamp - buffer_length; - } else { + } + else + { // NOTE(CH3): Uninitialized. But okay because it's just used for comparison. expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type()); } @@ -475,7 +421,8 @@ void Unicycle2D::updateStateHistoryEstimates( // - at least one entry remains at all times // - the history covers *at least* until the expiration time. Longer is acceptable. auto expiration_iter = state_history.upper_bound(expiration_time); - if (expiration_iter != state_history.begin()) { + if (expiration_iter != state_history.begin()) + { // expiration_iter points to the first element > expiration_time. // Back up one entry, to a point that is <= expiration_time state_history.erase(state_history.begin(), std::prev(expiration_iter)); @@ -484,87 +431,79 @@ void Unicycle2D::updateStateHistoryEstimates( // Update the states in the state history with information from the graph // If a state is not in the graph yet, predict the state in question from the closest previous // state - for (auto current_iter = state_history.begin(); current_iter != state_history.end(); - ++current_iter) + for (auto current_iter = state_history.begin(); current_iter != state_history.end(); ++current_iter) { - const auto & current_stamp = current_iter->first; - auto & current_state = current_iter->second; - if (graph.variableExists(current_state.position_uuid) && - graph.variableExists(current_state.yaw_uuid) && - graph.variableExists(current_state.vel_linear_uuid) && - graph.variableExists(current_state.vel_yaw_uuid) && - graph.variableExists(current_state.acc_linear_uuid)) + const auto& current_stamp = current_iter->first; + auto& current_state = current_iter->second; + if (graph.variableExists(current_state.position_uuid) && graph.variableExists(current_state.yaw_uuid) && + graph.variableExists(current_state.vel_linear_uuid) && graph.variableExists(current_state.vel_yaw_uuid) && + graph.variableExists(current_state.acc_linear_uuid)) { // This pose does exist in the graph. Update it directly. - const auto & position = graph.getVariable(current_state.position_uuid); - const auto & yaw = graph.getVariable(current_state.yaw_uuid); - const auto & vel_linear = graph.getVariable(current_state.vel_linear_uuid); - const auto & vel_yaw = graph.getVariable(current_state.vel_yaw_uuid); - const auto & acc_linear = graph.getVariable(current_state.acc_linear_uuid); + const auto& position = graph.getVariable(current_state.position_uuid); + const auto& yaw = graph.getVariable(current_state.yaw_uuid); + const auto& vel_linear = graph.getVariable(current_state.vel_linear_uuid); + const auto& vel_yaw = graph.getVariable(current_state.vel_yaw_uuid); + const auto& acc_linear = graph.getVariable(current_state.acc_linear_uuid); current_state.pose.setX(position.data()[fuse_variables::Position2DStamped::X]); current_state.pose.setY(position.data()[fuse_variables::Position2DStamped::Y]); current_state.pose.setAngle(yaw.data()[fuse_variables::Orientation2DStamped::YAW]); - current_state.velocity_linear.setX( - vel_linear.data()[fuse_variables::VelocityLinear2DStamped:: - X]); - current_state.velocity_linear.setY( - vel_linear.data()[fuse_variables::VelocityLinear2DStamped:: - Y]); + current_state.velocity_linear.setX(vel_linear.data()[fuse_variables::VelocityLinear2DStamped::X]); + current_state.velocity_linear.setY(vel_linear.data()[fuse_variables::VelocityLinear2DStamped::Y]); current_state.velocity_yaw = vel_yaw.data()[fuse_variables::VelocityAngular2DStamped::YAW]; - current_state.acceleration_linear.setX( - acc_linear.data()[fuse_variables:: - AccelerationLinear2DStamped::X]); - current_state.acceleration_linear.setY( - acc_linear.data()[fuse_variables:: - AccelerationLinear2DStamped::Y]); - } else if (current_iter != state_history.begin()) { + current_state.acceleration_linear.setX(acc_linear.data()[fuse_variables::AccelerationLinear2DStamped::X]); + current_state.acceleration_linear.setY(acc_linear.data()[fuse_variables::AccelerationLinear2DStamped::Y]); + } + else if (current_iter != state_history.begin()) + { auto previous_iter = std::prev(current_iter); - const auto & previous_stamp = previous_iter->first; - const auto & previous_state = previous_iter->second; + const auto& previous_stamp = previous_iter->first; + const auto& previous_state = previous_iter->second; // This state is not in the graph yet, so we can't update/correct the value in our state // history. However, the state *before* this one may have been corrected (or one of its // predecessors may have been), so we can use that corrected value, along with our prediction // logic, to provide a more accurate update to this state. - predict( - previous_state.pose, - previous_state.velocity_linear, - previous_state.velocity_yaw, - previous_state.acceleration_linear, - (current_stamp - previous_stamp).seconds(), - current_state.pose, - current_state.velocity_linear, - current_state.velocity_yaw, - current_state.acceleration_linear); + predict(previous_state.pose, previous_state.velocity_linear, previous_state.velocity_yaw, + previous_state.acceleration_linear, (current_stamp - previous_stamp).seconds(), current_state.pose, + current_state.velocity_linear, current_state.velocity_yaw, current_state.acceleration_linear); } } } -void Unicycle2D::validateMotionModel( - const StateHistoryElement & state1, const StateHistoryElement & state2, - const fuse_core::Matrix8d & process_noise_covariance) +void Unicycle2D::validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, + const fuse_core::Matrix8d& process_noise_covariance) { - try { + try + { state1.validate(); - } catch (const std::runtime_error & ex) { + } + catch (const std::runtime_error& ex) + { throw std::runtime_error("Invalid state #1: " + std::string(ex.what())); } - try { + try + { state2.validate(); - } catch (const std::runtime_error & ex) { + } + catch (const std::runtime_error& ex) + { throw std::runtime_error("Invalid state #2: " + std::string(ex.what())); } - try { + try + { fuse_core::validateCovariance(process_noise_covariance); - } catch (const std::runtime_error & ex) { + } + catch (const std::runtime_error& ex) + { throw std::runtime_error("Invalid process noise covariance: " + std::string(ex.what())); } } -std::ostream & operator<<(std::ostream & stream, const Unicycle2D & unicycle_2d) +std::ostream& operator<<(std::ostream& stream, const Unicycle2D& unicycle_2d) { unicycle_2d.print(stream); return stream; diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index c822c2199..1bec4e0fa 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -66,18 +66,16 @@ namespace fuse_models { Unicycle2DIgnition::Unicycle2DIgnition() -: fuse_core::AsyncSensorModel(1), - started_(false), - initial_transaction_sent_(false), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")) + : fuse_core::AsyncSensorModel(1) + , started_(false) + , initial_transaction_sent_(false) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) { } -void Unicycle2DIgnition::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Unicycle2DIgnition::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); @@ -94,52 +92,32 @@ void Unicycle2DIgnition::onInit() params_.loadFromROS(interfaces_, name_); // Connect to the reset service - if (!params_.reset_service.empty()) { + if (!params_.reset_service.empty()) + { reset_client_ = rclcpp::create_client( - interfaces_.get_node_base_interface(), - interfaces_.get_node_graph_interface(), - interfaces_.get_node_services_interface(), - params_.reset_service, - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_graph_interface(), + interfaces_.get_node_services_interface(), params_.reset_service, rclcpp::ServicesQoS(), cb_group_); } // Advertise rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind(&Unicycle2DIgnition::subscriberCallback, this, std::placeholders::_1), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&Unicycle2DIgnition::subscriberCallback, this, std::placeholders::_1), sub_options); set_pose_service_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.set_pose_service), - std::bind( - &Unicycle2DIgnition::setPoseServiceCallback, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3), - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_service), + std::bind(&Unicycle2DIgnition::setPoseServiceCallback, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + rclcpp::ServicesQoS(), cb_group_); set_pose_deprecated_service_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.set_pose_deprecated_service), - std::bind( - &Unicycle2DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3), - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_deprecated_service), + std::bind(&Unicycle2DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3), + rclcpp::ServicesQoS(), cb_group_); } void Unicycle2DIgnition::start() @@ -149,13 +127,13 @@ void Unicycle2DIgnition::start() // TODO(swilliams) Should this be executed every time optimizer.reset() is called, or only once // ever? I feel like it should be "only once ever". Send an initial state // transaction immediately, if requested - if (params_.publish_on_startup && !initial_transaction_sent_) { + if (params_.publish_on_startup && !initial_transaction_sent_) + { auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); pose.header.stamp = clock_->now(); pose.pose.pose.position.x = params_.initial_state[0]; pose.pose.pose.position.y = params_.initial_state[1]; - pose.pose.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), params_.initial_state[2])); + pose.pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), params_.initial_state[2])); pose.pose.covariance[0] = params_.initial_sigma[0] * params_.initial_sigma[0]; pose.pose.covariance[7] = params_.initial_sigma[1] * params_.initial_sigma[1]; pose.pose.covariance[35] = params_.initial_sigma[2] * params_.initial_sigma[2]; @@ -169,30 +147,32 @@ void Unicycle2DIgnition::stop() started_ = false; } -void Unicycle2DIgnition::subscriberCallback( - const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +void Unicycle2DIgnition::subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { - try { + try + { process(msg); - } catch (const std::exception & e) { + } + catch (const std::exception& e) + { RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); } } -bool Unicycle2DIgnition::setPoseServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPose::Request::SharedPtr req) +bool Unicycle2DIgnition::setPoseServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPose::Request::SharedPtr req) { - try { - process( - req->pose, - [service, request_id]() { - fuse_msgs::srv::SetPose::Response response; - response.success = true; - service->send_response(*request_id, response); - }); - } catch (const std::exception & e) { + try + { + process(req->pose, [service, request_id]() { + fuse_msgs::srv::SetPose::Response response; + response.success = true; + service->send_response(*request_id, response); + }); + } + catch (const std::exception& e) + { fuse_msgs::srv::SetPose::Response response; response.success = false; response.message = e.what(); @@ -203,18 +183,18 @@ bool Unicycle2DIgnition::setPoseServiceCallback( } bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) + rclcpp::Service::SharedPtr service, std::shared_ptr request_id, + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) { - try { - process( - req->pose, - [service, request_id]() { - fuse_msgs::srv::SetPoseDeprecated::Response response; - service->send_response(*request_id, response); - }); - } catch (const std::exception & e) { + try + { + process(req->pose, [service, request_id]() { + fuse_msgs::srv::SetPoseDeprecated::Response response; + service->send_response(*request_id, response); + }); + } + catch (const std::exception& e) + { fuse_msgs::srv::SetPoseDeprecated::Response response; RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); service->send_response(*request_id, response); @@ -222,63 +202,62 @@ bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback( return true; } -void Unicycle2DIgnition::process( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, std::function post_process) +void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + std::function post_process) { // Verify we are in the correct state to process set pose requests - if (!started_) { + if (!started_) + { throw std::runtime_error("Attempting to set the pose while the sensor is stopped."); } // Validate the requested pose and covariance before we do anything - if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y)) { - throw std::invalid_argument( - "Attempting to set the pose to an invalid position (" + - std::to_string(pose.pose.pose.position.x) + ", " + - std::to_string(pose.pose.pose.position.y) + ")."); + if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y)) + { + throw std::invalid_argument("Attempting to set the pose to an invalid position (" + + std::to_string(pose.pose.pose.position.x) + ", " + + std::to_string(pose.pose.pose.position.y) + ")."); } - auto orientation_norm = std::sqrt( - pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + - pose.pose.pose.orientation.y * pose.pose.pose.orientation.y + - pose.pose.pose.orientation.z * pose.pose.pose.orientation.z + - pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); - if (std::abs(orientation_norm - 1.0) > 1.0e-3) { + auto orientation_norm = std::sqrt(pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + + pose.pose.pose.orientation.y * pose.pose.pose.orientation.y + + pose.pose.pose.orientation.z * pose.pose.pose.orientation.z + + pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); + if (std::abs(orientation_norm - 1.0) > 1.0e-3) + { throw std::invalid_argument( - "Attempting to set the pose to an invalid orientation (" + - std::to_string(pose.pose.pose.orientation.x) + ", " + - std::to_string(pose.pose.pose.orientation.y) + ", " + - std::to_string(pose.pose.pose.orientation.z) + ", " + - std::to_string(pose.pose.pose.orientation.w) + ")."); + "Attempting to set the pose to an invalid orientation (" + std::to_string(pose.pose.pose.orientation.x) + ", " + + std::to_string(pose.pose.pose.orientation.y) + ", " + std::to_string(pose.pose.pose.orientation.z) + ", " + + std::to_string(pose.pose.pose.orientation.w) + ")."); } auto position_cov = fuse_core::Matrix2d(); - position_cov << pose.pose.covariance[0], pose.pose.covariance[1], - pose.pose.covariance[6], pose.pose.covariance[7]; - if (!fuse_core::isSymmetric(position_cov)) { - throw std::invalid_argument( - "Attempting to set the pose with a non-symmetric position covariance matri\n " + - fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[6], pose.pose.covariance[7]; + if (!fuse_core::isSymmetric(position_cov)) + { + throw std::invalid_argument("Attempting to set the pose with a non-symmetric position covariance matri\n " + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); } - if (!fuse_core::isPositiveDefinite(position_cov)) { - throw std::invalid_argument( - "Attempting to set the pose with a non-positive-definite position covariance matrix\n" + - fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + if (!fuse_core::isPositiveDefinite(position_cov)) + { + throw std::invalid_argument("Attempting to set the pose with a non-positive-definite position covariance matrix\n" + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); } auto orientation_cov = fuse_core::Matrix1d(); orientation_cov << pose.pose.covariance[35]; - if (orientation_cov(0) <= 0.0) { - throw std::invalid_argument( - "Attempting to set the pose with a non-positive-definite orientation covariance " - "matrix " + fuse_core::to_string(orientation_cov) + "."); + if (orientation_cov(0) <= 0.0) + { + throw std::invalid_argument("Attempting to set the pose with a non-positive-definite orientation covariance " + "matrix " + + fuse_core::to_string(orientation_cov) + "."); } // Tell the optimizer to reset before providing the initial state - if (!params_.reset_service.empty()) { + if (!params_.reset_service.empty()) + { // Wait for the reset service while (!reset_client_->wait_for_service(std::chrono::seconds(10)) && - interfaces_.get_node_base_interface()->get_context()->is_valid()) + interfaces_.get_node_base_interface()->get_context()->is_valid()) { - RCLCPP_WARN_STREAM( - logger_, - "Waiting for '" << reset_client_->get_service_name() << "' service to become avaiable."); + RCLCPP_WARN_STREAM(logger_, + "Waiting for '" << reset_client_->get_service_name() << "' service to become available."); } auto srv = std::make_shared(); @@ -286,27 +265,30 @@ void Unicycle2DIgnition::process( // It needs to be free to handle the response to this service call. // Have a callback do the rest of the work when a response comes. auto result_future = reset_client_->async_send_request( - srv, - [this, post_process, pose](rclcpp::Client::SharedFuture result) { - (void)result; - // Now that the pose has been validated and the optimizer has been reset, actually send the - // initial state constraints to the optimizer - sendPrior(pose); - if (post_process) { - post_process(); - } - }); - } else { + srv, [this, post_process, pose](rclcpp::Client::SharedFuture result) { + (void)result; + // Now that the pose has been validated and the optimizer has been reset, actually send the + // initial state constraints to the optimizer + sendPrior(pose); + if (post_process) + { + post_process(); + } + }); + } + else + { sendPrior(pose); - if (post_process) { + if (post_process) + { post_process(); } } } -void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose) { - const auto & stamp = pose.header.stamp; + const auto& stamp = pose.header.stamp; // Create variables for the full state. // The initial values of the pose are extracted from the provided PoseWithCovarianceStamped @@ -315,19 +297,14 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS position->x() = pose.pose.pose.position.x; position->y() = pose.pose.pose.position.y; auto orientation = fuse_variables::Orientation2DStamped::make_shared(stamp, device_id_); - orientation->yaw() = fuse_core::getYaw( - pose.pose.pose.orientation.w, - pose.pose.pose.orientation.x, - pose.pose.pose.orientation.y, - pose.pose.pose.orientation.z); + orientation->yaw() = fuse_core::getYaw(pose.pose.pose.orientation.w, pose.pose.pose.orientation.x, + pose.pose.pose.orientation.y, pose.pose.pose.orientation.z); auto linear_velocity = fuse_variables::VelocityLinear2DStamped::make_shared(stamp, device_id_); linear_velocity->x() = params_.initial_state[3]; linear_velocity->y() = params_.initial_state[4]; auto angular_velocity = fuse_variables::VelocityAngular2DStamped::make_shared(stamp, device_id_); angular_velocity->yaw() = params_.initial_state[5]; - auto linear_acceleration = fuse_variables::AccelerationLinear2DStamped::make_shared( - stamp, - device_id_); + auto linear_acceleration = fuse_variables::AccelerationLinear2DStamped::make_shared(stamp, device_id_); linear_acceleration->x() = params_.initial_state[6]; linear_acceleration->y() = params_.initial_state[7]; @@ -337,8 +314,7 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS auto position_mean = fuse_core::VectorXd(2); position_mean << position->x(), position->y(); auto position_cov = fuse_core::MatrixXd(2, 2); - position_cov << pose.pose.covariance[0], pose.pose.covariance[1], - pose.pose.covariance[6], pose.pose.covariance[7]; + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[6], pose.pose.covariance[7]; auto orientation_mean = fuse_core::VectorXd(1); orientation_mean << orientation->yaw(); auto orientation_cov = fuse_core::MatrixXd(1, 1); @@ -346,8 +322,8 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS auto linear_velocity_mean = fuse_core::VectorXd(2); linear_velocity_mean << linear_velocity->x(), linear_velocity->y(); auto linear_velocity_cov = fuse_core::MatrixXd(2, 2); - linear_velocity_cov << params_.initial_sigma[3] * params_.initial_sigma[3], 0.0, - 0.0, params_.initial_sigma[4] * params_.initial_sigma[4]; + linear_velocity_cov << params_.initial_sigma[3] * params_.initial_sigma[3], 0.0, 0.0, + params_.initial_sigma[4] * params_.initial_sigma[4]; auto angular_velocity_mean = fuse_core::VectorXd(1); angular_velocity_mean << angular_velocity->yaw(); auto angular_velocity_cov = fuse_core::MatrixXd(1, 1); @@ -355,39 +331,20 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS auto linear_acceleration_mean = fuse_core::VectorXd(2); linear_acceleration_mean << linear_acceleration->x(), linear_acceleration->y(); auto linear_acceleration_cov = fuse_core::MatrixXd(2, 2); - linear_acceleration_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, - 0.0, params_.initial_sigma[7] * params_.initial_sigma[7]; + linear_acceleration_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, 0.0, + params_.initial_sigma[7] * params_.initial_sigma[7]; // Create absolute constraints for each variable auto position_constraint = fuse_constraints::AbsolutePosition2DStampedConstraint::make_shared( - name(), - *position, - position_mean, - position_cov); - auto orientation_constraint = - fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared( - name(), - *orientation, - orientation_mean, - orientation_cov); - auto linear_velocity_constraint = - fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared( - name(), - *linear_velocity, - linear_velocity_mean, - linear_velocity_cov); - auto angular_velocity_constraint = - fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint::make_shared( - name(), - *angular_velocity, - angular_velocity_mean, - angular_velocity_cov); - auto linear_acceleration_constraint = - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( - name(), - *linear_acceleration, - linear_acceleration_mean, - linear_acceleration_cov); + name(), *position, position_mean, position_cov); + auto orientation_constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared( + name(), *orientation, orientation_mean, orientation_cov); + auto linear_velocity_constraint = fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared( + name(), *linear_velocity, linear_velocity_mean, linear_velocity_cov); + auto angular_velocity_constraint = fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint::make_shared( + name(), *angular_velocity, angular_velocity_mean, angular_velocity_cov); + auto linear_acceleration_constraint = fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint::make_shared( + name(), *linear_acceleration, linear_acceleration_mean, linear_acceleration_cov); // Create the transaction auto transaction = fuse_core::Transaction::make_shared(); @@ -407,12 +364,9 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceS // Send the transaction to the optimizer. sendTransaction(transaction); - RCLCPP_INFO_STREAM( - logger_, - "Received a set_pose request (stamp: " << rclcpp::Time(stamp).nanoseconds() - << ", x: " << position->x() << ", y: " - << position->y() << ", yaw: " << orientation->yaw() << - ")"); + RCLCPP_INFO_STREAM(logger_, "Received a set_pose request (stamp: " << rclcpp::Time(stamp).nanoseconds() << ", x: " + << position->x() << ", y: " << position->y() + << ", yaw: " << orientation->yaw() << ")"); } } // namespace fuse_models diff --git a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp index 9accc5061..51281873b 100644 --- a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp @@ -50,36 +50,24 @@ namespace fuse_models { Unicycle2DStateKinematicConstraint::Unicycle2DStateKinematicConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & position1, - const fuse_variables::Orientation2DStamped & yaw1, - const fuse_variables::VelocityLinear2DStamped & linear_velocity1, - const fuse_variables::VelocityAngular2DStamped & yaw_velocity1, - const fuse_variables::AccelerationLinear2DStamped & linear_acceleration1, - const fuse_variables::Position2DStamped & position2, - const fuse_variables::Orientation2DStamped & yaw2, - const fuse_variables::VelocityLinear2DStamped & linear_velocity2, - const fuse_variables::VelocityAngular2DStamped & yaw_velocity2, - const fuse_variables::AccelerationLinear2DStamped & linear_acceleration2, - const fuse_core::Matrix8d & covariance) -: fuse_core::Constraint( - source, - {position1.uuid(), - yaw1.uuid(), - linear_velocity1.uuid(), - yaw_velocity1.uuid(), - linear_acceleration1.uuid(), - position2.uuid(), - yaw2.uuid(), - linear_velocity2.uuid(), - yaw_velocity2.uuid(), - linear_acceleration2.uuid()}), // NOLINT - dt_((position2.stamp() - position1.stamp()).seconds()), - sqrt_information_(covariance.inverse().llt().matrixU()) + const std::string& source, const fuse_variables::Position2DStamped& position1, + const fuse_variables::Orientation2DStamped& yaw1, const fuse_variables::VelocityLinear2DStamped& linear_velocity1, + const fuse_variables::VelocityAngular2DStamped& yaw_velocity1, + const fuse_variables::AccelerationLinear2DStamped& linear_acceleration1, + const fuse_variables::Position2DStamped& position2, const fuse_variables::Orientation2DStamped& yaw2, + const fuse_variables::VelocityLinear2DStamped& linear_velocity2, + const fuse_variables::VelocityAngular2DStamped& yaw_velocity2, + const fuse_variables::AccelerationLinear2DStamped& linear_acceleration2, const fuse_core::Matrix8d& covariance) + : fuse_core::Constraint(source, { position1.uuid(), yaw1.uuid(), linear_velocity1.uuid(), yaw_velocity1.uuid(), + linear_acceleration1.uuid(), position2.uuid(), yaw2.uuid(), linear_velocity2.uuid(), + yaw_velocity2.uuid(), linear_acceleration2.uuid() }) + , // NOLINT + dt_((position2.stamp() - position1.stamp()).seconds()) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } -void Unicycle2DStateKinematicConstraint::print(std::ostream & stream) const +void Unicycle2DStateKinematicConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -98,7 +86,7 @@ void Unicycle2DStateKinematicConstraint::print(std::ostream & stream) const << " sqrt_info: " << sqrtInformation() << "\n"; } -ceres::CostFunction * Unicycle2DStateKinematicConstraint::costFunction() const +ceres::CostFunction* Unicycle2DStateKinematicConstraint::costFunction() const { // Here we return a cost function that computes the analytic derivatives/jacobians, but we could // use automatic differentiation as follows: diff --git a/fuse_models/test/CMakeLists.txt b/fuse_models/test/CMakeLists.txt index f8e4347f5..05f6747be 100644 --- a/fuse_models/test/CMakeLists.txt +++ b/fuse_models/test/CMakeLists.txt @@ -1,11 +1,9 @@ -# CORE GTESTS ====================================================================================== +# CORE GTESTS +# ====================================================================================== set(TEST_TARGETS - test_unicycle_2d - test_unicycle_2d_predict - test_unicycle_2d_state_cost_function - test_graph_ignition - test_unicycle_2d_ignition -) + test_unicycle_2d test_unicycle_2d_predict + test_unicycle_2d_state_cost_function test_graph_ignition + test_unicycle_2d_ignition) foreach(test_name ${TEST_TARGETS}) ament_add_gtest("${test_name}" "${test_name}.cpp") diff --git a/fuse_models/test/example_constraint.hpp b/fuse_models/test/example_constraint.hpp index 99d371f76..a33cdb666 100644 --- a/fuse_models/test/example_constraint.hpp +++ b/fuse_models/test/example_constraint.hpp @@ -49,7 +49,6 @@ #include #include - /** * @brief Dummy constraint implementation for testing */ @@ -60,33 +59,28 @@ class ExampleConstraint : public fuse_core::Constraint ExampleConstraint() = default; - ExampleConstraint( - const std::string & source, - std::initializer_list variable_uuid_list) - : fuse_core::Constraint(source, variable_uuid_list), data(0.0) + ExampleConstraint(const std::string& source, std::initializer_list variable_uuid_list) + : fuse_core::Constraint(source, variable_uuid_list), data(0.0) { } - template - ExampleConstraint( - const std::string & source, VariableUuidIterator first, - VariableUuidIterator last) - : fuse_core::Constraint(source, first, last), data(0.0) + template + ExampleConstraint(const std::string& source, VariableUuidIterator first, VariableUuidIterator last) + : fuse_core::Constraint(source, first, last), data(0.0) { } - void print(std::ostream & stream = std::cout) const override + void print(std::ostream& stream = std::cout) const override { stream << type() << ":\n" << " source: " << source() << '\n' << " uuid: " << uuid() << '\n' << " variables: ["; - const auto & variable_uuids = variables(); - if (!variable_uuids.empty()) { - std::copy( - variable_uuids.begin(), - variable_uuids.end() - 1, std::ostream_iterator(stream, ", ")); + const auto& variable_uuids = variables(); + if (!variable_uuids.empty()) + { + std::copy(variable_uuids.begin(), variable_uuids.end() - 1, std::ostream_iterator(stream, ", ")); stream << variable_uuids.back(); } @@ -94,7 +88,7 @@ class ExampleConstraint : public fuse_core::Constraint << " data: " << data << '\n'; } - ceres::CostFunction * costFunction() const override + ceres::CostFunction* costFunction() const override { return nullptr; } @@ -112,11 +106,11 @@ class ExampleConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data; + archive& boost::serialization::base_object(*this); + archive& data; } }; diff --git a/fuse_models/test/example_variable.hpp b/fuse_models/test/example_variable.hpp index 4eaa80e4b..182974429 100644 --- a/fuse_models/test/example_variable.hpp +++ b/fuse_models/test/example_variable.hpp @@ -52,8 +52,7 @@ class ExampleVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(ExampleVariable) - ExampleVariable() - : fuse_core::Variable(fuse_core::uuid::generate()), data_(0.0) + ExampleVariable() : fuse_core::Variable(fuse_core::uuid::generate()), data_(0.0) { } @@ -62,17 +61,17 @@ class ExampleVariable : public fuse_core::Variable return 1; } - const double * data() const override + const double* data() const override { return &data_; } - double * data() override + double* data() override { return &data_; } - void print(std::ostream & stream = std::cout) const override + void print(std::ostream& stream = std::cout) const override { stream << type() << ":\n" << " uuid: " << uuid() << '\n' @@ -92,11 +91,11 @@ class ExampleVariable : public fuse_core::Variable * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; diff --git a/fuse_models/test/example_variable_stamped.hpp b/fuse_models/test/example_variable_stamped.hpp index ea04bd1fd..7768ba7e9 100644 --- a/fuse_models/test/example_variable_stamped.hpp +++ b/fuse_models/test/example_variable_stamped.hpp @@ -55,12 +55,10 @@ class ExampleVariableStamped : public fuse_core::Variable, public fuse_variables ExampleVariableStamped() = default; - explicit ExampleVariableStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL) - : fuse_core::Variable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id), - data_(0.0) + explicit ExampleVariableStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL) + : fuse_core::Variable(fuse_core::uuid::generate(detail::type(), stamp, device_id)) + , Stamped(stamp, device_id) + , data_(0.0) { } @@ -69,17 +67,17 @@ class ExampleVariableStamped : public fuse_core::Variable, public fuse_variables return 1; } - const double * data() const override + const double* data() const override { return &data_; } - double * data() override + double* data() override { return &data_; } - void print(std::ostream & stream = std::cout) const override + void print(std::ostream& stream = std::cout) const override { stream << type() << ":\n" << " uuid: " << uuid() << '\n' @@ -101,12 +99,12 @@ class ExampleVariableStamped : public fuse_core::Variable, public fuse_variables * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); + archive& data_; } }; diff --git a/fuse_models/test/test_graph_ignition.cpp b/fuse_models/test/test_graph_ignition.cpp index d3ccebdc0..d4776b249 100644 --- a/fuse_models/test/test_graph_ignition.cpp +++ b/fuse_models/test/test_graph_ignition.cpp @@ -77,35 +77,38 @@ static std::string failure_description; // NOLINT(runtime/string) * @brief Compare all the properties of two Variable objects * @return True if all the properties match, false otherwise */ -bool compareVariables(const fuse_core::Variable & expected, const fuse_core::Variable & actual) +bool compareVariables(const fuse_core::Variable& expected, const fuse_core::Variable& actual) { failure_description = ""; bool variables_equal = true; - if (expected.type() != actual.type()) { + if (expected.type() != actual.type()) + { variables_equal = false; - failure_description += "The variables have different types.\n expected type is '" + - expected.type() + - "'\n actual type is '" + actual.type() + "'\n"; + failure_description += "The variables have different types.\n expected type is '" + expected.type() + + "'\n actual type is '" + actual.type() + "'\n"; } - if (expected.size() != actual.size()) { + if (expected.size() != actual.size()) + { variables_equal = false; failure_description += "The variables have different sizes.\n expected size is '" + - std::to_string(expected.size()) + "'\n actual size is '" + std::to_string(actual.size()) + - "'\n"; + std::to_string(expected.size()) + "'\n actual size is '" + std::to_string(actual.size()) + + "'\n"; } - if (expected.uuid() != actual.uuid()) { + if (expected.uuid() != actual.uuid()) + { variables_equal = false; failure_description += "The variables have different UUIDs.\n expected UUID is '" + - fuse_core::uuid::to_string(expected.uuid()) + "'\n actual UUID is '" + - fuse_core::uuid::to_string(actual.uuid()) + "'\n"; + fuse_core::uuid::to_string(expected.uuid()) + "'\n actual UUID is '" + + fuse_core::uuid::to_string(actual.uuid()) + "'\n"; } - for (size_t i = 0; i < expected.size(); ++i) { - if (expected.data()[i] != actual.data()[i]) { + for (size_t i = 0; i < expected.size(); ++i) + { + if (expected.data()[i] != actual.data()[i]) + { variables_equal = false; - failure_description += "The variables have different values.\n expected data(" + - std::to_string(i) + ") is '" + - std::to_string(expected.data()[i]) + "'\n actual data(" + std::to_string(i) + ") is '" + - std::to_string(actual.data()[i]) + "'\n"; + failure_description += "The variables have different values.\n expected data(" + std::to_string(i) + ") is '" + + std::to_string(expected.data()[i]) + "'\n actual data(" + std::to_string(i) + ") is '" + + std::to_string(actual.data()[i]) + "'\n"; } } return variables_equal; @@ -115,40 +118,40 @@ bool compareVariables(const fuse_core::Variable & expected, const fuse_core::Var * @brief Compare all the properties of two Constraint objects * @return True if all the properties match, false otherwise */ -bool compareConstraints( - const fuse_core::Constraint & expected, - const fuse_core::Constraint & actual) +bool compareConstraints(const fuse_core::Constraint& expected, const fuse_core::Constraint& actual) { failure_description = ""; bool constraints_equal = true; - if (expected.type() != actual.type()) { + if (expected.type() != actual.type()) + { constraints_equal = false; - failure_description += "The constraints have different types.\n expected type is '" + - expected.type() + - "'\n actual type is '" + actual.type() + "'\n"; + failure_description += "The constraints have different types.\n expected type is '" + expected.type() + + "'\n actual type is '" + actual.type() + "'\n"; } - if (expected.uuid() != actual.uuid()) { + if (expected.uuid() != actual.uuid()) + { constraints_equal = false; failure_description += "The constraints have different UUIDs.\n expected UUID is '" + - fuse_core::uuid::to_string(expected.uuid()) + "'\n actual UUID is '" + - fuse_core::uuid::to_string(actual.uuid()) + "'\n"; + fuse_core::uuid::to_string(expected.uuid()) + "'\n actual UUID is '" + + fuse_core::uuid::to_string(actual.uuid()) + "'\n"; } - if (expected.variables().size() != actual.variables().size()) { + if (expected.variables().size() != actual.variables().size()) + { constraints_equal = false; - failure_description += - "The constraints involve a different number of variables.\n expected variable count is '" + - std::to_string(expected.variables().size()) + "'\n actual variable count is '" + - std::to_string(actual.variables().size()) + "'\n"; + failure_description += "The constraints involve a different number of variables.\n expected variable count is '" + + std::to_string(expected.variables().size()) + "'\n actual variable count is '" + + std::to_string(actual.variables().size()) + "'\n"; } - for (size_t i = 0; i < expected.variables().size(); ++i) { - if (expected.variables().at(i) != actual.variables().at(i)) { + for (size_t i = 0; i < expected.variables().size(); ++i) + { + if (expected.variables().at(i) != actual.variables().at(i)) + { constraints_equal = false; std::string i_str = std::to_string(i); - failure_description += - "The constraints involve different variable UUIDs.\n expected variables(" + i_str + - ") is '" + fuse_core::uuid::to_string(expected.variables()[i]) + - "'\n actual variables(" + i_str + ") is '" + - fuse_core::uuid::to_string(actual.variables()[i]) + "'\n"; + failure_description += "The constraints involve different variable UUIDs.\n expected variables(" + i_str + + ") is '" + fuse_core::uuid::to_string(expected.variables()[i]) + + "'\n actual variables(" + i_str + ") is '" + + fuse_core::uuid::to_string(actual.variables()[i]) + "'\n"; } } return constraints_equal; @@ -157,22 +160,22 @@ bool compareConstraints( namespace fuse_core { -bool operator==(const fuse_core::Variable & rhs, const fuse_core::Variable & lhs) +bool operator==(const fuse_core::Variable& rhs, const fuse_core::Variable& lhs) { return compareVariables(rhs, lhs); } -bool operator!=(const fuse_core::Variable & rhs, const fuse_core::Variable & lhs) +bool operator!=(const fuse_core::Variable& rhs, const fuse_core::Variable& lhs) { return !(rhs == lhs); } -bool operator==(const fuse_core::Constraint & rhs, const fuse_core::Constraint & lhs) +bool operator==(const fuse_core::Constraint& rhs, const fuse_core::Constraint& lhs) { return compareConstraints(rhs, lhs); } -bool operator!=(const fuse_core::Constraint & rhs, const fuse_core::Constraint & lhs) +bool operator!=(const fuse_core::Constraint& rhs, const fuse_core::Constraint& lhs) { return !(rhs == lhs); } @@ -190,23 +193,21 @@ class GraphIgnitionTestFixture : public ::testing::Test { rclcpp::init(0, nullptr); executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); rclcpp::shutdown(); } - std::thread spinner_; //!< Internal thread for spinning the executor + std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; }; @@ -214,11 +215,8 @@ TEST_F(GraphIgnitionTestFixture, SetGraphService) { // Test that the expected PoseStamped message is published rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "ignition_sensor.set_graph_service:=set_graph", - "-p", "ignition_sensor.reset_service:=''"}); + options.arguments({ "--ros-args", "-p", "ignition_sensor.set_graph_service:=set_graph", "-p", + "ignition_sensor.reset_service:=''" }); auto node = rclcpp::Node::make_shared("graph_ignition_test", options); executor_->add_node(node); @@ -247,14 +245,12 @@ TEST_F(GraphIgnitionTestFixture, SetGraphService) graph.addVariable(variable3); auto constraint1 = ExampleConstraint::make_shared( - "test", - std::initializer_list{variable1->uuid(), variable2->uuid()}); // NOLINT + "test", std::initializer_list{ variable1->uuid(), variable2->uuid() }); // NOLINT constraint1->data = 1.5; graph.addConstraint(constraint1); auto constraint2 = ExampleConstraint::make_shared( - "test", - std::initializer_list{variable2->uuid(), variable3->uuid()}); // NOLINT + "test", std::initializer_list{ variable2->uuid(), variable3->uuid() }); // NOLINT constraint2->data = -3.7; graph.addConstraint(constraint2); @@ -293,24 +289,32 @@ TEST_F(GraphIgnitionTestFixture, SetGraphService) // // So even if the added constraints and variables are stored in std::vector containers in the // transaction, we cannot compare them with the straightforward approach mentioned above. Instead, - // we need to check that all added constraints and varaibles are in the graph, and check they are + // we need to check that all added constraints and variables are in the graph, and check they are // the same. - for (const auto & added_constraint : transaction->addedConstraints()) { - try { - const auto & constraint = graph.getConstraint(added_constraint.uuid()); + for (const auto& added_constraint : transaction->addedConstraints()) + { + try + { + const auto& constraint = graph.getConstraint(added_constraint.uuid()); EXPECT_EQ(constraint, added_constraint) << failure_description; - } catch (const std::out_of_range & ex) { + } + catch (const std::out_of_range& ex) + { ADD_FAILURE() << ex.what(); } } - for (const auto & added_variable : transaction->addedVariables()) { - try { - const auto & variable = graph.getVariable(added_variable.uuid()); + for (const auto& added_variable : transaction->addedVariables()) + { + try + { + const auto& variable = graph.getVariable(added_variable.uuid()); EXPECT_EQ(variable, added_variable) << failure_description; - } catch (const std::out_of_range & ex) { + } + catch (const std::out_of_range& ex) + { ADD_FAILURE() << ex.what(); } } @@ -327,11 +331,8 @@ TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) { // Set some configuration rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "ignition_sensor.set_graph_service:=set_graph", - "-p", "ignition_sensor.reset_service:=''"}); + options.arguments({ "--ros-args", "-p", "ignition_sensor.set_graph_service:=set_graph", "-p", + "ignition_sensor.reset_service:=''" }); auto node = rclcpp::Node::make_shared("graph_ignition_test", options); executor_->add_node(node); @@ -360,14 +361,12 @@ TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) graph.addVariable(variable3); auto constraint1 = ExampleConstraint::make_shared( - "test", - std::initializer_list{variable1->uuid(), variable2->uuid()}); // NOLINT + "test", std::initializer_list{ variable1->uuid(), variable2->uuid() }); // NOLINT constraint1->data = 1.5; graph.addConstraint(constraint1); auto constraint2 = ExampleConstraint::make_shared( - "test", - std::initializer_list{variable2->uuid(), variable3->uuid()}); // NOLINT + "test", std::initializer_list{ variable2->uuid(), variable3->uuid() }); // NOLINT constraint2->data = -3.7; graph.addConstraint(constraint2); @@ -406,24 +405,32 @@ TEST_F(GraphIgnitionTestFixture, SetGraphServiceWithStampedVariables) // // So even if the added constraints and variables are stored in std::vector containers in the // transaction, we cannot compare them with the straightforward approach mentioned above. Instead, - // we need to check that all added constraints and varaibles are in the graph, and check they are + // we need to check that all added constraints and variables are in the graph, and check they are // the same. - for (const auto & added_constraint : transaction->addedConstraints()) { - try { - const auto & constraint = graph.getConstraint(added_constraint.uuid()); + for (const auto& added_constraint : transaction->addedConstraints()) + { + try + { + const auto& constraint = graph.getConstraint(added_constraint.uuid()); EXPECT_EQ(constraint, added_constraint) << failure_description; - } catch (const std::out_of_range & ex) { + } + catch (const std::out_of_range& ex) + { ADD_FAILURE() << ex.what(); } } - for (const auto & added_variable : transaction->addedVariables()) { - try { - const auto & variable = graph.getVariable(added_variable.uuid()); + for (const auto& added_variable : transaction->addedVariables()) + { + try + { + const auto& variable = graph.getVariable(added_variable.uuid()); EXPECT_EQ(variable, added_variable) << failure_description; - } catch (const std::out_of_range & ex) { + } + catch (const std::out_of_range& ex) + { ADD_FAILURE() << ex.what(); } } diff --git a/fuse_models/test/test_sensor_proc.cpp b/fuse_models/test/test_sensor_proc.cpp index 637a93699..c61455768 100644 --- a/fuse_models/test/test_sensor_proc.cpp +++ b/fuse_models/test/test_sensor_proc.cpp @@ -14,54 +14,42 @@ namespace fm_common = fuse_models::common; TEST(TestSuite, mergeXYPositionAndYawOrientationIndices) { - const std::vector position_indices{0, 1}; - const std::vector orientation_indices{0}; + const std::vector position_indices{ 0, 1 }; + const std::vector orientation_indices{ 0 }; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); - EXPECT_THAT( - position_indices, - testing::ElementsAreArray( - merged_indices.begin(), - merged_indices.begin() + position_indices.size())); + EXPECT_THAT(position_indices, + testing::ElementsAreArray(merged_indices.begin(), merged_indices.begin() + position_indices.size())); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); } TEST(TestSuite, mergeXPositionAndYawOrientationIndices) { - const std::vector position_indices{0}; - const std::vector orientation_indices{0}; + const std::vector position_indices{ 0 }; + const std::vector orientation_indices{ 0 }; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); - EXPECT_THAT( - position_indices, - testing::ElementsAreArray( - merged_indices.begin(), - merged_indices.begin() + position_indices.size())); + EXPECT_THAT(position_indices, + testing::ElementsAreArray(merged_indices.begin(), merged_indices.begin() + position_indices.size())); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); } TEST(TestSuite, mergeXYPositionAndEmptyOrientationIndices) { - const std::vector position_indices{0, 1}; + const std::vector position_indices{ 0, 1 }; const std::vector orientation_indices; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size(), merged_indices.size()); EXPECT_THAT(position_indices, testing::ElementsAreArray(merged_indices)); @@ -70,13 +58,11 @@ TEST(TestSuite, mergeXYPositionAndEmptyOrientationIndices) TEST(TestSuite, mergeEmptyPositionAndYawOrientationIndices) { const std::vector position_indices; - const std::vector orientation_indices{0}; + const std::vector orientation_indices{ 0 }; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(orientation_indices.size(), merged_indices.size()); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); @@ -89,9 +75,7 @@ TEST(TestSuite, mergeEmptyPositionAndEmptyOrientationIndices) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_TRUE(merged_indices.empty()); } @@ -104,24 +88,18 @@ TEST(TestSuite, populatePartialMeasurementXYPositionYawOrientation) fuse_core::MatrixXd pose_covariance(3, 3); pose_covariance << 0.1, 0.01, 0.001, 0.01, 0.2, 0.002, 0.001, 0.002, 0.3; - const std::vector position_indices{0, 1}; - const std::vector orientation_indices{0}; + const std::vector position_indices{ 0, 1 }; + const std::vector orientation_indices{ 0 }; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); - fm_common::populatePartialMeasurement( - pose_mean, - pose_covariance, - merged_indices, - pose_mean_partial, - pose_covariance_partial); + fm_common::populatePartialMeasurement(pose_mean, pose_covariance, merged_indices, pose_mean_partial, + pose_covariance_partial); EXPECT_EQ(pose_mean, pose_mean_partial); EXPECT_EQ(pose_covariance, pose_covariance_partial); @@ -135,38 +113,33 @@ TEST(TestSuite, populatePartialMeasurementXPositionYawOrientation) fuse_core::MatrixXd pose_covariance(3, 3); pose_covariance << 0.1, 0.01, 0.001, 0.01, 0.2, 0.002, 0.001, 0.002, 0.3; - const std::vector position_indices{0}; - const std::vector orientation_indices{0}; + const std::vector position_indices{ 0 }; + const std::vector orientation_indices{ 0 }; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); - fm_common::populatePartialMeasurement( - pose_mean, - pose_covariance, - merged_indices, - pose_mean_partial, - pose_covariance_partial); + fm_common::populatePartialMeasurement(pose_mean, pose_covariance, merged_indices, pose_mean_partial, + pose_covariance_partial); // Eigen indexing is only supported in the latest stable versions, so we avoid using that feature // for backwards compatibility - const std::vector expected_merged_indices{0, 2}; + const std::vector expected_merged_indices{ 0, 2 }; EXPECT_THAT(expected_merged_indices, testing::ElementsAreArray(merged_indices)); - for (size_t row = 0; row < expected_merged_indices.size(); ++row) { + for (size_t row = 0; row < expected_merged_indices.size(); ++row) + { EXPECT_EQ(pose_mean(expected_merged_indices[row]), pose_mean_partial(row)); - for (size_t column = 0; column < expected_merged_indices.size(); ++column) { - EXPECT_EQ( - pose_covariance(expected_merged_indices[row], expected_merged_indices[column]), - pose_covariance_partial(row, column)); + for (size_t column = 0; column < expected_merged_indices.size(); ++column) + { + EXPECT_EQ(pose_covariance(expected_merged_indices[row], expected_merged_indices[column]), + pose_covariance_partial(row, column)); } } } @@ -180,23 +153,17 @@ TEST(TestSuite, populatePartialMeasurementEmptyPositionYawOrientation) pose_covariance << 0.1, 0.01, 0.001, 0.01, 0.2, 0.002, 0.001, 0.002, 0.3; const std::vector position_indices; - const std::vector orientation_indices{0}; + const std::vector orientation_indices{ 0 }; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); - fm_common::populatePartialMeasurement( - pose_mean, - pose_covariance, - merged_indices, - pose_mean_partial, - pose_covariance_partial); + fm_common::populatePartialMeasurement(pose_mean, pose_covariance, merged_indices, pose_mean_partial, + pose_covariance_partial); EXPECT_EQ(pose_mean.tail<1>(), pose_mean_partial); EXPECT_EQ(pose_covariance.bottomRightCorner(1, 1), pose_covariance_partial); @@ -210,24 +177,18 @@ TEST(TestSuite, populatePartialMeasurementXYPositionEmptyOrientation) fuse_core::MatrixXd pose_covariance(3, 3); pose_covariance << 0.1, 0.01, 0.001, 0.01, 0.2, 0.002, 0.001, 0.002, 0.3; - const std::vector position_indices{0, 1}; + const std::vector position_indices{ 0, 1 }; const std::vector orientation_indices; const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); - fm_common::populatePartialMeasurement( - pose_mean, - pose_covariance, - merged_indices, - pose_mean_partial, - pose_covariance_partial); + fm_common::populatePartialMeasurement(pose_mean, pose_covariance, merged_indices, pose_mean_partial, + pose_covariance_partial); EXPECT_EQ(pose_mean.head<2>(), pose_mean_partial); EXPECT_EQ(pose_covariance.topLeftCorner(2, 2), pose_covariance_partial); @@ -246,19 +207,13 @@ TEST(TestSuite, populatePartialMeasurementEmptyPositionEmptyOrientation) const size_t orientation_offset = 2; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); - fm_common::populatePartialMeasurement( - pose_mean, - pose_covariance, - merged_indices, - pose_mean_partial, - pose_covariance_partial); + fm_common::populatePartialMeasurement(pose_mean, pose_covariance, merged_indices, pose_mean_partial, + pose_covariance_partial); EXPECT_EQ(0, pose_mean_partial.size()); EXPECT_EQ(0, pose_covariance_partial.size()); diff --git a/fuse_models/test/test_unicycle_2d.cpp b/fuse_models/test/test_unicycle_2d.cpp index a132a82f3..1d60e4b81 100644 --- a/fuse_models/test/test_unicycle_2d.cpp +++ b/fuse_models/test/test_unicycle_2d.cpp @@ -21,9 +21,9 @@ class Unicycle2DModelTest : public fuse_models::Unicycle2D { public: - using fuse_models::Unicycle2D::updateStateHistoryEstimates; - using fuse_models::Unicycle2D::StateHistoryElement; using fuse_models::Unicycle2D::StateHistory; + using fuse_models::Unicycle2D::StateHistoryElement; + using fuse_models::Unicycle2D::updateStateHistoryEstimates; }; TEST(Unicycle2D, UpdateStateHistoryEstimates) @@ -33,8 +33,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1, 0)); auto linear_velocity1 = fuse_variables::VelocityLinear2DStamped::make_shared(rclcpp::Time(1, 0)); auto yaw_velocity1 = fuse_variables::VelocityAngular2DStamped::make_shared(rclcpp::Time(1, 0)); - auto linear_acceleration1 = - fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(1, 0)); + auto linear_acceleration1 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(1, 0)); position1->x() = 1.1; position1->y() = 2.1; yaw1->yaw() = 3.1; @@ -47,8 +46,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto yaw2 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(2, 0)); auto linear_velocity2 = fuse_variables::VelocityLinear2DStamped::make_shared(rclcpp::Time(2, 0)); auto yaw_velocity2 = fuse_variables::VelocityAngular2DStamped::make_shared(rclcpp::Time(2, 0)); - auto linear_acceleration2 = - fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(2, 0)); + auto linear_acceleration2 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(2, 0)); position2->x() = 1.2; position2->y() = 2.2; yaw2->yaw() = M_PI / 2.0; @@ -61,8 +59,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto yaw3 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(3, 0)); auto linear_velocity3 = fuse_variables::VelocityLinear2DStamped::make_shared(rclcpp::Time(3, 0)); auto yaw_velocity3 = fuse_variables::VelocityAngular2DStamped::make_shared(rclcpp::Time(3, 0)); - auto linear_acceleration3 = - fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(3, 0)); + auto linear_acceleration3 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(3, 0)); position3->x() = 1.3; position3->y() = 2.3; yaw3->yaw() = 3.3; @@ -75,8 +72,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto yaw4 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(4, 0)); auto linear_velocity4 = fuse_variables::VelocityLinear2DStamped::make_shared(rclcpp::Time(4, 0)); auto yaw_velocity4 = fuse_variables::VelocityAngular2DStamped::make_shared(rclcpp::Time(4, 0)); - auto linear_acceleration4 = - fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(4, 0)); + auto linear_acceleration4 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(4, 0)); position4->x() = 1.4; position4->y() = 2.4; yaw4->yaw() = 3.4; @@ -89,8 +85,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto yaw5 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(5, 0)); auto linear_velocity5 = fuse_variables::VelocityLinear2DStamped::make_shared(rclcpp::Time(5, 0)); auto yaw_velocity5 = fuse_variables::VelocityAngular2DStamped::make_shared(rclcpp::Time(5, 0)); - auto linear_acceleration5 = - fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(5, 0)); + auto linear_acceleration5 = fuse_variables::AccelerationLinear2DStamped::make_shared(rclcpp::Time(5, 0)); position5->x() = 1.5; position5->y() = 2.5; yaw5->yaw() = 3.5; @@ -116,71 +111,39 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) // Add all of the variables to the state history Unicycle2DModelTest::StateHistory state_history; - state_history.emplace( - position1->stamp(), - Unicycle2DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position1->uuid(), - yaw1->uuid(), - linear_velocity1->uuid(), - yaw_velocity1->uuid(), - linear_acceleration1->uuid(), - tf2_2d::Transform(1.0, 0.0, 0.0), - tf2_2d::Vector2(0.0, 0.0), - 0.0, - tf2_2d::Vector2(0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position2->stamp(), - Unicycle2DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position2->uuid(), - yaw2->uuid(), - linear_velocity2->uuid(), - yaw_velocity2->uuid(), - linear_acceleration2->uuid(), - tf2_2d::Transform(2.0, 0.0, 0.0), - tf2_2d::Vector2(0.0, 0.0), - 0.0, - tf2_2d::Vector2(0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position3->stamp(), - Unicycle2DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position3->uuid(), - yaw3->uuid(), - linear_velocity3->uuid(), - yaw_velocity3->uuid(), - linear_acceleration3->uuid(), - tf2_2d::Transform(3.0, 0.0, 0.0), - tf2_2d::Vector2(0.0, 0.0), - 0.0, - tf2_2d::Vector2(0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position4->stamp(), - Unicycle2DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position4->uuid(), - yaw4->uuid(), - linear_velocity4->uuid(), - yaw_velocity4->uuid(), - linear_acceleration4->uuid(), - tf2_2d::Transform(4.0, 0.0, 0.0), - tf2_2d::Vector2(0.0, 0.0), - 0.0, - tf2_2d::Vector2(0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position5->stamp(), - Unicycle2DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position5->uuid(), - yaw5->uuid(), - linear_velocity5->uuid(), - yaw_velocity5->uuid(), - linear_acceleration5->uuid(), - tf2_2d::Transform(5.0, 0.0, 0.0), - tf2_2d::Vector2(0.0, 0.0), - 0.0, - tf2_2d::Vector2(0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace(position1->stamp(), + Unicycle2DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position1->uuid(), yaw1->uuid(), linear_velocity1->uuid(), yaw_velocity1->uuid(), + linear_acceleration1->uuid(), tf2_2d::Transform(1.0, 0.0, 0.0), tf2_2d::Vector2(0.0, 0.0), + 0.0, tf2_2d::Vector2(0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position2->stamp(), + Unicycle2DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position2->uuid(), yaw2->uuid(), linear_velocity2->uuid(), yaw_velocity2->uuid(), + linear_acceleration2->uuid(), tf2_2d::Transform(2.0, 0.0, 0.0), tf2_2d::Vector2(0.0, 0.0), + 0.0, tf2_2d::Vector2(0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position3->stamp(), + Unicycle2DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position3->uuid(), yaw3->uuid(), linear_velocity3->uuid(), yaw_velocity3->uuid(), + linear_acceleration3->uuid(), tf2_2d::Transform(3.0, 0.0, 0.0), tf2_2d::Vector2(0.0, 0.0), + 0.0, tf2_2d::Vector2(0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position4->stamp(), + Unicycle2DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position4->uuid(), yaw4->uuid(), linear_velocity4->uuid(), yaw_velocity4->uuid(), + linear_acceleration4->uuid(), tf2_2d::Transform(4.0, 0.0, 0.0), tf2_2d::Vector2(0.0, 0.0), + 0.0, tf2_2d::Vector2(0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position5->stamp(), + Unicycle2DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position5->uuid(), yaw5->uuid(), linear_velocity5->uuid(), yaw_velocity5->uuid(), + linear_acceleration5->uuid(), tf2_2d::Transform(5.0, 0.0, 0.0), tf2_2d::Vector2(0.0, 0.0), + 0.0, tf2_2d::Vector2(0.0, 0.0) }); // NOLINT(whitespace/braces) // Update the state history - Unicycle2DModelTest::updateStateHistoryEstimates( - graph, state_history, rclcpp::Duration::from_seconds( - 10.0)); + Unicycle2DModelTest::updateStateHistoryEstimates(graph, state_history, rclcpp::Duration::from_seconds(10.0)); // Check the state estimates in the state history { @@ -274,9 +237,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) { // The fifth entry is missing from the graph. It will get predicted from previous state. // These values were verified with Octave - auto expected_pose = tf2_2d::Transform( - -3.9778707804360529, -8.9511455751801616, - -2.7663706143591722); + auto expected_pose = tf2_2d::Transform(-3.9778707804360529, -8.9511455751801616, -2.7663706143591722); auto actual_pose = state_history[rclcpp::Time(5, 0)].pose; EXPECT_NEAR(expected_pose.x(), actual_pose.x(), 1.0e-9); EXPECT_NEAR(expected_pose.y(), actual_pose.y(), 1.0e-9); diff --git a/fuse_models/test/test_unicycle_2d_ignition.cpp b/fuse_models/test/test_unicycle_2d_ignition.cpp index 4f799a3e2..8122e8513 100644 --- a/fuse_models/test/test_unicycle_2d_ignition.cpp +++ b/fuse_models/test/test_unicycle_2d_ignition.cpp @@ -49,12 +49,11 @@ #include #include -using fuse_constraints::AbsolutePosition2DStampedConstraint; +using fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint; using fuse_constraints::AbsoluteOrientation2DStampedConstraint; -using fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint; +using fuse_constraints::AbsolutePosition2DStampedConstraint; using fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint; -using fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint; - +using fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint; /** * @brief Promise used to communicate between the tests and the callback @@ -72,19 +71,20 @@ void transactionCallback(fuse_core::Transaction::SharedPtr transaction) /** * @brief Helper function for fetching the desired constraint from a transaction */ -template -const Derived * getConstraint(const fuse_core::Transaction & transaction) +template +const Derived* getConstraint(const fuse_core::Transaction& transaction) { - for (const auto & constraint : transaction.addedConstraints()) { - auto derived = dynamic_cast(&constraint); - if (derived) { + for (const auto& constraint : transaction.addedConstraints()) + { + auto derived = dynamic_cast(&constraint); + if (derived) + { return derived; } } return nullptr; } - class Unicycle2DIgnitionTestFixture : public ::testing::Test { public: @@ -96,23 +96,21 @@ class Unicycle2DIgnitionTestFixture : public ::testing::Test { rclcpp::init(0, nullptr); executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); rclcpp::shutdown(); } - std::thread spinner_; //!< Internal thread for spinning the executor + std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; }; @@ -120,13 +118,12 @@ TEST_F(Unicycle2DIgnitionTestFixture, InitialTransaction) { // Set some configuration rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "ignition_sensor.initial_state:=" - "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", - "-p", "ignition_sensor.initial_sigma:=" - "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]"}); + options.arguments({ "--ros-args", "-p", + "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", + "-p", + "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]" }); auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); executor_->add_node(node); @@ -201,14 +198,13 @@ TEST_F(Unicycle2DIgnitionTestFixture, SkipInitialTransaction) { // Set some configuration rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "ignition_sensor.initial_state:=" - "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", - "-p", "ignition_sensor.initial_sigma:=" - "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", - "-p", "ignition_sensor.publish_on_startup:=false"}); + options.arguments({ "--ros-args", "-p", + "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", + "-p", + "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", + "-p", "ignition_sensor.publish_on_startup:=false" }); auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); executor_->add_node(node); @@ -230,16 +226,14 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseService) { // Set some configuration rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "ignition_sensor.initial_state:=" - "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", - "-p", "ignition_sensor.initial_sigma:=" - "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", - "-p", "ignition_sensor.set_pose_service:=set_pose", - "-p", "ignition_sensor.reset_service:=''", - "-p", "ignition_sensor.publish_on_startup:=false"}); + options.arguments({ "--ros-args", "-p", + "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", + "-p", + "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", + "-p", "ignition_sensor.set_pose_service:=set_pose", "-p", "ignition_sensor.reset_service:=''", + "-p", "ignition_sensor.publish_on_startup:=false" }); auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); executor_->add_node(node); @@ -331,16 +325,14 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseDeprecatedService) { // Set some configuration rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "ignition_sensor.initial_state:=" - "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", - "-p", "ignition_sensor.initial_sigma:=" - "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", - "-p", "ignition_sensor.set_pose_deprecated_service:=set_pose_deprecated", - "-p", "ignition_sensor.reset_service:=''", - "-p", "ignition_sensor.publish_on_startup:=false"}); + options.arguments({ "--ros-args", "-p", + "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 3.4, 4.5, 5.6, 6.7, 7.8]", + "-p", + "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0]", + "-p", "ignition_sensor.set_pose_deprecated_service:=set_pose_deprecated", "-p", + "ignition_sensor.reset_service:=''", "-p", "ignition_sensor.publish_on_startup:=false" }); auto node = rclcpp::Node::make_shared("unicycle_2d_ignition_test", options); executor_->add_node(node); @@ -363,8 +355,8 @@ TEST_F(Unicycle2DIgnitionTestFixture, SetPoseDeprecatedService) srv->pose.pose.covariance[0] = 1.0; srv->pose.pose.covariance[7] = 2.0; srv->pose.pose.covariance[35] = 3.0; - auto client = node->create_client( - "/unicycle_2d_ignition_test/set_pose_deprecated"); + auto client = + node->create_client("/unicycle_2d_ignition_test/set_pose_deprecated"); ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); auto result = client->async_send_request(srv); ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); diff --git a/fuse_models/test/test_unicycle_2d_predict.cpp b/fuse_models/test/test_unicycle_2d_predict.cpp index 1b3fc57b2..d57547005 100644 --- a/fuse_models/test/test_unicycle_2d_predict.cpp +++ b/fuse_models/test/test_unicycle_2d_predict.cpp @@ -61,24 +61,9 @@ TEST(Predict, predictDirectVals) double acc_linear2_x = 0.0; double acc_linear2_y = 0.0; - fuse_models::predict( - position1_x, - position1_y, - yaw1, - vel_linear1_x, - vel_linear1_y, - vel_yaw1, - acc_linear1_x, - acc_linear1_y, - dt, - position2_x, - position2_y, - yaw2, - vel_linear2_x, - vel_linear2_y, - vel_yaw2, - acc_linear2_x, - acc_linear2_y); + fuse_models::predict(position1_x, position1_y, yaw1, vel_linear1_x, vel_linear1_y, vel_yaw1, acc_linear1_x, + acc_linear1_y, dt, position2_x, position2_y, yaw2, vel_linear2_x, vel_linear2_y, vel_yaw2, + acc_linear2_x, acc_linear2_y); EXPECT_DOUBLE_EQ(0.105, position2_x); EXPECT_DOUBLE_EQ(0.0, position2_y); @@ -90,24 +75,9 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(0.0, acc_linear2_y); // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - position2_x, - position2_y, - yaw2, - vel_linear2_x, - vel_linear2_y, - vel_yaw2, - acc_linear2_x, - acc_linear2_y, - dt, - position2_x, - position2_y, - yaw2, - vel_linear2_x, - vel_linear2_y, - vel_yaw2, - acc_linear2_x, - acc_linear2_y); + fuse_models::predict(position2_x, position2_y, yaw2, vel_linear2_x, vel_linear2_y, vel_yaw2, acc_linear2_x, + acc_linear2_y, dt, position2_x, position2_y, yaw2, vel_linear2_x, vel_linear2_y, vel_yaw2, + acc_linear2_x, acc_linear2_y); EXPECT_DOUBLE_EQ(0.21858415916807189, position2_x); EXPECT_DOUBLE_EQ(0.017989963481956205, position2_y); @@ -123,24 +93,9 @@ TEST(Predict, predictDirectVals) vel_yaw1 = -1.570796327; acc_linear1_y = -1.0; - fuse_models::predict( - position1_x, - position1_y, - yaw1, - vel_linear1_x, - vel_linear1_y, - vel_yaw1, - acc_linear1_x, - acc_linear1_y, - dt, - position2_x, - position2_y, - yaw2, - vel_linear2_x, - vel_linear2_y, - vel_yaw2, - acc_linear2_x, - acc_linear2_y); + fuse_models::predict(position1_x, position1_y, yaw1, vel_linear1_x, vel_linear1_y, vel_yaw1, acc_linear1_x, + acc_linear1_y, dt, position2_x, position2_y, yaw2, vel_linear2_x, vel_linear2_y, vel_yaw2, + acc_linear2_x, acc_linear2_y); EXPECT_DOUBLE_EQ(0.105, position2_x); EXPECT_DOUBLE_EQ(-0.105, position2_y); @@ -169,18 +124,8 @@ TEST(Predict, predictPointers) double vel_yaw2 = 1.570796327; std::vector acc_linear2(2, 0.0); - fuse_models::predict( - position1.data(), - &yaw1, - vel_linear1.data(), - &vel_yaw1, - acc_linear1.data(), - dt, - position2.data(), - &yaw2, - vel_linear2.data(), - &vel_yaw2, - acc_linear2.data()); + fuse_models::predict(position1.data(), &yaw1, vel_linear1.data(), &vel_yaw1, acc_linear1.data(), dt, position2.data(), + &yaw2, vel_linear2.data(), &vel_yaw2, acc_linear2.data()); EXPECT_DOUBLE_EQ(0.105, position2[0]); EXPECT_DOUBLE_EQ(0.0, position2[1]); @@ -192,18 +137,8 @@ TEST(Predict, predictPointers) EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - position2.data(), - &yaw2, - vel_linear2.data(), - &vel_yaw2, - acc_linear2.data(), - dt, - position2.data(), - &yaw2, - vel_linear2.data(), - &vel_yaw2, - acc_linear2.data()); + fuse_models::predict(position2.data(), &yaw2, vel_linear2.data(), &vel_yaw2, acc_linear2.data(), dt, position2.data(), + &yaw2, vel_linear2.data(), &vel_yaw2, acc_linear2.data()); EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); EXPECT_DOUBLE_EQ(0.017989963481956205, position2[1]); @@ -219,18 +154,8 @@ TEST(Predict, predictPointers) vel_yaw1 = -1.570796327; acc_linear1[1] = -1.0; - fuse_models::predict( - position1.data(), - &yaw1, - vel_linear1.data(), - &vel_yaw1, - acc_linear1.data(), - dt, - position2.data(), - &yaw2, - vel_linear2.data(), - &vel_yaw2, - acc_linear2.data()); + fuse_models::predict(position1.data(), &yaw1, vel_linear1.data(), &vel_yaw1, acc_linear1.data(), dt, position2.data(), + &yaw2, vel_linear2.data(), &vel_yaw2, acc_linear2.data()); EXPECT_DOUBLE_EQ(0.105, position2[0]); EXPECT_DOUBLE_EQ(-0.105, position2[1]); @@ -256,16 +181,7 @@ TEST(Predict, predictObjects) double vel_yaw2 = 0.0; tf2_2d::Vector2 acc_linear2; - fuse_models::predict( - pose1, - vel_linear1, - vel_yaw1, - acc_linear1, - dt, - pose2, - vel_linear2, - vel_yaw2, - acc_linear2); + fuse_models::predict(pose1, vel_linear1, vel_yaw1, acc_linear1, dt, pose2, vel_linear2, vel_yaw2, acc_linear2); EXPECT_DOUBLE_EQ(0.105, pose2.x()); EXPECT_DOUBLE_EQ(0.0, pose2.y()); @@ -277,17 +193,7 @@ TEST(Predict, predictObjects) EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - pose2, - vel_linear2, - vel_yaw2, - acc_linear2, - dt, - pose2, - vel_linear2, - vel_yaw2, - acc_linear2); - + fuse_models::predict(pose2, vel_linear2, vel_yaw2, acc_linear2, dt, pose2, vel_linear2, vel_yaw2, acc_linear2); EXPECT_DOUBLE_EQ(0.21858415916807189, pose2.x()); EXPECT_DOUBLE_EQ(0.017989963481956205, pose2.y()); @@ -303,16 +209,7 @@ TEST(Predict, predictObjects) vel_yaw1 = -1.570796327; acc_linear1.setY(-1.0); - fuse_models::predict( - pose1, - vel_linear1, - vel_yaw1, - acc_linear1, - dt, - pose2, - vel_linear2, - vel_yaw2, - acc_linear2); + fuse_models::predict(pose1, vel_linear1, vel_yaw1, acc_linear1, dt, pose2, vel_linear2, vel_yaw2, acc_linear2); EXPECT_DOUBLE_EQ(0.105, pose2.x()); EXPECT_DOUBLE_EQ(-0.105, pose2.y()); @@ -344,38 +241,23 @@ TEST(Predict, predictJacobians) double acc_linear2_x = 0.0; double acc_linear2_y = 0.0; - const std::array block_sizes = {2, 1, 2, 1, 2}; + const std::array block_sizes = { 2, 1, 2, 1, 2 }; const auto num_parameter_blocks = block_sizes.size(); - const size_t num_residuals{8}; + const size_t num_residuals{ 8 }; std::array J; - std::array jacobians; + std::array jacobians; - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } - fuse_models::predict( - position1_x, - position1_y, - yaw1, - vel_linear1_x, - vel_linear1_y, - vel_yaw1, - acc_linear1_x, - acc_linear1_y, - dt, - position2_x, - position2_y, - yaw2, - vel_linear2_x, - vel_linear2_y, - vel_yaw2, - acc_linear2_x, - acc_linear2_y, - jacobians.data()); + fuse_models::predict(position1_x, position1_y, yaw1, vel_linear1_x, vel_linear1_y, vel_yaw1, acc_linear1_x, + acc_linear1_y, dt, position2_x, position2_y, yaw2, vel_linear2_x, vel_linear2_y, vel_yaw2, + acc_linear2_x, acc_linear2_y, jacobians.data()); fuse_core::Matrix8d J_analytic; J_analytic << J[0], J[1], J[2], J[3], J[4]; @@ -410,41 +292,20 @@ TEST(Predict, predictJacobians) Jet jet_acc_linear2_x(acc_linear2_x, 6); Jet jet_acc_linear2_y(acc_linear2_y, 7); - fuse_models::predict( - jet_position1_x, - jet_position1_y, - jet_yaw1, - jet_vel_linear1_x, - jet_vel_linear1_y, - jet_vel_yaw1, - jet_acc_linear1_x, - jet_acc_linear1_y, - jet_dt, - jet_position2_x, - jet_position2_y, - jet_yaw2, - jet_vel_linear2_x, - jet_vel_linear2_y, - jet_vel_yaw2, - jet_acc_linear2_x, - jet_acc_linear2_y); + fuse_models::predict(jet_position1_x, jet_position1_y, jet_yaw1, jet_vel_linear1_x, jet_vel_linear1_y, jet_vel_yaw1, + jet_acc_linear1_x, jet_acc_linear1_y, jet_dt, jet_position2_x, jet_position2_y, jet_yaw2, + jet_vel_linear2_x, jet_vel_linear2_y, jet_vel_yaw2, jet_acc_linear2_x, jet_acc_linear2_y); fuse_core::Matrix8d J_autodiff; - J_autodiff << jet_position2_x.v, - jet_position2_y.v, - jet_yaw2.v, - jet_vel_linear2_x.v, - jet_vel_linear2_y.v, - jet_vel_yaw2.v, - jet_acc_linear2_x.v, - jet_acc_linear2_y.v; + J_autodiff << jet_position2_x.v, jet_position2_y.v, jet_yaw2.v, jet_vel_linear2_x.v, jet_vel_linear2_y.v, + jet_vel_yaw2.v, jet_acc_linear2_x.v, jet_acc_linear2_y.v; J_autodiff.transposeInPlace(); - const Eigen::IOFormat HeavyFmt( - Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + const Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); EXPECT_MATRIX_NEAR(J_autodiff, J_analytic, std::numeric_limits::epsilon()) - << "Autodiff Jacobian =\n" << J_autodiff.format(HeavyFmt) - << "\nAnalytic Jacobian =\n" << J_analytic.format(HeavyFmt); + << "Autodiff Jacobian =\n" + << J_autodiff.format(HeavyFmt) << "\nAnalytic Jacobian =\n" + << J_analytic.format(HeavyFmt); } diff --git a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp index ac3202d40..c548d44a8 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -47,44 +47,42 @@ TEST(CostFunction, evaluateCostFunction) { // Create cost function - const double process_noise_diagonal[] = {1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9}; + const double process_noise_diagonal[] = { 1e-3, 1e-3, 1e-2, 1e-6, 1e-6, 1e-4, 1e-9, 1e-9 }; const fuse_core::Matrix8d covariance = fuse_core::Vector8d(process_noise_diagonal).asDiagonal(); - const double dt{0.1}; - const fuse_core::Matrix8d sqrt_information{covariance.inverse().llt().matrixU()}; + const double dt{ 0.1 }; + const fuse_core::Matrix8d sqrt_information{ covariance.inverse().llt().matrixU() }; - const fuse_models::Unicycle2DStateCostFunction cost_function{dt, sqrt_information}; + const fuse_models::Unicycle2DStateCostFunction cost_function{ dt, sqrt_information }; // Evaluate cost function - const double position1[] = {0.0, 0.0}; - const double yaw1[] = {0.0}; - const double vel_linear1[] = {1.0, 0.0}; - const double vel_yaw1[] = {1.570796327}; - const double acc_linear1[] = {1.0, 0.0}; - - const double position2[] = {0.105, 0.0}; - const double yaw2[] = {0.1570796327}; - const double vel_linear2[] = {1.1, 0.0}; - const double vel_yaw2[] = {1.570796327}; - const double acc_linear2[] = {1.0, 0.0}; - - const double * parameters[] = - { - position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, - position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 - }; + const double position1[] = { 0.0, 0.0 }; + const double yaw1[] = { 0.0 }; + const double vel_linear1[] = { 1.0, 0.0 }; + const double vel_yaw1[] = { 1.570796327 }; + const double acc_linear1[] = { 1.0, 0.0 }; + + const double position2[] = { 0.105, 0.0 }; + const double yaw2[] = { 0.1570796327 }; + const double vel_linear2[] = { 1.1, 0.0 }; + const double vel_yaw2[] = { 1.570796327 }; + const double acc_linear2[] = { 1.0, 0.0 }; + + const double* parameters[] = { position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, + position2, yaw2, vel_linear2, vel_yaw2, acc_linear2 }; fuse_core::Vector8d residuals; - const auto & block_sizes = cost_function.parameter_block_sizes(); + const auto& block_sizes = cost_function.parameter_block_sizes(); const auto num_parameter_blocks = block_sizes.size(); const auto num_residuals = cost_function.num_residuals(); std::vector J(num_parameter_blocks); - std::vector jacobians(num_parameter_blocks); + std::vector jacobians(num_parameter_blocks); - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } @@ -109,30 +107,28 @@ TEST(CostFunction, evaluateCostFunction) // probe_results.error_log; // Create cost function using automatic differentiation on the cost functor - ceres::AutoDiffCostFunction - cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); + ceres::AutoDiffCostFunction + cost_function_autodiff(new fuse_models::Unicycle2DStateCostFunctor(dt, sqrt_information)); // Evaluate cost function that uses automatic differentiation std::vector J_autodiff(num_parameter_blocks); - std::vector jacobians_autodiff(num_parameter_blocks); + std::vector jacobians_autodiff(num_parameter_blocks); - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J_autodiff[i].resize(num_residuals, block_sizes[i]); jacobians_autodiff[i] = J_autodiff[i].data(); } - EXPECT_TRUE( - cost_function_autodiff.Evaluate( - parameters, residuals.data(), - jacobians_autodiff.data())); + EXPECT_TRUE(cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians_autodiff.data())); - const Eigen::IOFormat HeavyFmt( - Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + const Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], std::numeric_limits::epsilon()) - << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) - << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); + << "Autodiff Jacobian[" << i << "] =\n" + << J_autodiff[i].format(HeavyFmt) << "\nAnalytic Jacobian[" << i << "] =\n" + << J[i].format(HeavyFmt); } } diff --git a/fuse_msgs/CMakeLists.txt b/fuse_msgs/CMakeLists.txt index 392e9f73b..43753d173 100644 --- a/fuse_msgs/CMakeLists.txt +++ b/fuse_msgs/CMakeLists.txt @@ -13,27 +13,18 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -set(msg_files - "msg/SerializedGraph.msg" - "msg/SerializedTransaction.msg" -) +set(MSG_FILES "msg/SerializedGraph.msg" "msg/SerializedTransaction.msg") -set(srv_files - "srv/SetGraph.srv" - "srv/SetPose.srv" - "srv/SetPoseDeprecated.srv" -) +set(SRV_FILES "srv/SetGraph.srv" "srv/SetPose.srv" "srv/SetPoseDeprecated.srv") -rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} +rosidl_generate_interfaces( + ${PROJECT_NAME} + ${MSG_FILES} + ${SRV_FILES} DEPENDENCIES - std_msgs geometry_msgs - ADD_LINTER_TESTS -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() + std_msgs + geometry_msgs + ADD_LINTER_TESTS) ament_export_dependencies(rosidl_default_runtime) ament_package() diff --git a/fuse_msgs/package.xml b/fuse_msgs/package.xml index cbb8de899..f7ac39965 100644 --- a/fuse_msgs/package.xml +++ b/fuse_msgs/package.xml @@ -20,9 +20,6 @@ rosidl_default_runtime - ament_lint_auto - ament_lint_common - rosidl_interface_packages diff --git a/fuse_optimizers/CHANGELOG.rst b/fuse_optimizers/CHANGELOG.rst index ae76dfff9..ce98beb63 100644 --- a/fuse_optimizers/CHANGELOG.rst +++ b/fuse_optimizers/CHANGELOG.rst @@ -9,7 +9,7 @@ Changelog for package fuse_optimizers ------------------ * Fix fuse optimizer unit test (`#369 `_) * Fix fuse optimizer unit test. The rclcpp::wait_for_message call was throwing an exception 'subscription already associated with a wait set'. Switched to a standard subscriber instead. -* Required formatting changes for the lastest version of ROS 2 Rolling (`#368 `_) +* Required formatting changes for the latest version of ROS 2 Rolling (`#368 `_) * Contributors: Stephen Williams 1.1.0 (2024-04-20) diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 6f21b1ce8..8ccc7ce40 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -28,69 +28,60 @@ find_package(rclcpp_components REQUIRED) find_package(Ceres REQUIRED) find_package(Eigen3 REQUIRED) -########### -## Build ## -########### - -## fuse_optimizers library -add_library(${PROJECT_NAME} - src/batch_optimizer.cpp - src/fixed_lag_smoother.cpp - src/optimizer.cpp - src/variable_stamp_index.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$" -) -target_link_libraries(${PROJECT_NAME} +# ############################################################################## +# Build ## +# ############################################################################## + +# fuse_optimizers library +add_library(${PROJECT_NAME} src/batch_optimizer.cpp src/fixed_lag_smoother.cpp + src/optimizer.cpp src/variable_stamp_index.cpp) +target_include_directories( + ${PROJECT_NAME} + PUBLIC "$" + "$") +target_link_libraries( + ${PROJECT_NAME} fuse_constraints::fuse_constraints fuse_core::fuse_core fuse_graphs::fuse_graphs ${fuse_msgs_TARGETS} fuse_variables::fuse_variables pluginlib::pluginlib - ${std_srvs_TARGETS} -) + ${std_srvs_TARGETS}) ament_target_dependencies(${PROJECT_NAME} diagnostic_updater) -## batch_optimizer node +# batch_optimizer node add_executable(batch_optimizer_node src/batch_optimizer_node.cpp) target_link_libraries(batch_optimizer_node ${PROJECT_NAME}) -## fixed_lag_smoother node +# fixed_lag_smoother node add_executable(fixed_lag_smoother_node src/fixed_lag_smoother_node.cpp) target_link_libraries(fixed_lag_smoother_node ${PROJECT_NAME}) -############# -## Testing ## -############# +# ############################################################################## +# Testing ## +# ############################################################################## if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + find_package(nav_msgs REQUIRED) add_subdirectory(test) endif() -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## -install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-export ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) -install(TARGETS - batch_optimizer_node - fixed_lag_smoother_node - DESTINATION lib/${PROJECT_NAME} -) +install(TARGETS batch_optimizer_node fixed_lag_smoother_node + DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET) ament_export_dependencies( @@ -105,9 +96,7 @@ ament_export_dependencies( diagnostic_updater pluginlib rclcpp_components - Ceres - Eigen3 -) + Eigen3) ament_package() diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp index 48e9dc254..d3d905322 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_optimizers { @@ -112,10 +111,8 @@ class BatchOptimizer : public Optimizer * @param[in] interfaces The node interfaces for the node driving the optimizer * @param[in] graph The graph used with the optimizer */ - BatchOptimizer( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph = nullptr - ); + BatchOptimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph = nullptr); /** * @brief Destructor @@ -131,11 +128,10 @@ class BatchOptimizer : public Optimizer std::string sensor_name; fuse_core::Transaction::SharedPtr transaction; - TransactionQueueElement( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) - : sensor_name(sensor_name), - transaction(std::move(transaction)) {} + TransactionQueueElement(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) + : sensor_name(sensor_name), transaction(std::move(transaction)) + { + } }; /** @@ -151,24 +147,24 @@ class BatchOptimizer : public Optimizer //!< constraints and variables from //!< multiple sensors and motions models //!< before being applied to the graph. - std::mutex combined_transaction_mutex_; //!< Synchronize access to the combined transaction - //!< across different threads - ParameterType params_; //!< Configuration settings for this optimizer - std::atomic optimization_request_; //!< Flag to trigger a new optimization - std::condition_variable optimization_requested_; //!< Condition variable used by the optimization - //!< thread to wait until a new optimization is - //!< requested by the main thread - std::mutex optimization_requested_mutex_; //!< Required condition variable mutex - std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process + std::mutex combined_transaction_mutex_; //!< Synchronize access to the combined transaction + //!< across different threads + ParameterType params_; //!< Configuration settings for this optimizer + std::atomic optimization_request_; //!< Flag to trigger a new optimization + std::condition_variable optimization_requested_; //!< Condition variable used by the optimization + //!< thread to wait until a new optimization is + //!< requested by the main thread + std::mutex optimization_requested_mutex_; //!< Required condition variable mutex + std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process rclcpp::TimerBase::SharedPtr optimize_timer_; //!< Trigger an optimization operation at a fixed //!< frequency - TransactionQueue pending_transactions_; //!< The set of received transactions that have not been - //!< added to the optimizer yet. Transactions are added - //!< by the main thread, and removed and processed by the - //!< optimization thread. - std::mutex pending_transactions_mutex_; //!< Synchronize modification of the - //!< pending_transactions_ container - rclcpp::Time start_time_; //!< The timestamp of the first ignition sensor transaction + TransactionQueue pending_transactions_; //!< The set of received transactions that have not been + //!< added to the optimizer yet. Transactions are added + //!< by the main thread, and removed and processed by the + //!< optimization thread. + std::mutex pending_transactions_mutex_; //!< Synchronize modification of the + //!< pending_transactions_ container + rclcpp::Time start_time_; //!< The timestamp of the first ignition sensor transaction bool started_; //!< Flag indicating the optimizer is ready/has received a transaction from an //!< ignition sensor @@ -215,15 +211,13 @@ class BatchOptimizer : public Optimizer * @param[in] transaction The populated Transaction object created by the loaded SensorModel * plugin */ - void transactionCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) override; + void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; /** - * @brief Update and publish diagnotics + * @brief Update and publish diagnostics * @param[in] status The diagnostic status */ - void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status) override; + void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) override; }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp index ba4011db3..5d64d5f9f 100644 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp +++ b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.hpp @@ -45,7 +45,6 @@ #include #include - namespace fuse_optimizers { @@ -63,7 +62,7 @@ struct BatchOptimizerParams * or in the "optimization_frequency" parameter in Hz. "optimization_frequency" will be * prioritized. */ - rclcpp::Duration optimization_period {0, static_cast(RCUTILS_S_TO_NS(0.1))}; + rclcpp::Duration optimization_period{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; /** * @brief The maximum time to wait for motion models to be generated for a received transaction. @@ -72,7 +71,7 @@ struct BatchOptimizerParams * while waiting for motion models to be generated. Once the timeout expires, that transaction * will be deleted from the queue. */ - rclcpp::Duration transaction_timeout {0, static_cast(RCUTILS_S_TO_NS(0.1))}; + rclcpp::Duration transaction_timeout{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; /** * @brief Ceres Solver::Options object that controls various aspects of the optimizer. @@ -85,28 +84,24 @@ struct BatchOptimizerParams * @param[in] interfaces - The node interfaces used to load the parameter */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces) + fuse_core::node_interfaces::NodeInterfaces + interfaces) { // Read settings from the parameter server - double optimization_frequency{-1.0}; - optimization_frequency = fuse_core::getParam( - interfaces, "optimization_frequency", - optimization_frequency); + double optimization_frequency{ -1.0 }; + optimization_frequency = fuse_core::getParam(interfaces, "optimization_frequency", optimization_frequency); fuse_core::getPositiveParam(interfaces, "optimization_period", optimization_period); - if (optimization_frequency != -1.0) { - if (optimization_frequency < 0) { - RCLCPP_WARN_STREAM( - interfaces.get_node_logging_interface()->get_logger(), - "The requested optimization_frequency parameter is < 0. Using the optimization_period" - "parameter instead!"); + if (optimization_frequency != -1.0) + { + if (optimization_frequency < 0) + { + RCLCPP_WARN_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "The requested optimization_frequency parameter is < 0. Using the optimization_period" + "parameter instead!"); } - optimization_period = - rclcpp::Duration::from_seconds(1.0 / optimization_frequency); + optimization_period = rclcpp::Duration::from_seconds(1.0 / optimization_frequency); } fuse_core::getPositiveParam(interfaces, "transaction_timeout", transaction_timeout); diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp index af60d1a37..53a07f651 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.hpp @@ -55,7 +55,6 @@ #include #include - namespace fuse_optimizers { @@ -125,10 +124,8 @@ 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 - ); + FixedLagSmoother(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph = nullptr); /** * @brief Destructor @@ -144,9 +141,18 @@ class FixedLagSmoother : public Optimizer std::string sensor_name; fuse_core::Transaction::SharedPtr transaction; - const rclcpp::Time & stamp() const {return transaction->stamp();} - const rclcpp::Time & minStamp() const {return transaction->minStamp();} - const rclcpp::Time & maxStamp() const {return transaction->maxStamp();} + const rclcpp::Time& stamp() const + { + return transaction->stamp(); + } + const rclcpp::Time& minStamp() const + { + return transaction->minStamp(); + } + const rclcpp::Time& maxStamp() const + { + return transaction->maxStamp(); + } }; /** @@ -163,15 +169,15 @@ class FixedLagSmoother : public Optimizer // Read-only after construction std::thread optimization_thread_; //!< Thread used to run the optimizer as a background process - ParameterType params_; //!< Configuration settings for this fixed-lag smoother + ParameterType params_; //!< Configuration settings for this fixed-lag smoother // Inherently thread-safe - std::atomic ignited_; //!< Flag indicating the optimizer has received a transaction from an - //!< ignition sensor and it is queued but not processed yet + std::atomic ignited_; //!< Flag indicating the optimizer has received a transaction from an + //!< ignition sensor and it is queued but not processed yet std::atomic optimization_running_; //!< Flag indicating the optimization thread should be //!< running - std::atomic started_; //!< Flag indicating the optimizer has received a transaction from an - //!< ignition sensor + std::atomic started_; //!< Flag indicating the optimizer has received a transaction from an + //!< ignition sensor // Guarded by pending_transactions_mutex_ std::mutex pending_transactions_mutex_; //!< Synchronize modification of the @@ -184,27 +190,27 @@ class FixedLagSmoother : public Optimizer // Guarded by optimization_mutex_ std::mutex optimization_mutex_; //!< Mutex held while the graph is begin optimized // fuse_core::Graph* graph_ member from the base class - rclcpp::Time lag_expiration_; //!< The oldest stamp that is inside the fixed-lag smoother window + rclcpp::Time lag_expiration_; //!< The oldest stamp that is inside the fixed-lag smoother window fuse_core::Transaction marginal_transaction_; //!< The marginals to add during the next //!< optimization cycle - VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with - //!< each variable - ceres::Solver::Summary summary_; //!< Optimization summary, written by optimizationLoop and read - //!< by setDiagnostics + VariableStampIndex timestamp_tracking_; //!< Object that tracks the timestamp associated with + //!< each variable + ceres::Solver::Summary summary_; //!< Optimization summary, written by optimizationLoop and read + //!< by setDiagnostics // Guarded by optimization_requested_mutex_ - std::mutex optimization_requested_mutex_; //!< Required condition variable mutex - rclcpp::Time optimization_deadline_; //!< The deadline for the optimization to complete. Triggers - //!< a warning if exceeded. - bool optimization_request_; //!< Flag to trigger a new optimization + std::mutex optimization_requested_mutex_; //!< Required condition variable mutex + rclcpp::Time optimization_deadline_; //!< The deadline for the optimization to complete. Triggers + //!< a warning if exceeded. + bool optimization_request_; //!< Flag to trigger a new optimization std::condition_variable optimization_requested_; //!< Condition variable used by the optimization //!< thread to wait until a new optimization is //!< requested by the main thread // Guarded by start_time_mutex_ - mutable std::mutex start_time_mutex_; //!< Synchronize modification to the start_time_ variable - rclcpp::Time start_time_ {0, 0, RCL_ROS_TIME}; //!< The timestamp of the first ignition sensor - //!< transaction + mutable std::mutex start_time_mutex_; //!< Synchronize modification to the start_time_ variable + rclcpp::Time start_time_{ 0, 0, RCL_ROS_TIME }; //!< The timestamp of the first ignition sensor + //!< transaction // Ordering ROS objects with callbacks last rclcpp::TimerBase::SharedPtr optimize_timer_; //!< Trigger an optimization operation at a fixed @@ -229,7 +235,7 @@ class FixedLagSmoother : public Optimizer * @param[in] new_transaction All new, non-marginal-related transactions that *will be* applied to * the graph */ - void preprocessMarginalization(const fuse_core::Transaction & new_transaction); + void preprocessMarginalization(const fuse_core::Transaction& new_transaction); /** * @brief Compute the oldest timestamp that is part of the configured lag window @@ -246,7 +252,7 @@ class FixedLagSmoother : public Optimizer * @return A container with the set of variables to marginalize out. Order of the variables is not * specified. */ - std::vector computeVariablesToMarginalize(const rclcpp::Time & lag_expiration); + std::vector computeVariablesToMarginalize(const rclcpp::Time& lag_expiration); /** * @brief Perform any required post-marginalization bookkeeping @@ -257,7 +263,7 @@ class FixedLagSmoother : public Optimizer * @param[in] marginal_transaction The actual changes to the graph caused my marginalizing out the * requested variables. */ - void postprocessMarginalization(const fuse_core::Transaction & marginal_transaction); + void postprocessMarginalization(const fuse_core::Transaction& marginal_transaction); /** * @brief Function that optimizes all constraints, designed to be run in a separate thread. @@ -290,15 +296,13 @@ class FixedLagSmoother : public Optimizer * sensor transactions * @param[in] lag_expiration The oldest timestamp that should remain in the graph */ - void processQueue(fuse_core::Transaction & transaction, const rclcpp::Time & lag_expiration); + void processQueue(fuse_core::Transaction& transaction, const rclcpp::Time& lag_expiration); /** * @brief Service callback that resets the optimizer to its original state */ - bool resetServiceCallback( - const std::shared_ptr, - std::shared_ptr - ); + bool resetServiceCallback(const std::shared_ptr, + std::shared_ptr); /** * @brief Thread-safe read-only access to the timestamp of the first transaction @@ -312,7 +316,7 @@ class FixedLagSmoother : public Optimizer /** * @brief Thread-safe write access to the optimizer start time */ - void setStartTime(const rclcpp::Time & start_time) + void setStartTime(const rclcpp::Time& start_time) { std::lock_guard lock(start_time_mutex_); start_time_ = start_time; @@ -331,15 +335,13 @@ class FixedLagSmoother : public Optimizer * @param[in] transaction The populated Transaction object created by the loaded SensorModel * plugin */ - void transactionCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) override; + void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override; /** - * @brief Update and publish diagnotics + * @brief Update and publish diagnostics * @param[in] status The diagnostic status */ - void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status) override; + void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) override; }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h index fadba05d7..735b6e3cc 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h @@ -35,8 +35,7 @@ #ifndef FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_PARAMS_H_ #define FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_PARAMS_H_ -#warning \ - This header is obsolete, please include fuse_optimizers/fixed_lag_smoother_params.hpp instead +#warning This header is obsolete, please include fuse_optimizers/fixed_lag_smoother_params.hpp instead #include 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 139261ca5..ee967cc54 100644 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp +++ b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.hpp @@ -45,7 +45,6 @@ #include #include - namespace fuse_optimizers { @@ -58,7 +57,7 @@ struct FixedLagSmootherParams /** * @brief The duration of the smoothing window in seconds */ - rclcpp::Duration lag_duration {5, 0}; + rclcpp::Duration lag_duration{ 5, 0 }; /** * @brief The target duration for optimization cycles @@ -68,12 +67,12 @@ struct FixedLagSmootherParams * or in the "optimization_frequency" parameter in Hz. "optimization_frequency" will be * prioritized. */ - rclcpp::Duration optimization_period {0, static_cast(RCUTILS_S_TO_NS(0.1))}; + rclcpp::Duration optimization_period{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; /** * @brief The topic name of the advertised reset service */ - std::string reset_service {"~/reset"}; + std::string reset_service{ "~/reset" }; /** * @brief The maximum time to wait for motion models to be generated for a received transaction. @@ -82,7 +81,7 @@ struct FixedLagSmootherParams * while waiting for motion models to be generated. Once the timeout expires, that transaction * will be deleted from the queue. */ - rclcpp::Duration transaction_timeout {0, static_cast(RCUTILS_S_TO_NS(0.1))}; + rclcpp::Duration transaction_timeout{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; /** * @brief Ceres Solver::Options object that controls various aspects of the optimizer. @@ -96,30 +95,26 @@ struct FixedLagSmootherParams * @param[in] node - The node used to load the parameter */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces) + fuse_core::node_interfaces::NodeInterfaces + interfaces) { // Read settings from the parameter server fuse_core::getPositiveParam(interfaces, "lag_duration", lag_duration); - double optimization_frequency{-1.0}; - optimization_frequency = fuse_core::getParam( - interfaces, "optimization_frequency", - optimization_frequency); + double optimization_frequency{ -1.0 }; + optimization_frequency = fuse_core::getParam(interfaces, "optimization_frequency", optimization_frequency); fuse_core::getPositiveParam(interfaces, "optimization_period", optimization_period); - if (optimization_frequency != -1.0) { - if (optimization_frequency < 0) { - RCLCPP_WARN_STREAM( - interfaces.get_node_logging_interface()->get_logger(), - "The requested optimization_frequency parameter is < 0. Using the optimization_period" - "parameter instead!"); + if (optimization_frequency != -1.0) + { + if (optimization_frequency < 0) + { + RCLCPP_WARN_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "The requested optimization_frequency parameter is < 0. Using the optimization_period" + "parameter instead!"); } - optimization_period = - rclcpp::Duration::from_seconds(1.0 / optimization_frequency); + optimization_period = rclcpp::Duration::from_seconds(1.0 / optimization_frequency); } fuse_core::getParam(interfaces, "reset_service", reset_service); diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp index ce6dc6729..54ede9a92 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.hpp +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.hpp @@ -52,7 +52,6 @@ #include #include - namespace fuse_optimizers { @@ -104,10 +103,8 @@ class Optimizer * @param[in] interfaces The node interfaces for the node driving the optimizer * @param[in] graph The graph used with the optimizer */ - Optimizer( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph = nullptr - ); + Optimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph = nullptr); /** * @brief Destructor @@ -134,8 +131,7 @@ class Optimizer * @param[in] model The sensor model * @param[in] ignition Whether this sensor model is an ignition one or not */ - SensorModelInfo(SensorModelUniquePtr model, const bool ignition) - : model(std::move(model)), ignition(ignition) + SensorModelInfo(SensorModelUniquePtr model, const bool ignition) : model(std::move(model)), ignition(ignition) { } @@ -157,12 +153,12 @@ class Optimizer AssociatedMotionModels associated_motion_models_; //!< Tracks what motion models should be used //!< for each sensor - fuse_core::Graph::UniquePtr graph_; //!< The graph object that holds all variables and - //!< constraints + fuse_core::Graph::UniquePtr graph_; //!< The graph object that holds all variables and + //!< constraints pluginlib::ClassLoader motion_model_loader_; //!< Pluginlib class loader //!< for MotionModels - MotionModels motion_models_; //!< The set of motion models, addressable by name + MotionModels motion_models_; //!< The set of motion models, addressable by name pluginlib::ClassLoader publisher_loader_; //!< Pluginlib class loader for //!< Publishers Publishers publishers_; //!< The set of publishers to execute after every graph optimization @@ -174,7 +170,6 @@ class Optimizer std::shared_ptr callback_queue_; - /** * @brief Callback fired every time a SensorModel plugin creates a new transaction * @@ -184,9 +179,7 @@ class Optimizer * @param[in] transaction The populated Transaction object created by the loaded SensorModel * plugin */ - virtual void transactionCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) = 0; + virtual void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) = 0; /** * @brief Configure the motion model plugins specified on the parameter server @@ -225,9 +218,7 @@ class Optimizer * @return Flag indicating if all motion model constraints were successfully * generated */ - bool applyMotionModels( - const std::string & sensor_name, - fuse_core::Transaction & transaction) const; + bool applyMotionModels(const std::string& sensor_name, fuse_core::Transaction& transaction) const; /** * @brief Send the sensors, motion models, and publishers updated graph information @@ -236,9 +227,7 @@ class Optimizer * removals * @param[in] graph A read-only pointer to the graph object */ - void notify( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph); + void notify(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph); /** * @brief Inject a transaction callback function into the global callback queue @@ -247,9 +236,7 @@ class Optimizer * @param[in] transaction The populated Transaction object created by the loaded SensorModel * plugin */ - void injectCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction); + void injectCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction); /** * @brief Clear all of the callbacks inserted into the callback queue by the injectCallback() @@ -268,10 +255,10 @@ class Optimizer void stopPlugins(); /** - * @brief Update and publish diagnotics + * @brief Update and publish diagnostics * @param[in] status The diagnostic status */ - virtual void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status); + virtual void setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status); }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp index 2a85d2a75..ab166db75 100644 --- a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp +++ b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.hpp @@ -44,7 +44,6 @@ #include - namespace fuse_optimizers { /** @@ -74,12 +73,18 @@ class VariableStampIndex /** * @brief Return true if no variables exist in the index */ - bool empty() const {return variables_.empty() && constraints_.empty();} + bool empty() const + { + return variables_.empty() && constraints_.empty(); + } /** * @brief Returns the number of variables in the index */ - size_t size() const {return variables_.size();} + size_t size() const + { + return variables_.size(); + } /** * @brief Clear all tracked state @@ -103,7 +108,7 @@ class VariableStampIndex * * @param[in] transaction The set of variables and constraints to add and remove */ - void addNewTransaction(const fuse_core::Transaction & transaction); + void addNewTransaction(const fuse_core::Transaction& transaction); /** * @brief Update the index with the information from a marginal transaction @@ -113,7 +118,7 @@ class VariableStampIndex * * @param[in] transaction The set of variables and constraints to remove */ - void addMarginalTransaction(const fuse_core::Transaction & transaction); + void addMarginalTransaction(const fuse_core::Transaction& transaction); /** * @brief Add all variables that are not directly connected to a stamped variable with a timestamp @@ -123,30 +128,37 @@ class VariableStampIndex * greater than or equal to this will be added to the output container * @param[out] result An output iterator capable of receiving fuse_core::UUID objects */ - template - void query(const rclcpp::Time & stamp, OutputUuidIterator result) const + template + void query(const rclcpp::Time& stamp, OutputUuidIterator result) const { // First get all of the stamped variables greater than or equal to the input stamp std::unordered_set recent_variable_uuids; - for (const auto & variable_stamp_pair : stamped_index_) { - if (variable_stamp_pair.second >= stamp) { + for (const auto& variable_stamp_pair : stamped_index_) + { + if (variable_stamp_pair.second >= stamp) + { recent_variable_uuids.insert(variable_stamp_pair.first); } } // Now find all of the variables connected to the recent variables std::unordered_set connected_variable_uuids; - for (const auto & recent_variable_uuid : recent_variable_uuids) { + for (const auto& recent_variable_uuid : recent_variable_uuids) + { // Add the recent variable to ensure connected_variable_uuids is a superset of // recent_variable_uuids connected_variable_uuids.insert(recent_variable_uuid); const auto variables_iter = variables_.find(recent_variable_uuid); - if (variables_iter != variables_.end()) { - for (const auto & connected_constraint_uuid : variables_iter->second) { + if (variables_iter != variables_.end()) + { + for (const auto& connected_constraint_uuid : variables_iter->second) + { const auto constraints_iter = constraints_.find(connected_constraint_uuid); - if (constraints_iter != constraints_.end()) { - for (const auto & connected_variable_uuid : constraints_iter->second) { + if (constraints_iter != constraints_.end()) + { + for (const auto& connected_variable_uuid : constraints_iter->second) + { connected_variable_uuids.insert(connected_variable_uuid); } } @@ -155,8 +167,10 @@ class VariableStampIndex } // Return the set of variables that are not connected - for (const auto & variable : variables_) { - if (connected_variable_uuids.find(variable.first) == connected_variable_uuids.end()) { + for (const auto& variable : variables_) + { + if (connected_variable_uuids.find(variable.first) == connected_variable_uuids.end()) + { *result = variable.first; ++result; } @@ -168,34 +182,32 @@ class VariableStampIndex StampedMap stamped_index_; //!< Container that holds the UUID->Stamp mapping for //!< fuse_variables::Stamped variables - using VariableToConstraintsMap = std::unordered_map>; + using VariableToConstraintsMap = std::unordered_map>; VariableToConstraintsMap variables_; - using ConstraintToVariablesMap = std::unordered_map>; + using ConstraintToVariablesMap = std::unordered_map>; ConstraintToVariablesMap constraints_; /** * @brief Update this VariableStampIndex with the added constraints from the provided transaction */ - void applyAddedConstraints(const fuse_core::Transaction & transaction); + void applyAddedConstraints(const fuse_core::Transaction& transaction); /** * @brief Update this VariableStampIndex with the added variables from the provided transaction */ - void applyAddedVariables(const fuse_core::Transaction & transaction); + void applyAddedVariables(const fuse_core::Transaction& transaction); /** * @brief Update this VariableStampIndex with the removed constraints from the provided * transaction */ - void applyRemovedConstraints(const fuse_core::Transaction & transaction); + void applyRemovedConstraints(const fuse_core::Transaction& transaction); /** * @brief Update this VariableStampIndex with the removed variables from the provided transaction */ - void applyRemovedVariables(const fuse_core::Transaction & transaction); + void applyRemovedVariables(const fuse_core::Transaction& transaction); }; } // namespace fuse_optimizers diff --git a/fuse_optimizers/package.xml b/fuse_optimizers/package.xml index 9a5424737..7ec5efbb9 100644 --- a/fuse_optimizers/package.xml +++ b/fuse_optimizers/package.xml @@ -45,8 +45,6 @@ std_srvs ament_cmake_gtest - ament_lint_auto - ament_lint_common fuse_models geometry_msgs nav_msgs diff --git a/fuse_optimizers/src/batch_optimizer.cpp b/fuse_optimizers/src/batch_optimizer.cpp index a4b520706..c87fa4b28 100644 --- a/fuse_optimizers/src/batch_optimizer.cpp +++ b/fuse_optimizers/src/batch_optimizer.cpp @@ -43,30 +43,23 @@ #include #include - namespace fuse_optimizers { -BatchOptimizer::BatchOptimizer( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph -) -: fuse_optimizers::Optimizer(interfaces, std::move(graph)), - combined_transaction_(fuse_core::Transaction::make_shared()), - optimization_request_(false), - start_time_(rclcpp::Time::max()), - started_(false) +BatchOptimizer::BatchOptimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph) + : fuse_optimizers::Optimizer(interfaces, std::move(graph)) + , combined_transaction_(fuse_core::Transaction::make_shared()) + , optimization_request_(false) + , start_time_(rclcpp::Time::max()) + , started_(false) { params_.loadFromROS(interfaces_); // Configure a timer to trigger optimizations - optimize_timer_ = rclcpp::create_timer( - interfaces_, - clock_, - params_.optimization_period, - std::bind(&BatchOptimizer::optimizerTimerCallback, this), - interfaces_.get_node_base_interface()->get_default_callback_group() - ); + optimize_timer_ = rclcpp::create_timer(interfaces_, clock_, params_.optimization_period, + std::bind(&BatchOptimizer::optimizerTimerCallback, this), + interfaces_.get_node_base_interface()->get_default_callback_group()); // Start the optimization thread optimization_thread_ = std::thread(&BatchOptimizer::optimizationLoop, this); @@ -77,7 +70,8 @@ BatchOptimizer::~BatchOptimizer() // Wake up any sleeping threads optimization_requested_.notify_all(); // Wait for the threads to shutdown - if (optimization_thread_.joinable()) { + if (optimization_thread_.joinable()) + { optimization_thread_.join(); } } @@ -88,29 +82,34 @@ void BatchOptimizer::applyMotionModelsToQueue() std::lock_guard pending_transactions_lock(pending_transactions_mutex_); rclcpp::Time current_time; // Use the most recent transaction time as the current time - if (!pending_transactions_.empty()) { + if (!pending_transactions_.empty()) + { // Use the most recent transaction time as the current time current_time = pending_transactions_.rbegin()->first; } // TODO(CH3): We might have to check for time validity here? Attempt to process each pending // transaction - while (!pending_transactions_.empty()) { - auto & element = pending_transactions_.begin()->second; + while (!pending_transactions_.empty()) + { + auto& element = pending_transactions_.begin()->second; // Apply the motion models to the transaction - if (!applyMotionModels(element.sensor_name, *element.transaction)) { - if (element.transaction->stamp() + params_.transaction_timeout < current_time) { + if (!applyMotionModels(element.sensor_name, *element.transaction)) + { + if (element.transaction->stamp() + params_.transaction_timeout < current_time) + { // Warn that this transaction has expired, then skip it. - RCLCPP_ERROR_STREAM( - logger_, - "The queued transaction with timestamp " - << element.transaction->stamp().nanoseconds() << " could not be processed after " - << (current_time - element.transaction->stamp()).nanoseconds() - << " seconds, which is greater than the 'transaction_timeout' value of " - << params_.transaction_timeout.nanoseconds() << ". Ignoring this transaction."); + RCLCPP_ERROR_STREAM(logger_, + "The queued transaction with timestamp " + << element.transaction->stamp().nanoseconds() << " could not be processed after " + << (current_time - element.transaction->stamp()).nanoseconds() + << " seconds, which is greater than the 'transaction_timeout' value of " + << params_.transaction_timeout.nanoseconds() << ". Ignoring this transaction."); pending_transactions_.erase(pending_transactions_.begin()); continue; - } else { + } + else + { // Stop processing future transactions. Try again next time. break; } @@ -129,23 +128,20 @@ void BatchOptimizer::applyMotionModelsToQueue() void BatchOptimizer::optimizationLoop() { // Optimize constraints until told to exit - while (interfaces_.get_node_base_interface()->get_context()->is_valid()) { + while (interfaces_.get_node_base_interface()->get_context()->is_valid()) + { // Wait for the next signal to start the next optimization cycle { std::unique_lock lock(optimization_requested_mutex_); - optimization_requested_.wait( - lock, - [this] { - /* *INDENT-OFF* */ - return ( - optimization_request_ || - !interfaces_.get_node_base_interface()->get_context()->is_valid() - ); - /* *INDENT-ON* */ - }); + optimization_requested_.wait(lock, [this] { + /* *INDENT-OFF* */ + return (optimization_request_ || !interfaces_.get_node_base_interface()->get_context()->is_valid()); + /* *INDENT-ON* */ + }); } // If a shutdown is requested, exit now. - if (!interfaces_.get_node_base_interface()->get_context()->is_valid()) { + if (!interfaces_.get_node_base_interface()->get_context()->is_valid()) + { break; } // Copy the combined transaction so it can be shared with all the plugins @@ -171,7 +167,8 @@ void BatchOptimizer::optimizationLoop() void BatchOptimizer::optimizerTimerCallback() { // If an "ignition" transaction hasn't been received, then we can't do anything yet. - if (!started_) { + if (!started_) + { return; } // Attempt to generate motion models for any queued transactions @@ -184,14 +181,13 @@ void BatchOptimizer::optimizerTimerCallback() // If there is some pending work, trigger the next optimization cycle. // If the optimizer has not completed the previous optimization cycle, then it // will not be waiting on the condition variable signal, so nothing will happen. - if (optimization_request_) { + if (optimization_request_) + { optimization_requested_.notify_one(); } } -void BatchOptimizer::transactionCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) +void BatchOptimizer::transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) { // Add the new transaction to the pending set // Either we haven't "started" yet and we want to keep a short history of transactions around @@ -200,28 +196,29 @@ void BatchOptimizer::transactionCallback( rclcpp::Time transaction_time = transaction->stamp(); rclcpp::Time last_pending_time(0, 0, transaction_clock_type); // NOTE(CH3): Uninitialized - if (!started_ || transaction_time >= start_time_) { + if (!started_ || transaction_time >= start_time_) + { std::lock_guard lock(pending_transactions_mutex_); - pending_transactions_.emplace( - transaction_time, - TransactionQueueElement(sensor_name, std::move(transaction))); + pending_transactions_.emplace(transaction_time, TransactionQueueElement(sensor_name, std::move(transaction))); last_pending_time = pending_transactions_.rbegin()->first; } // If we haven't "started" yet... - if (!started_) { + if (!started_) + { // Check if this transaction "starts" the system - if (sensor_models_.at(sensor_name).ignition) { + if (sensor_models_.at(sensor_name).ignition) + { started_ = true; start_time_ = transaction_time; } // Purge old transactions from the pending queue rclcpp::Time purge_time(0, 0, transaction_clock_type); // NOTE(CH3): Uninitialized - if (started_) { + if (started_) + { purge_time = start_time_; - } else if ( // prevent a bad subtraction // NOLINT - rclcpp::Time( - params_.transaction_timeout.nanoseconds(), last_pending_time.get_clock_type() - ) < last_pending_time) + } + else if ( // prevent a bad subtraction // NOLINT + rclcpp::Time(params_.transaction_timeout.nanoseconds(), last_pending_time.get_clock_type()) < last_pending_time) { purge_time = last_pending_time - params_.transaction_timeout; } @@ -230,12 +227,13 @@ void BatchOptimizer::transactionCallback( pending_transactions_.erase(pending_transactions_.begin(), purge_iter); } // If we have "started", attempt to process any pending transactions - if (started_) { + if (started_) + { applyMotionModelsToQueue(); } } -void BatchOptimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status) +void BatchOptimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) { status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "BatchOptimizer"); diff --git a/fuse_optimizers/src/batch_optimizer_node.cpp b/fuse_optimizers/src/batch_optimizer_node.cpp index e7d00b85f..efc2d3c14 100644 --- a/fuse_optimizers/src/batch_optimizer_node.cpp +++ b/fuse_optimizers/src/batch_optimizer_node.cpp @@ -35,7 +35,7 @@ #include #include -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared("batch_optimizer_node"); diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 87de2e8fe..36e22e6f7 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -58,10 +58,9 @@ namespace * @param[in] position A reverse iterator that access the element to be erased * @return A reverse iterator pointing to the element after the erased element */ -template -typename std::vector::reverse_iterator erase( - std::vector & container, - typename std::vector::reverse_iterator position) +template +typename std::vector::reverse_iterator erase(std::vector& container, + typename std::vector::reverse_iterator position) { // Reverse iterators are weird // https://stackoverflow.com/questions/1830158/how-to-call-erase-with-a-reverse-iterator @@ -78,15 +77,13 @@ typename std::vector::reverse_iterator erase( namespace fuse_optimizers { -FixedLagSmoother::FixedLagSmoother( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph -) -: fuse_optimizers::Optimizer(interfaces, std::move(graph)), - ignited_(false), - optimization_running_(true), - started_(false), - optimization_request_(false) +FixedLagSmoother::FixedLagSmoother(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph) + : fuse_optimizers::Optimizer(interfaces, std::move(graph)) + , ignited_(false) + , optimization_running_(true) + , started_(false) + , optimization_request_(false) { params_.loadFromROS(interfaces_); @@ -97,30 +94,16 @@ FixedLagSmoother::FixedLagSmoother( optimization_thread_ = std::thread(&FixedLagSmoother::optimizationLoop, this); // Configure a timer to trigger optimizations - optimize_timer_ = rclcpp::create_timer( - interfaces_, - clock_, - params_.optimization_period, - std::bind(&FixedLagSmoother::optimizerTimerCallback, this), - interfaces_.get_node_base_interface()->get_default_callback_group() - ); + optimize_timer_ = rclcpp::create_timer(interfaces_, clock_, params_.optimization_period, + std::bind(&FixedLagSmoother::optimizerTimerCallback, this), + interfaces_.get_node_base_interface()->get_default_callback_group()); // Advertise a service that resets the optimizer to its initial state reset_service_server_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.reset_service), - std::bind( - &FixedLagSmoother::resetServiceCallback, - this, - std::placeholders::_1, - std::placeholders::_2 - ), - rclcpp::ServicesQoS(), - interfaces_.get_node_base_interface()->get_default_callback_group() - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.reset_service), + std::bind(&FixedLagSmoother::resetServiceCallback, this, std::placeholders::_1, std::placeholders::_2), + rclcpp::ServicesQoS(), interfaces_.get_node_base_interface()->get_default_callback_group()); } FixedLagSmoother::~FixedLagSmoother() @@ -129,27 +112,25 @@ FixedLagSmoother::~FixedLagSmoother() optimization_running_ = false; optimization_requested_.notify_all(); // Wait for the threads to shutdown - if (optimization_thread_.joinable()) { + if (optimization_thread_.joinable()) + { optimization_thread_.join(); } } void FixedLagSmoother::autostart() { - if (std::none_of( - sensor_models_.begin(), sensor_models_.end(), - [](const auto & element) {return element.second.ignition;})) // NOLINT(whitespace/braces) + if (std::none_of(sensor_models_.begin(), sensor_models_.end(), + [](const auto& element) { return element.second.ignition; })) // NOLINT(whitespace/braces) { // No ignition sensors were provided. Auto-start. started_ = true; setStartTime(rclcpp::Time(0, 1, RCL_ROS_TIME)); // Initialized - RCLCPP_INFO_STREAM( - logger_, - "No ignition sensors were specified. Optimization will begin immediately."); + RCLCPP_INFO_STREAM(logger_, "No ignition sensors were specified. Optimization will begin immediately."); } } -void FixedLagSmoother::preprocessMarginalization(const fuse_core::Transaction & new_transaction) +void FixedLagSmoother::preprocessMarginalization(const fuse_core::Transaction& new_transaction) { timestamp_tracking_.addNewTransaction(new_transaction); } @@ -160,7 +141,8 @@ rclcpp::Time FixedLagSmoother::computeLagExpirationTime() const auto start_time = getStartTime(); auto now = timestamp_tracking_.currentStamp(); - if (0u == now.nanoseconds()) { + if (0u == now.nanoseconds()) + { return start_time; } @@ -168,32 +150,26 @@ rclcpp::Time FixedLagSmoother::computeLagExpirationTime() const return (start_time + params_.lag_duration < now) ? now - params_.lag_duration : start_time; } -std::vector FixedLagSmoother::computeVariablesToMarginalize( - const rclcpp::Time & lag_expiration) +std::vector FixedLagSmoother::computeVariablesToMarginalize(const rclcpp::Time& lag_expiration) { auto marginalize_variable_uuids = std::vector(); timestamp_tracking_.query(lag_expiration, std::back_inserter(marginalize_variable_uuids)); return marginalize_variable_uuids; } -void FixedLagSmoother::postprocessMarginalization( - const fuse_core::Transaction & marginal_transaction) +void FixedLagSmoother::postprocessMarginalization(const fuse_core::Transaction& marginal_transaction) { timestamp_tracking_.addMarginalTransaction(marginal_transaction); } void FixedLagSmoother::optimizationLoop() { - auto exit_wait_condition = [this]() - { - return - this->optimization_request_ || - !this->optimization_running_ || - !interfaces_.get_node_base_interface()->get_context()->is_valid(); - }; + auto exit_wait_condition = [this]() { + return this->optimization_request_ || !this->optimization_running_ || + !interfaces_.get_node_base_interface()->get_context()->is_valid(); + }; // Optimize constraints until told to exit - while (interfaces_.get_node_base_interface()->get_context()->is_valid() && - optimization_running_) + while (interfaces_.get_node_base_interface()->get_context()->is_valid() && optimization_running_) { // Wait for the next signal to start the next optimization cycle // NOTE(CH3): Uninitialized, but it's ok since it's meant to get overwritten. @@ -205,8 +181,7 @@ void FixedLagSmoother::optimizationLoop() optimization_deadline = optimization_deadline_; } // If a shutdown is requested, exit now. - if (!optimization_running_ || - !interfaces_.get_node_base_interface()->get_context()->is_valid()) + if (!optimization_running_ || !interfaces_.get_node_base_interface()->get_context()->is_valid()) { break; } @@ -224,7 +199,8 @@ void FixedLagSmoother::optimizationLoop() processQueue(*new_transaction, lag_expiration_); // Skip this optimization cycle if the transaction is empty because something failed while // processing the pending transactions queue. - if (new_transaction->empty()) { + if (new_transaction->empty()) + { continue; } // Prepare for selecting the marginal variables @@ -232,20 +208,21 @@ void FixedLagSmoother::optimizationLoop() // Combine the new transactions with any marginal transaction from the end of the last cycle new_transaction->merge(marginal_transaction_); // Update the graph - try { + try + { graph_->update(*new_transaction); - } catch (const std::exception & ex) { + } + catch (const std::exception& ex) + { std::ostringstream oss; oss << "Graph:\n"; graph_->print(oss); oss << "\nTransaction:\n"; new_transaction->print(oss); - RCLCPP_FATAL_STREAM( - logger_, - "Failed to update graph with transaction: " - << ex.what() << "\nLeaving optimization loop and requesting node shutdown...\n" - << oss.str()); + RCLCPP_FATAL_STREAM(logger_, "Failed to update graph with transaction: " + << ex.what() << "\nLeaving optimization loop and requesting node shutdown...\n" + << oss.str()); rclcpp::shutdown(); break; } @@ -258,12 +235,11 @@ void FixedLagSmoother::optimizationLoop() // Abort if optimization failed. Not converging is not a failure because the solution found is // usable. - if (!summary_.IsSolutionUsable()) { - 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..."); + if (!summary_.IsSolutionUsable()) + { + 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::shutdown(); break; @@ -272,21 +248,17 @@ void FixedLagSmoother::optimizationLoop() // Compute a transaction that marginalizes out those variables. lag_expiration_ = computeLagExpirationTime(); marginal_transaction_ = fuse_constraints::marginalizeVariables( - interfaces_.get_node_base_interface()->get_name(), - computeVariablesToMarginalize(lag_expiration_), - *graph_); + interfaces_.get_node_base_interface()->get_name(), computeVariablesToMarginalize(lag_expiration_), *graph_); // Perform any post-marginal cleanup postprocessMarginalization(marginal_transaction_); // Note: The marginal transaction will not be applied until the next optimization iteration // Log a warning if the optimization took too long auto optimization_complete = clock_->now(); - if (optimization_complete > optimization_deadline) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, - *clock_, - 10.0 * 1000, - "Optimization exceeded the configured duration by " - << (optimization_complete - optimization_deadline).nanoseconds() << "ns"); + if (optimization_complete > optimization_deadline) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Optimization exceeded the configured duration by " + << (optimization_complete - optimization_deadline).nanoseconds() << "ns"); } } } @@ -295,7 +267,8 @@ void FixedLagSmoother::optimizationLoop() void FixedLagSmoother::optimizerTimerCallback() { // If an "ignition" transaction hasn't been received, then we can't do anything yet. - if (!started_) { + if (!started_) + { return; } // If there is some pending work, trigger the next optimization cycle. @@ -306,7 +279,8 @@ void FixedLagSmoother::optimizerTimerCallback() std::lock_guard lock(pending_transactions_mutex_); optimization_request = !pending_transactions_.empty(); } - if (optimization_request) { + if (optimization_request) + { { std::lock_guard lock(optimization_requested_mutex_); optimization_request_ = true; @@ -316,14 +290,13 @@ void FixedLagSmoother::optimizerTimerCallback() } } -void FixedLagSmoother::processQueue( - fuse_core::Transaction & transaction, - const rclcpp::Time & lag_expiration) +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_); - if (pending_transactions_.empty()) { + if (pending_transactions_.empty()) + { return; } @@ -337,37 +310,40 @@ void FixedLagSmoother::processQueue( // been optimized yet, and they will have to use a default zero state instead. This can easily // lead to local minima because the variables in the graph are not initialized properly, i.e. they // do not take the ignition sensor transaction into account. - if (ignited_) { + if (ignited_) + { // The ignition sensor transaction is assumed to be at the end of the queue, because it must be // the oldest one. If there is more than one ignition sensor transaction in the queue, it is // always the oldest one that started things up. ignited_ = false; const auto transaction_rbegin = pending_transactions_.rbegin(); - auto & element = *transaction_rbegin; - if (!sensor_models_.at(element.sensor_name).ignition) { + auto& element = *transaction_rbegin; + if (!sensor_models_.at(element.sensor_name).ignition) + { // We just started, but the oldest transaction is not from an ignition sensor. We will still // process the transaction, but we do not enforce it is processed individually. - RCLCPP_ERROR_STREAM( - logger_, - "The queued transaction with timestamp " - << element.stamp().nanoseconds() << " from sensor " << element.sensor_name - << " is not an ignition sensor transaction. " - << "This transaction will not be processed individually."); - } else { - if (applyMotionModels(element.sensor_name, *element.transaction)) { + RCLCPP_ERROR_STREAM(logger_, "The queued transaction with timestamp " + << element.stamp().nanoseconds() << " from sensor " << element.sensor_name + << " is not an ignition sensor transaction. " + << "This transaction will not be processed individually."); + } + else + { + if (applyMotionModels(element.sensor_name, *element.transaction)) + { // Processing was successful. Add the results to the final transaction, delete this one, and // return, so the transaction from the ignition sensor is processed individually. transaction.merge(*element.transaction, true); erase(pending_transactions_, transaction_rbegin); - } else { + } + else + { // The motion model processing failed. When this happens to an ignition sensor transaction // there is no point on trying again next time, so we ignore this transaction. - RCLCPP_ERROR_STREAM( - logger_, - "The queued ignition transaction with timestamp " - << element.stamp().nanoseconds() << " from sensor " << element.sensor_name - << " could not be processed. Ignoring this ignition transaction."); + RCLCPP_ERROR_STREAM(logger_, "The queued ignition transaction with timestamp " + << element.stamp().nanoseconds() << " from sensor " << element.sensor_name + << " could not be processed. Ignoring this ignition transaction."); // Remove the ignition transaction that just failed and purge all transactions after it. But // if we find another ignition transaction, we schedule it to be processed in the next @@ -375,21 +351,22 @@ void FixedLagSmoother::processQueue( erase(pending_transactions_, transaction_rbegin); const auto pending_ignition_transaction_iter = - std::find_if( - pending_transactions_.rbegin(), pending_transactions_.rend(), - [this](const auto & element) { // NOLINT(whitespace/braces) - return sensor_models_.at(element.sensor_name).ignition; - }); // NOLINT(whitespace/braces) - if (pending_ignition_transaction_iter == pending_transactions_.rend()) { + std::find_if(pending_transactions_.rbegin(), pending_transactions_.rend(), + [this](const auto& element) { // NOLINT(whitespace/braces) + return sensor_models_.at(element.sensor_name).ignition; + }); // NOLINT(whitespace/braces) + if (pending_ignition_transaction_iter == pending_transactions_.rend()) + { // There is no other ignition transaction pending. We simply roll back to not started // state and all other pending transactions will be handled later in the transaction // callback, as usual. started_ = false; - } else { + } + else + { // Erase all transactions before the other ignition transaction pending. This other // ignition transaction will be processed in the next optimization cycle. - pending_transactions_.erase( - pending_ignition_transaction_iter.base(), pending_transactions_.rbegin().base()); + pending_transactions_.erase(pending_ignition_transaction_iter.base(), pending_transactions_.rbegin().base()); ignited_ = true; } } @@ -406,47 +383,52 @@ void FixedLagSmoother::processQueue( // Attempt to process each pending transaction auto sensor_blacklist = std::vector(); auto transaction_riter = pending_transactions_.rbegin(); - while (transaction_riter != pending_transactions_.rend()) { - auto & element = *transaction_riter; - const auto & min_stamp = element.minStamp(); - if (min_stamp < lag_expiration) { - RCLCPP_DEBUG_STREAM( - logger_, - "The current lag expiration time is " - << lag_expiration.nanoseconds() << ". The queued transaction with timestamp " - << element.stamp().nanoseconds() << " from sensor " << element.sensor_name - << " has a minimum involved timestamp of " << min_stamp.nanoseconds() << ", which is " - << (lag_expiration - min_stamp).nanoseconds() - << " seconds too old. Ignoring this transaction."); + while (transaction_riter != pending_transactions_.rend()) + { + auto& element = *transaction_riter; + const auto& min_stamp = element.minStamp(); + if (min_stamp < lag_expiration) + { + RCLCPP_DEBUG_STREAM(logger_, "The current lag expiration time is " + << lag_expiration.nanoseconds() << ". The queued transaction with timestamp " + << element.stamp().nanoseconds() << " from sensor " << element.sensor_name + << " has a minimum involved timestamp of " << min_stamp.nanoseconds() + << ", which is " << (lag_expiration - min_stamp).nanoseconds() + << " seconds too old. Ignoring this transaction."); transaction_riter = erase(pending_transactions_, transaction_riter); - } else if ( // NOLINT - std::find( - sensor_blacklist.begin(), sensor_blacklist.end(), element.sensor_name - ) != sensor_blacklist.end()) + } + else if ( // NOLINT + std::find(sensor_blacklist.begin(), sensor_blacklist.end(), element.sensor_name) != sensor_blacklist.end()) { // We should not process transactions from this sensor ++transaction_riter; - } else if (applyMotionModels(element.sensor_name, *element.transaction)) { + } + else if (applyMotionModels(element.sensor_name, *element.transaction)) + { // Processing was successful. Add the results to the final transaction, delete this one, and // move to the next. transaction.merge(*element.transaction, true); transaction_riter = erase(pending_transactions_, transaction_riter); - } else { + } + else + { // The motion model processing failed. // Check the transaction timeout to determine if it should be removed or skipped. - const auto & max_stamp = element.maxStamp(); - if (max_stamp + params_.transaction_timeout < current_time) { + const auto& max_stamp = element.maxStamp(); + if (max_stamp + params_.transaction_timeout < current_time) + { // Warn that this transaction has expired, then skip it. - RCLCPP_ERROR_STREAM( - logger_, - "The queued transaction with timestamp " - << element.stamp().nanoseconds() << " and maximum involved stamp of " - << max_stamp.nanoseconds() << " from sensor " << element.sensor_name - << " could not be processed after " << (current_time - max_stamp).nanoseconds() - << " seconds, which is greater than the 'transaction_timeout' value of " - << params_.transaction_timeout.nanoseconds() << ". Ignoring this transaction."); + RCLCPP_ERROR_STREAM(logger_, "The queued transaction with timestamp " + << element.stamp().nanoseconds() << " and maximum involved stamp of " + << max_stamp.nanoseconds() << " from sensor " << element.sensor_name + << " could not be processed after " << (current_time - max_stamp).nanoseconds() + << " seconds, which is greater than the 'transaction_timeout' value of " + << params_.transaction_timeout.nanoseconds() + << ". Ignoring this transaction."); transaction_riter = erase(pending_transactions_, transaction_riter); - } else { + } + else + { // The motion model failed. Stop further processing of this sensor and try again next time. sensor_blacklist.push_back(element.sensor_name); ++transaction_riter; @@ -455,10 +437,8 @@ void FixedLagSmoother::processQueue( } } -bool FixedLagSmoother::resetServiceCallback( - const std::shared_ptr, - std::shared_ptr -) +bool FixedLagSmoother::resetServiceCallback(const std::shared_ptr, + std::shared_ptr) { // Tell all the plugins to stop stopPlugins(); @@ -494,20 +474,17 @@ bool FixedLagSmoother::resetServiceCallback( return true; } -void FixedLagSmoother::transactionCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) +void FixedLagSmoother::transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) { // If this transaction occurs before the start time, just ignore it auto start_time = getStartTime(); const auto max_time = transaction->maxStamp(); - if (started_ && max_time < start_time) { - RCLCPP_DEBUG_STREAM( - logger_, - "Received a transaction before the start time from sensor '" - << sensor_name << "'.\n start_time: " << start_time.nanoseconds() - << ", maximum involved stamp: " << max_time.nanoseconds() << ", difference: " - << (start_time - max_time).nanoseconds() << "ns"); + if (started_ && max_time < start_time) + { + RCLCPP_DEBUG_STREAM(logger_, "Received a transaction before the start time from sensor '" + << sensor_name << "'.\n start_time: " << start_time.nanoseconds() + << ", maximum involved stamp: " << max_time.nanoseconds() + << ", difference: " << (start_time - max_time).nanoseconds() << "ns"); return; } { @@ -517,21 +494,19 @@ void FixedLagSmoother::transactionCallback( // 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 + 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_) { + if (!started_) + { // ...check if we should - if (sensor_models_.at(sensor_name).ignition) { + if (sensor_models_.at(sensor_name).ignition) + { started_ = true; ignited_ = true; start_time = position->minStamp(); @@ -545,31 +520,29 @@ void FixedLagSmoother::transactionCallback( // 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 { + 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) + 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) + while (!pending_transactions_.empty() && pending_transactions_.back().maxStamp() < purge_time) { pending_transactions_.pop_back(); } @@ -584,9 +557,7 @@ void FixedLagSmoother::transactionCallback( * @param[in] level The diagnostic status level * @param[in] message The diagnostic status message */ -diagnostic_msgs::msg::DiagnosticStatus makeDiagnosticStatus( - const int8_t level, - const std::string & message) +diagnostic_msgs::msg::DiagnosticStatus makeDiagnosticStatus(const int8_t level, const std::string& message) { diagnostic_msgs::msg::DiagnosticStatus status; @@ -609,27 +580,21 @@ diagnostic_msgs::msg::DiagnosticStatus makeDiagnosticStatus( * @return The diagnostic status with the level and message corresponding to the optimization * termination type */ -diagnostic_msgs::msg::DiagnosticStatus terminationTypeToDiagnosticStatus( - const ceres::TerminationType termination_type) +diagnostic_msgs::msg::DiagnosticStatus terminationTypeToDiagnosticStatus(const ceres::TerminationType termination_type) { - switch (termination_type) { + switch (termination_type) + { case ceres::TerminationType::CONVERGENCE: case ceres::TerminationType::USER_SUCCESS: - return makeDiagnosticStatus( - diagnostic_msgs::msg::DiagnosticStatus::OK, - "Optimization converged"); + return makeDiagnosticStatus(diagnostic_msgs::msg::DiagnosticStatus::OK, "Optimization converged"); case ceres::TerminationType::NO_CONVERGENCE: - return makeDiagnosticStatus( - diagnostic_msgs::msg::DiagnosticStatus::WARN, - "Optimization didn't converge"); + return makeDiagnosticStatus(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Optimization didn't converge"); default: - return makeDiagnosticStatus( - diagnostic_msgs::msg::DiagnosticStatus::ERROR, - "Optimization failed"); + return makeDiagnosticStatus(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Optimization failed"); } } -void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status) +void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) { Optimizer::setDiagnostics(status); @@ -642,24 +607,27 @@ void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrappe status.add("Pending Transactions", pending_transactions_.size()); } - if (started) { + if (started) + { // Add some optimization summary report fields to the diagnostics status if the optimizer has // started auto summary = decltype(summary_)(); { const std::unique_lock lock(optimization_mutex_, std::try_to_lock); - if (lock) { + if (lock) + { summary = summary_; - } else { + } + else + { status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Optimization running"); } } // summary.total_time_in_seconds is -1 if default-constructed - if (summary.total_time_in_seconds >= 0.0) { - status.add( - "Optimization Termination Type", - ceres::TerminationTypeToString(summary.termination_type)); + if (summary.total_time_in_seconds >= 0.0) + { + status.add("Optimization Termination Type", ceres::TerminationTypeToString(summary.termination_type)); status.add("Optimization Total Time [s]", summary.total_time_in_seconds); status.add("Optimization Iterations", summary.iterations.size()); status.add("Initial Cost", summary.initial_cost); @@ -673,18 +641,17 @@ void FixedLagSmoother::setDiagnostics(diagnostic_updater::DiagnosticStatusWrappe auto optimization_deadline = decltype(optimization_deadline_)(); { const std::unique_lock lock(optimization_requested_mutex_, std::try_to_lock); - if (lock) { + if (lock) + { optimization_deadline = optimization_deadline_; } } - if (0u != optimization_deadline.nanoseconds()) { + if (0u != optimization_deadline.nanoseconds()) + { const auto optimization_request_time = optimization_deadline - params_.optimization_period; - const auto time_since_last_optimization_request = - clock_->now() - optimization_request_time; - status.add( - "Time Since Last Optimization Request [s]", - time_since_last_optimization_request.seconds()); + const auto time_since_last_optimization_request = clock_->now() - optimization_request_time; + status.add("Time Since Last Optimization Request [s]", time_since_last_optimization_request.seconds()); } } } diff --git a/fuse_optimizers/src/fixed_lag_smoother_node.cpp b/fuse_optimizers/src/fixed_lag_smoother_node.cpp index 7be160efa..02a6d02d7 100644 --- a/fuse_optimizers/src/fixed_lag_smoother_node.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother_node.cpp @@ -37,7 +37,7 @@ #include #include -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared("fixed_lag_smoother_node"); diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp index e23bd3353..5d9571eb3 100644 --- a/fuse_optimizers/src/optimizer.cpp +++ b/fuse_optimizers/src/optimizer.cpp @@ -52,30 +52,22 @@ namespace fuse_optimizers { -Optimizer::Optimizer( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph -) -: interfaces_(interfaces), - clock_(interfaces.get_node_clock_interface()->get_clock()), - logger_(interfaces.get_node_logging_interface()->get_logger()), - graph_(std::move(graph)), - motion_model_loader_("fuse_core", "fuse_core::MotionModel"), - publisher_loader_("fuse_core", "fuse_core::Publisher"), - sensor_model_loader_("fuse_core", "fuse_core::SensorModel"), - diagnostic_updater_( - interfaces.get_node_base_interface(), - interfaces.get_node_clock_interface(), - interfaces.get_node_logging_interface(), - interfaces.get_node_parameters_interface(), - interfaces.get_node_timers_interface(), - interfaces.get_node_topics_interface() - ), - callback_queue_(std::make_shared( - interfaces_.get_node_base_interface() - ->get_context())) +Optimizer::Optimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph) + : interfaces_(interfaces) + , clock_(interfaces.get_node_clock_interface()->get_clock()) + , logger_(interfaces.get_node_logging_interface()->get_logger()) + , graph_(std::move(graph)) + , motion_model_loader_("fuse_core", "fuse_core::MotionModel") + , publisher_loader_("fuse_core", "fuse_core::Publisher") + , sensor_model_loader_("fuse_core", "fuse_core::SensorModel") + , diagnostic_updater_(interfaces.get_node_base_interface(), interfaces.get_node_clock_interface(), + interfaces.get_node_logging_interface(), interfaces.get_node_parameters_interface(), + interfaces.get_node_timers_interface(), interfaces.get_node_topics_interface()) + , callback_queue_(std::make_shared(interfaces_.get_node_base_interface()->get_context())) { - if (!graph) { + if (!graph) + { fuse_graphs::HashGraphParams hash_graph_params; hash_graph_params.loadFromROS(interfaces_); graph_ = std::move(fuse_graphs::HashGraph::make_unique(hash_graph_params)); @@ -83,11 +75,9 @@ Optimizer::Optimizer( // add a ros1 style callback queue so that transactions can be processed in the optimiser's // executor - interfaces_.get_node_waitables_interface()->add_waitable( - callback_queue_, (rclcpp::CallbackGroup::SharedPtr) nullptr); + interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, (rclcpp::CallbackGroup::SharedPtr) nullptr); - diagnostic_updater_.add( - interfaces_.get_node_base_interface()->get_namespace(), this, &Optimizer::setDiagnostics); + diagnostic_updater_.add(interfaces_.get_node_base_interface()->get_namespace(), this, &Optimizer::setDiagnostics); diagnostic_updater_.setHardwareID("fuse"); // Wait for a valid time before loading any of the plugins @@ -122,48 +112,44 @@ void Optimizer::loadMotionModels() std::vector motion_model_config; std::unordered_set motion_model_names = - fuse_core::list_parameter_override_prefixes( - interfaces_, "motion_models."); + fuse_core::list_parameter_override_prefixes(interfaces_, "motion_models."); // declare config parameters for each model - for (const auto & param_name : motion_model_names) { - ModelConfig & config = motion_model_config.emplace_back(); + for (const auto& param_name : motion_model_names) + { + ModelConfig& config = motion_model_config.emplace_back(); config.name = param_name.substr(param_name.rfind('.') + 1); config.param_name = param_name + ".type"; - if (!interfaces_.get_node_parameters_interface()->has_parameter(config.param_name)) { + if (!interfaces_.get_node_parameters_interface()->has_parameter(config.param_name)) + { rcl_interfaces::msg::ParameterDescriptor descr; - descr.description = - "the PLUGINLIB type string to load for this motion_model (eg: 'fuse_models::Unicycle2D')"; - interfaces_.get_node_parameters_interface()->declare_parameter( - config.param_name, - rclcpp::ParameterValue(std::string()), - descr - ); + descr.description = "the PLUGINLIB type string to load for this motion_model (eg: 'fuse_models::Unicycle2D')"; + interfaces_.get_node_parameters_interface()->declare_parameter(config.param_name, + rclcpp::ParameterValue(std::string()), descr); } // get the type parameter for the motion model rclcpp::Parameter motion_model_type_param = - interfaces_.get_node_parameters_interface()->get_parameter(config.param_name); + interfaces_.get_node_parameters_interface()->get_parameter(config.param_name); // extract the type string from the parameter - if (motion_model_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) { + if (motion_model_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + { config.type = motion_model_type_param.as_string(); } // quickly check for common errors - if (config.type == "") { - RCLCPP_WARN_STREAM( - logger_, - "parameter '" << config.param_name << - "' should be the string of a motion_model type " << - "for the motion_model named '" << config.name << - "'."); + if (config.type == "") + { + RCLCPP_WARN_STREAM(logger_, "parameter '" << config.param_name << "' should be the string of a motion_model type " + << "for the motion_model named '" << config.name << "'."); } } // now load the models defined above - for (const ModelConfig & config : motion_model_config) { + for (const ModelConfig& config : motion_model_config) + { // Create a motion_model object using pluginlib. This will throw if the plugin name is not // found. auto motion_model = motion_model_loader_.createUniqueInstance(config.type); @@ -194,120 +180,110 @@ void Optimizer::loadSensorModels() std::vector sensor_model_config; std::unordered_set sensor_model_names = - fuse_core::list_parameter_override_prefixes( - interfaces_, "sensor_models."); + fuse_core::list_parameter_override_prefixes(interfaces_, "sensor_models."); // declare config parameters for each model - for (const auto & param_name : sensor_model_names) { - ModelConfig & config = sensor_model_config.emplace_back(); + for (const auto& param_name : sensor_model_names) + { + ModelConfig& config = sensor_model_config.emplace_back(); config.name = param_name.substr(param_name.rfind('.') + 1); config.type_param_name = param_name + ".type"; config.models_param_name = param_name + ".motion_models"; config.ignition_param_name = param_name + ".ignition"; - // get the type parameter for the sensor model - if (!interfaces_.get_node_parameters_interface()->has_parameter(config.type_param_name)) { + if (!interfaces_.get_node_parameters_interface()->has_parameter(config.type_param_name)) + { rcl_interfaces::msg::ParameterDescriptor descr; - descr.description = - "the PLUGINLIB type string to load for this sensor_model " - "(eg: 'fuse_models::Acceleration2D')"; - interfaces_.get_node_parameters_interface()->declare_parameter( - config.type_param_name, - rclcpp::ParameterValue(std::string()), - descr - ); + descr.description = "the PLUGINLIB type string to load for this sensor_model " + "(eg: 'fuse_models::Acceleration2D')"; + interfaces_.get_node_parameters_interface()->declare_parameter(config.type_param_name, + rclcpp::ParameterValue(std::string()), descr); } // get the type parameter for the sensor model rclcpp::Parameter sensor_model_type_param = - interfaces_.get_node_parameters_interface()->get_parameter(config.type_param_name); + interfaces_.get_node_parameters_interface()->get_parameter(config.type_param_name); // extract the type string from the parameter - if (sensor_model_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) { + if (sensor_model_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + { config.type = sensor_model_type_param.as_string(); } - // get the type parameter for the sensor model - if (!interfaces_.get_node_parameters_interface()->has_parameter(config.models_param_name)) { + if (!interfaces_.get_node_parameters_interface()->has_parameter(config.models_param_name)) + { rcl_interfaces::msg::ParameterDescriptor descr; descr.description = "the list of motion models this sensor is associated with"; interfaces_.get_node_parameters_interface()->declare_parameter( - config.models_param_name, - rclcpp::ParameterValue(std::vector()), - descr - ); + config.models_param_name, rclcpp::ParameterValue(std::vector()), descr); } // get the model_list parameter for the sensor model rclcpp::Parameter sensor_model_model_list_param = - interfaces_.get_node_parameters_interface()->get_parameter(config.models_param_name); + interfaces_.get_node_parameters_interface()->get_parameter(config.models_param_name); // extract the model_list string from the parameter - if (sensor_model_model_list_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) { + if (sensor_model_model_list_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) + { config.associated_motion_models = sensor_model_model_list_param.as_string_array(); } - // get the ignition parameter for the sensor model - if (!interfaces_.get_node_parameters_interface()->has_parameter(config.ignition_param_name)) { + if (!interfaces_.get_node_parameters_interface()->has_parameter(config.ignition_param_name)) + { rcl_interfaces::msg::ParameterDescriptor descr; descr.description = "does the first message for this sensor start the optimizer"; - interfaces_.get_node_parameters_interface()->declare_parameter( - config.ignition_param_name, - rclcpp::ParameterValue(false), - descr - ); + interfaces_.get_node_parameters_interface()->declare_parameter(config.ignition_param_name, + rclcpp::ParameterValue(false), descr); } // get the model list parameter for the sensor model rclcpp::Parameter sensor_model_ignition_param = - interfaces_.get_node_parameters_interface()->get_parameter(config.ignition_param_name); + interfaces_.get_node_parameters_interface()->get_parameter(config.ignition_param_name); // extract the ignition bool from the parameter - if (sensor_model_ignition_param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) { + if (sensor_model_ignition_param.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) + { config.ignition = sensor_model_ignition_param.as_bool(); } - // quickly check for common errors - if (config.type == "") { - RCLCPP_WARN_STREAM( - logger_, - "parameter '" << config.type_param_name << "' should be the string of a sensor_model type " - << "for the sensor_model named '" << config.name << "'."); + if (config.type == "") + { + RCLCPP_WARN_STREAM(logger_, "parameter '" << config.type_param_name + << "' should be the string of a sensor_model type " + << "for the sensor_model named '" << config.name << "'."); } } // now load the models defined above - for (const ModelConfig & config : sensor_model_config) { + for (const ModelConfig& config : sensor_model_config) + { // Create a sensor object using pluginlib. This will throw if the plugin name is not found. auto sensor_model = sensor_model_loader_.createUniqueInstance(config.type); // Initialize the sensor - sensor_model->initialize( - interfaces_, config.name, - std::bind(&Optimizer::injectCallback, this, config.name, std::placeholders::_1)); + sensor_model->initialize(interfaces_, config.name, + std::bind(&Optimizer::injectCallback, this, config.name, std::placeholders::_1)); // Store the sensor in a member variable for use later - sensor_models_.emplace( - config.name, - SensorModelInfo{std::move(sensor_model), config.ignition}); // NOLINT(whitespace/braces) + sensor_models_.emplace(config.name, + SensorModelInfo{ std::move(sensor_model), config.ignition }); // NOLINT(whitespace/braces) // Parse out the list of associated motion models, if any associated_motion_models_[config.name] = config.associated_motion_models; - for (const auto & motion_model_name : config.associated_motion_models) { - if (motion_models_.find(motion_model_name) == motion_models_.end()) { - RCLCPP_WARN_STREAM( - logger_, - "Sensor model '" - << config.name << "' is configured to use motion model '" << motion_model_name - << "', but no motion model with that name currently exists. " - << "This is likely a configuration error."); + for (const auto& motion_model_name : config.associated_motion_models) + { + if (motion_models_.find(motion_model_name) == motion_models_.end()) + { + RCLCPP_WARN_STREAM(logger_, "Sensor model '" << config.name << "' is configured to use motion model '" + << motion_model_name + << "', but no motion model with that name currently exists. " + << "This is likely a configuration error."); } } } diagnostic_updater_.force_update(); } - void Optimizer::loadPublishers() { // struct for readability @@ -322,49 +298,45 @@ void Optimizer::loadPublishers() std::vector publisher_config; std::unordered_set publisher_names = - fuse_core::list_parameter_override_prefixes( - interfaces_, "publishers."); + fuse_core::list_parameter_override_prefixes(interfaces_, "publishers."); // declare config parameters for each model - for (const auto & param_name : publisher_names) { - PublisherConfig & config = publisher_config.emplace_back(); + for (const auto& param_name : publisher_names) + { + PublisherConfig& config = publisher_config.emplace_back(); config.name = param_name.substr(param_name.rfind('.') + 1); config.param_name = param_name + ".type"; - if (!interfaces_.get_node_parameters_interface()->has_parameter(config.param_name)) { + if (!interfaces_.get_node_parameters_interface()->has_parameter(config.param_name)) + { rcl_interfaces::msg::ParameterDescriptor descr; - descr.description = - "the PLUGINLIB type string to load for this publisher " - "(eg: 'fuse_publishers::Path2DPublisher')"; - interfaces_.get_node_parameters_interface()->declare_parameter( - config.param_name, - rclcpp::ParameterValue(std::string()), - descr - ); + descr.description = "the PLUGINLIB type string to load for this publisher " + "(eg: 'fuse_publishers::Path2DPublisher')"; + interfaces_.get_node_parameters_interface()->declare_parameter(config.param_name, + rclcpp::ParameterValue(std::string()), descr); } // get the type parameter for the publisher rclcpp::Parameter publisher_type_param = - interfaces_.get_node_parameters_interface()->get_parameter(config.param_name); + interfaces_.get_node_parameters_interface()->get_parameter(config.param_name); // extract the type string from the parameter - if (publisher_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) { + if (publisher_type_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + { config.type = publisher_type_param.as_string(); } // quickly check for common errors - if (config.type == "") { - RCLCPP_WARN_STREAM( - logger_, - "parameter '" << config.param_name << - "' should be the string of a publisher type " << - "for the publisher named '" << config.name << - "'."); + if (config.type == "") + { + RCLCPP_WARN_STREAM(logger_, "parameter '" << config.param_name << "' should be the string of a publisher type " + << "for the publisher named '" << config.name << "'."); } } // now load the models defined above - for (const PublisherConfig & config : publisher_config) { + for (const PublisherConfig& config : publisher_config) + { // Create a publisher object using pluginlib. This will throw if the plugin name is not found. auto publisher = publisher_loader_.createUniqueInstance(config.type); // Initialize the publisher @@ -376,82 +348,85 @@ void Optimizer::loadPublishers() diagnostic_updater_.force_update(); } -bool Optimizer::applyMotionModels( - const std::string & sensor_name, - fuse_core::Transaction & transaction) const +bool Optimizer::applyMotionModels(const std::string& sensor_name, fuse_core::Transaction& transaction) const { // Check for trivial cases where we don't have to do anything auto iter = associated_motion_models_.find(sensor_name); - if (iter == associated_motion_models_.end()) { + if (iter == associated_motion_models_.end()) + { return true; } // Generate constraints for each configured motion model - const auto & motion_model_names = iter->second; + const auto& motion_model_names = iter->second; bool success = true; - for (const auto & motion_model_name : motion_model_names) { - try { + for (const auto& motion_model_name : motion_model_names) + { + try + { success &= motion_models_.at(motion_model_name)->apply(transaction); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - logger_, - "Error generating constraints for sensor '" << sensor_name << "' from motion model '" - << motion_model_name << "'. Error: " << - e.what()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM(logger_, "Error generating constraints for sensor '" << sensor_name << "' from motion model '" + << motion_model_name + << "'. Error: " << e.what()); success = false; } } return success; } -void Optimizer::notify( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) +void Optimizer::notify(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph) { - for (const auto & name__sensor_model : sensor_models_) { - try { + for (const auto& name__sensor_model : sensor_models_) + { + try + { name__sensor_model.second.model->graphCallback(graph); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - logger_, - "Failed calling graphCallback() on sensor '" << name__sensor_model.first - << "'. Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM(logger_, "Failed calling graphCallback() on sensor '" << name__sensor_model.first + << "'. Error: " << e.what()); continue; } } - for (const auto & name__motion_model : motion_models_) { - try { + for (const auto& name__motion_model : motion_models_) + { + try + { name__motion_model.second->graphCallback(graph); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - logger_, - "Failed calling graphCallback() on motion model '" << name__motion_model.first - << ". Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM(logger_, "Failed calling graphCallback() on motion model '" << name__motion_model.first + << ". Error: " << e.what()); continue; } } - for (const auto & name__publisher : publishers_) { - try { + for (const auto& name__publisher : publishers_) + { + try + { name__publisher.second->notify(transaction, graph); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - logger_, - "Failed calling notify() on publisher '" << name__publisher.first - << ". Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM(logger_, + "Failed calling notify() on publisher '" << name__publisher.first << ". Error: " << e.what()); continue; } } } -void Optimizer::injectCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) +void Optimizer::injectCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) { // We are going to insert a call to the derived class's transactionCallback() method into the // global callback queue. This returns execution to the sensor's thread quickly by moving the // transaction processing to the optimizer's thread. And by using the existing ROS callback queue, // we simplify the threading model of the optimizer. auto callback = std::make_shared>( - std::bind(&Optimizer::transactionCallback, this, sensor_name, std::move(transaction))); + std::bind(&Optimizer::transactionCallback, this, sensor_name, std::move(transaction))); callback_queue_->addCallback(callback); } @@ -462,13 +437,16 @@ void Optimizer::clearCallbacks() void Optimizer::startPlugins() { - for (const auto & name_plugin : motion_models_) { + for (const auto& name_plugin : motion_models_) + { name_plugin.second->start(); } - for (const auto & name_plugin : sensor_models_) { + for (const auto& name_plugin : sensor_models_) + { name_plugin.second.model->start(); } - for (const auto & name_plugin : publishers_) { + for (const auto& name_plugin : publishers_) + { name_plugin.second->start(); } @@ -477,22 +455,26 @@ void Optimizer::startPlugins() void Optimizer::stopPlugins() { - for (const auto & name_plugin : publishers_) { + for (const auto& name_plugin : publishers_) + { name_plugin.second->stop(); } - for (const auto & name_plugin : sensor_models_) { + for (const auto& name_plugin : sensor_models_) + { name_plugin.second.model->stop(); } - for (const auto & name_plugin : motion_models_) { + for (const auto& name_plugin : motion_models_) + { name_plugin.second->stop(); } diagnostic_updater_.force_update(); } -void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & status) +void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) { - if (!clock_->started()) { + if (!clock_->started()) + { status.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Waiting for valid ROS time"); return; } @@ -501,20 +483,11 @@ void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & sta status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Optimizer exists"); - auto print_key = [](const std::string & result, const auto & entry) - { - return result + entry.first + ' '; - }; - - status.add( - "Sensor Models", - std::accumulate(sensor_models_.begin(), sensor_models_.end(), std::string(), print_key)); - status.add( - "Motion Models", - std::accumulate(motion_models_.begin(), motion_models_.end(), std::string(), print_key)); - status.add( - "Publishers", - std::accumulate(publishers_.begin(), publishers_.end(), std::string(), print_key)); + auto print_key = [](const std::string& result, const auto& entry) { return result + entry.first + ' '; }; + + status.add("Sensor Models", std::accumulate(sensor_models_.begin(), sensor_models_.end(), std::string(), print_key)); + status.add("Motion Models", std::accumulate(motion_models_.begin(), motion_models_.end(), std::string(), print_key)); + status.add("Publishers", std::accumulate(publishers_.begin(), publishers_.end(), std::string(), print_key)); } } // namespace fuse_optimizers diff --git a/fuse_optimizers/src/variable_stamp_index.cpp b/fuse_optimizers/src/variable_stamp_index.cpp index 60038053d..a2fbd3a04 100644 --- a/fuse_optimizers/src/variable_stamp_index.cpp +++ b/fuse_optimizers/src/variable_stamp_index.cpp @@ -46,19 +46,21 @@ namespace fuse_optimizers { rclcpp::Time VariableStampIndex::currentStamp() const { - auto compare_stamps = [](const StampedMap::value_type & lhs, const StampedMap::value_type & rhs) - { - return lhs.second < rhs.second; - }; + auto compare_stamps = [](const StampedMap::value_type& lhs, const StampedMap::value_type& rhs) { + return lhs.second < rhs.second; + }; auto iter = std::max_element(stamped_index_.begin(), stamped_index_.end(), compare_stamps); - if (iter != stamped_index_.end()) { + if (iter != stamped_index_.end()) + { return iter->second; - } else { + } + else + { return rclcpp::Time(0, 0, RCL_ROS_TIME); } } -void VariableStampIndex::addNewTransaction(const fuse_core::Transaction & transaction) +void VariableStampIndex::addNewTransaction(const fuse_core::Transaction& transaction) { applyAddedVariables(transaction); applyAddedConstraints(transaction); @@ -66,7 +68,7 @@ void VariableStampIndex::addNewTransaction(const fuse_core::Transaction & transa applyRemovedVariables(transaction); } -void VariableStampIndex::addMarginalTransaction(const fuse_core::Transaction & transaction) +void VariableStampIndex::addMarginalTransaction(const fuse_core::Transaction& transaction) { // Only the removed variables and removed constraints should be applied to the VariableStampIndex // No variables will be added by a marginal transaction, and the added constraints add variable @@ -76,42 +78,47 @@ void VariableStampIndex::addMarginalTransaction(const fuse_core::Transaction & t applyRemovedVariables(transaction); } -void VariableStampIndex::applyAddedConstraints(const fuse_core::Transaction & transaction) +void VariableStampIndex::applyAddedConstraints(const fuse_core::Transaction& transaction) { - for (const auto & constraint : transaction.addedConstraints()) { - constraints_[constraint.uuid()].insert( - constraint.variables().begin(), - constraint.variables().end()); - for (const auto & variable_uuid : constraint.variables()) { + for (const auto& constraint : transaction.addedConstraints()) + { + constraints_[constraint.uuid()].insert(constraint.variables().begin(), constraint.variables().end()); + for (const auto& variable_uuid : constraint.variables()) + { variables_[variable_uuid].insert(constraint.uuid()); } } } -void VariableStampIndex::applyAddedVariables(const fuse_core::Transaction & transaction) +void VariableStampIndex::applyAddedVariables(const fuse_core::Transaction& transaction) { - for (const auto & variable : transaction.addedVariables()) { - auto stamped_variable = dynamic_cast(&variable); - if (stamped_variable) { + for (const auto& variable : transaction.addedVariables()) + { + auto stamped_variable = dynamic_cast(&variable); + if (stamped_variable) + { stamped_index_[variable.uuid()] = stamped_variable->stamp(); } variables_[variable.uuid()]; // Add an empty set of constraints } } -void VariableStampIndex::applyRemovedConstraints(const fuse_core::Transaction & transaction) +void VariableStampIndex::applyRemovedConstraints(const fuse_core::Transaction& transaction) { - for (const auto & constraint_uuid : transaction.removedConstraints()) { - for (auto & variable_uuid : constraints_[constraint_uuid]) { + for (const auto& constraint_uuid : transaction.removedConstraints()) + { + for (auto& variable_uuid : constraints_[constraint_uuid]) + { variables_[variable_uuid].erase(constraint_uuid); } constraints_.erase(constraint_uuid); } } -void VariableStampIndex::applyRemovedVariables(const fuse_core::Transaction & transaction) +void VariableStampIndex::applyRemovedVariables(const fuse_core::Transaction& transaction) { - for (const auto & variable_uuid : transaction.removedVariables()) { + for (const auto& variable_uuid : transaction.removedVariables()) + { stamped_index_.erase(variable_uuid); variables_.erase(variable_uuid); } diff --git a/fuse_optimizers/test/CMakeLists.txt b/fuse_optimizers/test/CMakeLists.txt index a4e47bb3a..e8c5f1ca4 100644 --- a/fuse_optimizers/test/CMakeLists.txt +++ b/fuse_optimizers/test/CMakeLists.txt @@ -1,34 +1,28 @@ -# CORE GTESTS ====================================================================================== +# CORE GTESTS +# ====================================================================================== ament_add_gtest(test_variable_stamp_index "test_variable_stamp_index.cpp") target_link_libraries(test_variable_stamp_index ${PROJECT_NAME}) - -# ROS TESTS (WITH LAUNCH) ========================================================================== +# ROS TESTS (WITH LAUNCH) +# ========================================================================== find_package(ament_cmake_pytest REQUIRED) -ament_add_gtest_executable(test_fixed_lag_ignition launch_tests/test_fixed_lag_ignition.cpp) -target_link_libraries(test_fixed_lag_ignition ${PROJECT_NAME} ${nav_msgs_TARGETS}) +ament_add_gtest_executable(test_fixed_lag_ignition + launch_tests/test_fixed_lag_ignition.cpp) +target_link_libraries(test_fixed_lag_ignition ${PROJECT_NAME} + ${nav_msgs_TARGETS}) ament_add_pytest_test( - test_fixed_lag_ignition_py - "launch_tests/test_fixed_lag_ignition.py" - WORKING_DIRECTORY - "${CMAKE_CURRENT_BINARY_DIR}" -) + test_fixed_lag_ignition_py "launch_tests/test_fixed_lag_ignition.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") ament_add_gtest_executable(test_optimizer launch_tests/test_optimizer.cpp) target_link_libraries(test_optimizer ${PROJECT_NAME} ${nav_msgs_TARGETS}) -ament_add_pytest_test( - test_optimizer_py - "launch_tests/test_optimizer.py" - WORKING_DIRECTORY - "${CMAKE_CURRENT_BINARY_DIR}" -) +ament_add_pytest_test(test_optimizer_py "launch_tests/test_optimizer.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") -configure_file( - "launch_tests/config/optimizer_params.yaml" - "launch_tests/config/optimizer_params.yaml" COPYONLY) -configure_file( - "launch_tests/config/fixed_lag_ignition_params.yaml" - "launch_tests/config/fixed_lag_ignition_params.yaml" COPYONLY) +configure_file("launch_tests/config/optimizer_params.yaml" + "launch_tests/config/optimizer_params.yaml" COPYONLY) +configure_file("launch_tests/config/fixed_lag_ignition_params.yaml" + "launch_tests/config/fixed_lag_ignition_params.yaml" COPYONLY) diff --git a/fuse_optimizers/test/launch_tests/common.hpp b/fuse_optimizers/test/launch_tests/common.hpp index 43d1ca0a2..3beac67fc 100644 --- a/fuse_optimizers/test/launch_tests/common.hpp +++ b/fuse_optimizers/test/launch_tests/common.hpp @@ -42,7 +42,6 @@ #include #include - /** * @brief Helper function to print the elements of std::vector objects * @@ -50,12 +49,13 @@ * @param[in] v A vector to print into the stream * @return The output stream with the vector printed into it */ -template -std::ostream & operator<<(std::ostream & os, const std::vector & v) +template +std::ostream& operator<<(std::ostream& os, const std::vector& v) { os << '['; - if (!v.empty()) { + if (!v.empty()) + { std::copy(v.begin(), v.end() - 1, std::ostream_iterator(os, ", ")); os << v.back(); } @@ -72,12 +72,13 @@ std::ostream & operator<<(std::ostream & os, const std::vector & v) * @param[in] m An unordered map with the keys to print into the stream * @return The output stream with the vector printed into it */ -template -std::ostream & operator<<(std::ostream & os, const std::unordered_map & m) +template +std::ostream& operator<<(std::ostream& os, const std::unordered_map& m) { os << '['; - for (const auto & entry : m) { + for (const auto& entry : m) + { os << entry.first << ", "; } @@ -94,28 +95,23 @@ std::ostream & operator<<(std::ostream & os, const std::unordered_map & m) * @param[in] rhs An unordered map of key strings * @return A vector with the symmetric difference strings */ -template -std::vector set_symmetric_difference( - const std::vector & lhs, - const std::unordered_map & rhs) +template +std::vector set_symmetric_difference(const std::vector& lhs, + const std::unordered_map& rhs) { // Retrieve the keys: std::vector rhs_keys; - std::transform( - rhs.begin(), rhs.end(), std::back_inserter(rhs_keys), - [](const auto & pair) {return pair.first;}); // NOLINT(whitespace/braces) + std::transform(rhs.begin(), rhs.end(), std::back_inserter(rhs_keys), + [](const auto& pair) { return pair.first; }); // NOLINT(whitespace/braces) // Sort the keys so we can use std::set_symmetric_difference: std::sort(rhs_keys.begin(), rhs_keys.end()); // Compute the symmetric difference: std::vector diff; - std::set_symmetric_difference( - lhs.begin(), lhs.end(), rhs_keys.begin(), - rhs_keys.end(), std::back_inserter(diff)); + std::set_symmetric_difference(lhs.begin(), lhs.end(), rhs_keys.begin(), rhs_keys.end(), std::back_inserter(diff)); return diff; } - #endif // FUSE_OPTIMIZERS__TEST_COMMON_HPP_ // NOLINT{build/header_guard} diff --git a/fuse_optimizers/test/launch_tests/config/fixed_lag_ignition_params.yaml b/fuse_optimizers/test/launch_tests/config/fixed_lag_ignition_params.yaml index d6975857e..fe34ab6d4 100644 --- a/fuse_optimizers/test/launch_tests/config/fixed_lag_ignition_params.yaml +++ b/fuse_optimizers/test/launch_tests/config/fixed_lag_ignition_params.yaml @@ -1,51 +1,45 @@ fixed_lag_node: ros__parameters: - optimization_frequency: 2.0 - transaction_timeout: 5.0 - lag_duration: 5.0 - - - solver_options: - max_num_iterations: 0 - - - motion_models: - unicycle_motion_model: - type: fuse_models::Unicycle2D - - - sensor_models: - unicycle_ignition_sensor: - type: fuse_models::Unicycle2DIgnition - motion_models: [unicycle_motion_model] - ignition: true - pose_sensor: - type: fuse_models::Pose2D - motion_models: [unicycle_motion_model] - - - publishers: - - odometry_publisher: - type: fuse_models::Odometry2DPublisher + optimization_frequency: 2.0 + transaction_timeout: 5.0 + lag_duration: 5.0 + solver_options: + max_num_iterations: 0 + motion_models: unicycle_motion_model: - buffer_length: 5.0 - process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + type: fuse_models::Unicycle2D + sensor_models: unicycle_ignition_sensor: - publish_on_startup: false - initial_state: [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] - + type: fuse_models::Unicycle2DIgnition + motion_models: [unicycle_motion_model] + ignition: true pose_sensor: - differential: true - topic: relative_pose - position_dimensions: ['x', 'y'] - orientation_dimensions: ['yaw'] + type: fuse_models::Pose2D + motion_models: [unicycle_motion_model] + publishers: odometry_publisher: - topic: odom - world_frame_id: map - publish_tf: false + type: fuse_models::Odometry2DPublisher + + unicycle_motion_model: + buffer_length: 5.0 + process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + + unicycle_ignition_sensor: + publish_on_startup: false + initial_state: [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] + + pose_sensor: + differential: true + topic: relative_pose + position_dimensions: ["x", "y"] + orientation_dimensions: ["yaw"] + + odometry_publisher: + topic: odom + world_frame_id: map + publish_tf: false diff --git a/fuse_optimizers/test/launch_tests/example_optimizer.hpp b/fuse_optimizers/test/launch_tests/example_optimizer.hpp index 5b6f5cc98..1aed0d680 100644 --- a/fuse_optimizers/test/launch_tests/example_optimizer.hpp +++ b/fuse_optimizers/test/launch_tests/example_optimizer.hpp @@ -40,7 +40,6 @@ #include - /** * @brief Example optimizer that exposes the motion and sensor models, and the publishers, so we can * check the expected ones are loaded. @@ -50,32 +49,28 @@ class ExampleOptimizer : public fuse_optimizers::Optimizer public: FUSE_SMART_PTR_DEFINITIONS(ExampleOptimizer) - ExampleOptimizer( - fuse_core::node_interfaces::NodeInterfaces interfaces, - fuse_core::Graph::UniquePtr graph = nullptr - ) - : fuse_optimizers::Optimizer(interfaces, std::move(graph)) + ExampleOptimizer(fuse_core::node_interfaces::NodeInterfaces interfaces, + fuse_core::Graph::UniquePtr graph = nullptr) + : fuse_optimizers::Optimizer(interfaces, std::move(graph)) { } - const MotionModels & getMotionModels() const + const MotionModels& getMotionModels() const { return motion_models_; } - const SensorModels & getSensorModels() const + const SensorModels& getSensorModels() const { return sensor_models_; } - const Publishers & getPublishers() const + const Publishers& getPublishers() const { return publishers_; } - void transactionCallback( - const std::string & sensor_name, - fuse_core::Transaction::SharedPtr transaction) override + void transactionCallback(const std::string& sensor_name, fuse_core::Transaction::SharedPtr transaction) override { (void)sensor_name; (void)transaction; diff --git a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp index eafd79d6e..aff3502e1 100644 --- a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp +++ b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp @@ -54,16 +54,14 @@ class FixedLagIgnitionFixture : public ::testing::Test void SetUp() override { executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); @@ -81,7 +79,7 @@ class FixedLagIgnitionFixture : public ::testing::Test return received_odom_msg_; } - std::thread spinner_; //!< Internal thread for spinning the executor + std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; nav_msgs::msg::Odometry::SharedPtr received_odom_msg_; std::mutex received_odom_mutex_; @@ -93,12 +91,10 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) executor_->add_node(node); auto relative_pose_publisher = - node->create_publisher( - "/relative_pose", 5); + node->create_publisher("/relative_pose", 5); - auto odom_subscriber = - node->create_subscription( - "/odom", 5, std::bind(&FixedLagIgnitionFixture::odom_callback, this, std::placeholders::_1)); + auto odom_subscriber = node->create_subscription( + "/odom", 5, std::bind(&FixedLagIgnitionFixture::odom_callback, this, std::placeholders::_1)); // Time should be valid after rclcpp::init() returns in main(). But it doesn't hurt to verify. ASSERT_TRUE(node->get_clock()->wait_until_started(rclcpp::Duration::from_seconds(1.0))); @@ -130,8 +126,7 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) // The 'set_pose' service call triggers all of the sensors to resubscribe to their topics. // I need to wait for those subscribers to be ready before sending them sensor data. rclcpp::Time subscriber_timeout = node->now() + rclcpp::Duration::from_seconds(10.0); - while ((relative_pose_publisher->get_subscription_count() < 1u) && - (node->now() < subscriber_timeout)) + while ((relative_pose_publisher->get_subscription_count() < 1u) && (node->now() < subscriber_timeout)) { rclcpp::sleep_for(std::chrono::milliseconds(10)); } @@ -175,8 +170,7 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) // Wait for the optimizer to process all queued transactions and publish the last odometry msg rclcpp::Time result_timeout = node->now() + rclcpp::Duration::from_seconds(1.0); auto odom_msg = nav_msgs::msg::Odometry::SharedPtr(); - while ((!odom_msg || odom_msg->header.stamp != rclcpp::Time(3, 0, - RCL_ROS_TIME)) && (node->now() < result_timeout)) + while ((!odom_msg || odom_msg->header.stamp != rclcpp::Time(3, 0, RCL_ROS_TIME)) && (node->now() < result_timeout)) { rclcpp::sleep_for(std::chrono::milliseconds(100)); odom_msg = this->get_last_odom_msg(); @@ -194,7 +188,7 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) } // NOTE(CH3): This main is required because the test is manually run by a launch test -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); diff --git a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.py b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.py index da1a501c6..db028d0a6 100755 --- a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.py +++ b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.py @@ -30,31 +30,36 @@ @pytest.fixture def test_proc(): - test_root = '.' - test_path = os.path.join(test_root, 'test_fixed_lag_ignition') + test_root = "." + test_path = os.path.join(test_root, "test_fixed_lag_ignition") cmd = [test_path] - return ExecuteProcess(cmd=cmd, shell=True, output='screen', cached_output=True) + return ExecuteProcess(cmd=cmd, shell=True, output="screen", cached_output=True) @launch_pytest.fixture def generate_test_description(test_proc): - test_root = '.' + test_root = "." return LaunchDescription( [ test_proc, Node( - package='fuse_optimizers', - executable='fixed_lag_smoother_node', - name='fixed_lag_node', - output='screen', + package="fuse_optimizers", + executable="fixed_lag_smoother_node", + name="fixed_lag_node", + output="screen", parameters=[ PathJoinSubstitution( - [test_root, 'launch_tests', 'config', 'fixed_lag_ignition_params.yaml'] + [ + test_root, + "launch_tests", + "config", + "fixed_lag_ignition_params.yaml", + ] ) ], ), - ReadyToTest() + ReadyToTest(), ] ) @@ -62,4 +67,4 @@ def generate_test_description(test_proc): @pytest.mark.launch(fixture=generate_test_description) async def test_no_failed_gtests(test_proc, launch_context): await process_tools.wait_for_exit(launch_context, test_proc, timeout=30) - assert test_proc.return_code == 0, 'GTests failed' + assert test_proc.return_code == 0, "GTests failed" diff --git a/fuse_optimizers/test/launch_tests/test_optimizer.cpp b/fuse_optimizers/test/launch_tests/test_optimizer.cpp index 1e765ea19..3356c9b0c 100644 --- a/fuse_optimizers/test/launch_tests/test_optimizer.cpp +++ b/fuse_optimizers/test/launch_tests/test_optimizer.cpp @@ -42,7 +42,6 @@ #include // NOLINT - TEST(Optimizer, Constructor) { // Create optimizer: @@ -50,54 +49,47 @@ TEST(Optimizer, Constructor) ExampleOptimizer optimizer(*node); // Check the motion and sensor models, and publishers were loaded: - const auto & motion_models = optimizer.getMotionModels(); - const auto & sensor_models = optimizer.getSensorModels(); - const auto & publishers = optimizer.getPublishers(); + const auto& motion_models = optimizer.getMotionModels(); + const auto& sensor_models = optimizer.getSensorModels(); + const auto& publishers = optimizer.getPublishers(); EXPECT_FALSE(motion_models.empty()); EXPECT_FALSE(sensor_models.empty()); EXPECT_FALSE(publishers.empty()); // Check the expected motion and sensor models, and publisher were loaded: - const std::vector expected_motion_models = {"noisy_unicycle_2d", "unicycle_2d"}; - const std::vector expected_sensor_models = {"imu", "laser_localization", // NOLINT - "unicycle_2d_ignition", "wheel_odometry"}; - const std::vector expected_publishers = - {"odometry_publisher", "serialized_publisher"}; + const std::vector expected_motion_models = { "noisy_unicycle_2d", "unicycle_2d" }; + const std::vector expected_sensor_models = { "imu", "laser_localization", // NOLINT + "unicycle_2d_ignition", "wheel_odometry" }; + const std::vector expected_publishers = { "odometry_publisher", "serialized_publisher" }; ASSERT_TRUE(std::is_sorted(expected_motion_models.begin(), expected_motion_models.end())) - << expected_motion_models << " is not sorted."; + << expected_motion_models << " is not sorted."; ASSERT_TRUE(std::is_sorted(expected_sensor_models.begin(), expected_sensor_models.end())) - << expected_sensor_models << " is not sorted."; + << expected_sensor_models << " is not sorted."; ASSERT_TRUE(std::is_sorted(expected_publishers.begin(), expected_publishers.end())) - << expected_publishers << " is not sorted."; + << expected_publishers << " is not sorted."; // Compute the symmetric difference between the expected and actual motion and sensor models, and // publishers: - const auto difference_motion_models = set_symmetric_difference( - expected_motion_models, - motion_models); - const auto difference_sensor_models = set_symmetric_difference( - expected_sensor_models, - sensor_models); + const auto difference_motion_models = set_symmetric_difference(expected_motion_models, motion_models); + const auto difference_sensor_models = set_symmetric_difference(expected_sensor_models, sensor_models); const auto difference_publishers = set_symmetric_difference(expected_publishers, publishers); // Check the symmetric difference is empty, i.e. the actual motion and sensor models, and // publishers are the same as the expected ones: EXPECT_TRUE(difference_motion_models.empty()) - << "Actual: " << motion_models << "\nExpected: " << expected_motion_models - << "\nDifference: " << difference_motion_models; + << "Actual: " << motion_models << "\nExpected: " << expected_motion_models + << "\nDifference: " << difference_motion_models; EXPECT_TRUE(difference_sensor_models.empty()) - << "Actual: " << sensor_models << "\nExpected: " << expected_sensor_models - << "\nDifference: " << difference_sensor_models; + << "Actual: " << sensor_models << "\nExpected: " << expected_sensor_models + << "\nDifference: " << difference_sensor_models; EXPECT_TRUE(difference_publishers.empty()) - << "Actual: " << publishers << "\nExpected: " << expected_publishers << "\nDifference: " << - difference_publishers; + << "Actual: " << publishers << "\nExpected: " << expected_publishers << "\nDifference: " << difference_publishers; } - // NOTE(CH3): This main is required because the test is manually run by a launch test -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); diff --git a/fuse_optimizers/test/launch_tests/test_optimizer.py b/fuse_optimizers/test/launch_tests/test_optimizer.py index e1a88a8ad..c09c6bac1 100755 --- a/fuse_optimizers/test/launch_tests/test_optimizer.py +++ b/fuse_optimizers/test/launch_tests/test_optimizer.py @@ -28,13 +28,15 @@ @pytest.fixture def test_proc(): - test_root = '.' + test_root = "." - param_path = os.path.join(test_root, 'launch_tests', 'config', 'optimizer_params.yaml') - test_path = os.path.join(test_root, 'test_optimizer') + param_path = os.path.join( + test_root, "launch_tests", "config", "optimizer_params.yaml" + ) + test_path = os.path.join(test_root, "test_optimizer") - cmd = [test_path, '--ros-args', '--params-file', param_path] - return ExecuteProcess(cmd=cmd, shell=True, output='screen', cached_output=True) + cmd = [test_path, "--ros-args", "--params-file", param_path] + return ExecuteProcess(cmd=cmd, shell=True, output="screen", cached_output=True) @launch_pytest.fixture @@ -45,4 +47,4 @@ def generate_test_description(test_proc): @pytest.mark.launch(fixture=generate_test_description) async def test_no_failed_gtests(test_proc, launch_context): await process_tools.wait_for_exit(launch_context, test_proc, timeout=30) - assert test_proc.return_code == 0, 'GTests failed' + assert test_proc.return_code == 0, "GTests failed" diff --git a/fuse_optimizers/test/test_variable_stamp_index.cpp b/fuse_optimizers/test/test_variable_stamp_index.cpp index bcf9df18d..02c2a4506 100644 --- a/fuse_optimizers/test/test_variable_stamp_index.cpp +++ b/fuse_optimizers/test/test_variable_stamp_index.cpp @@ -59,10 +59,8 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp public: FUSE_VARIABLE_DEFINITIONS(StampedVariable) - explicit StampedVariable(const rclcpp::Time & stamp = rclcpp::Time(0, 0, RCL_ROS_TIME)) - : fuse_core::Variable(fuse_core::uuid::generate()), - fuse_variables::Stamped(stamp), - data_{} + explicit StampedVariable(const rclcpp::Time& stamp = rclcpp::Time(0, 0, RCL_ROS_TIME)) + : fuse_core::Variable(fuse_core::uuid::generate()), fuse_variables::Stamped(stamp), data_{} { } @@ -71,17 +69,17 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp return 1; } - const double * data() const override + const double* data() const override { return &data_; } - double * data() override + double* data() override { return &data_; } - void print(std::ostream & /*stream = std::cout*/) const override + void print(std::ostream& /*stream = std::cout*/) const override { } @@ -98,12 +96,12 @@ class StampedVariable : public fuse_core::Variable, public fuse_variables::Stamp * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); + archive& data_; } }; @@ -117,9 +115,7 @@ class UnstampedVariable : public fuse_core::Variable public: FUSE_VARIABLE_DEFINITIONS(UnstampedVariable) - UnstampedVariable() - : fuse_core::Variable(fuse_core::uuid::generate()), - data_{} + UnstampedVariable() : fuse_core::Variable(fuse_core::uuid::generate()), data_{} { } @@ -128,17 +124,17 @@ class UnstampedVariable : public fuse_core::Variable return 1; } - const double * data() const override + const double* data() const override { return &data_; } - double * data() override + double* data() override { return &data_; } - void print(std::ostream & /*stream = std::cout*/) const override + void print(std::ostream& /*stream = std::cout*/) const override { } @@ -155,11 +151,11 @@ class UnstampedVariable : public fuse_core::Variable * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; @@ -175,40 +171,32 @@ class GenericConstraint : public fuse_core::Constraint GenericConstraint() = default; - GenericConstraint( - const std::string & source, - std::initializer_list variable_uuids) - : Constraint(source, variable_uuids) + GenericConstraint(const std::string& source, std::initializer_list variable_uuids) + : Constraint(source, variable_uuids) { } - explicit GenericConstraint(const std::string & source, const fuse_core::UUID & variable1) - : fuse_core::Constraint(source, {variable1}) + explicit GenericConstraint(const std::string& source, const fuse_core::UUID& variable1) + : fuse_core::Constraint(source, { variable1 }) { } - GenericConstraint( - const std::string & source, - const fuse_core::UUID & variable1, - const fuse_core::UUID & variable2) - : fuse_core::Constraint(source, {variable1, variable2}) + GenericConstraint(const std::string& source, const fuse_core::UUID& variable1, const fuse_core::UUID& variable2) + : fuse_core::Constraint(source, { variable1, variable2 }) { } - GenericConstraint( - const std::string & source, - const fuse_core::UUID & variable1, - const fuse_core::UUID & variable2, - const fuse_core::UUID & variable3) - : fuse_core::Constraint(source, {variable1, variable2, variable3}) + GenericConstraint(const std::string& source, const fuse_core::UUID& variable1, const fuse_core::UUID& variable2, + const fuse_core::UUID& variable3) + : fuse_core::Constraint(source, { variable1, variable2, variable3 }) { } - void print(std::ostream & /*stream = std::cout*/) const override + void print(std::ostream& /*stream = std::cout*/) const override { } - ceres::CostFunction * costFunction() const override + ceres::CostFunction* costFunction() const override { return nullptr; } @@ -224,16 +212,15 @@ class GenericConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; BOOST_CLASS_EXPORT(GenericConstraint); - TEST(VariableStampIndex, Size) { // Create an empty index @@ -322,7 +309,7 @@ TEST(VariableStampIndex, Query) index.query(rclcpp::Time(1, 500000, RCL_ROS_TIME), std::back_inserter(actual1)); EXPECT_EQ(expected1, actual1); - auto expected2 = std::vector{x1->uuid(), l1->uuid()}; + auto expected2 = std::vector{ x1->uuid(), l1->uuid() }; std::sort(expected2.begin(), expected2.end()); auto actual2 = std::vector(); index.query(rclcpp::Time(2, 500000, RCL_ROS_TIME), std::back_inserter(actual2)); @@ -375,7 +362,7 @@ TEST(VariableStampIndex, MarginalTransaction) EXPECT_EQ(4u, index.size()); // And the marginal constraint x3->l1 should not affect future queries - auto expected = std::vector{l1->uuid()}; + auto expected = std::vector{ l1->uuid() }; std::sort(expected.begin(), expected.end()); auto actual = std::vector(); index.query(rclcpp::Time(2, 500000, RCL_ROS_TIME), std::back_inserter(actual)); diff --git a/fuse_publishers/CMakeLists.txt b/fuse_publishers/CMakeLists.txt index c8473b506..94316d01f 100644 --- a/fuse_publishers/CMakeLists.txt +++ b/fuse_publishers/CMakeLists.txt @@ -23,56 +23,50 @@ find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) -########### -## Build ## -########### +# ############################################################################## +# Build ## +# ############################################################################## # fuse_publishers library -add_library(${PROJECT_NAME} - src/path_2d_publisher.cpp - src/pose_2d_publisher.cpp - src/serialized_publisher.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$" -) -target_link_libraries(${PROJECT_NAME} PUBLIC - fuse_core::fuse_core - ${fuse_msgs_TARGETS} - fuse_variables::fuse_variables - ${geometry_msgs_TARGETS} - ${nav_msgs_TARGETS} - pluginlib::pluginlib - rclcpp::rclcpp - tf2::tf2 - ${tf2_geometry_msgs_TARGETS} - tf2_ros::tf2_ros -) +add_library(${PROJECT_NAME} src/path_2d_publisher.cpp src/pose_2d_publisher.cpp + src/serialized_publisher.cpp) +target_include_directories( + ${PROJECT_NAME} + PUBLIC "$" + "$") +target_link_libraries( + ${PROJECT_NAME} + PUBLIC fuse_core::fuse_core + ${fuse_msgs_TARGETS} + fuse_variables::fuse_variables + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + tf2::tf2 + ${tf2_geometry_msgs_TARGETS} + tf2_ros::tf2_ros) -############# -## Testing ## -############# +# ############################################################################## +# Testing ## +# ############################################################################## if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() add_subdirectory(test) endif() -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## -install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-export ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) pluginlib_export_plugin_description_file(fuse_core fuse_plugins.xml) @@ -88,7 +82,6 @@ ament_export_dependencies( rclcpp tf2 tf2_geometry_msgs - tf2_ros -) + tf2_ros) ament_package() diff --git a/fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp b/fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp index 78e8a0a16..516391022 100644 --- a/fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp +++ b/fuse_publishers/include/fuse_publishers/path_2d_publisher.hpp @@ -47,7 +47,6 @@ #include #include - namespace fuse_publishers { @@ -79,9 +78,8 @@ class Path2DPublisher : public fuse_core::AsyncPublisher /** * @brief Shadowing extension to the AsyncPublisher::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; /** * @brief Perform any required post-construction initialization, such as advertising publishers or @@ -97,20 +95,17 @@ class Path2DPublisher : public fuse_core::AsyncPublisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) override; + void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; protected: - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncPublisher interfaces_ + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncPublisher interfaces_ fuse_core::UUID device_id_; //!< The UUID of the device to be published - std::string frame_id_; //!< The name of the frame for this path + std::string frame_id_; //!< The name of the frame for this path //!< The publisher that sends the entire robot trajectory as a path rclcpp::Publisher::SharedPtr path_publisher_; diff --git a/fuse_publishers/include/fuse_publishers/pose_2d_publisher.hpp b/fuse_publishers/include/fuse_publishers/pose_2d_publisher.hpp index b37ac61c1..a00b66688 100644 --- a/fuse_publishers/include/fuse_publishers/pose_2d_publisher.hpp +++ b/fuse_publishers/include/fuse_publishers/pose_2d_publisher.hpp @@ -61,7 +61,6 @@ #include - namespace fuse_publishers { @@ -134,9 +133,8 @@ class Pose2DPublisher : public fuse_core::AsyncPublisher /** * @brief Shadowing extension to the AsyncPublisher::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; /** * @brief Perform any required post-construction initialization, such as advertising publishers or @@ -166,9 +164,8 @@ class Pose2DPublisher : public fuse_core::AsyncPublisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) override; + void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; /** * @brief Timer-based callback that publishes the latest map->odom transform @@ -180,38 +177,32 @@ class Pose2DPublisher : public fuse_core::AsyncPublisher void tfPublishTimerCallback(); protected: - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Timers, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncPublisher interfaces_ - - using Synchronizer = StampedVariableSynchronizer< - fuse_variables::Orientation2DStamped, - fuse_variables::Position2DStamped>; - - std::string base_frame_; //!< The name of the robot's base_link frame - fuse_core::UUID device_id_; //!< The UUID of the device to be published + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncPublisher interfaces_ + + using Synchronizer = + StampedVariableSynchronizer; + + std::string base_frame_; //!< The name of the robot's base_link frame + fuse_core::UUID device_id_; //!< The UUID of the device to be published rclcpp::Clock::SharedPtr clock_; //!< The publisher's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The publisher's logger + rclcpp::Logger logger_; //!< The publisher's logger - std::string map_frame_; //!< The name of the robot's map frame + std::string map_frame_; //!< The name of the robot's map frame std::string odom_frame_; //!< The name of the odom frame for this pose (or empty if the odom is //!< not used) rclcpp::Publisher::SharedPtr pose_publisher_; - rclcpp::Publisher< - geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_with_covariance_publisher_; - bool publish_to_tf_; //!< Flag indicating the pose should be sent to the tf system as well as the - //!< pose topics + rclcpp::Publisher::SharedPtr pose_with_covariance_publisher_; + bool publish_to_tf_; //!< Flag indicating the pose should be sent to the tf system as well as the + //!< pose topics Synchronizer::UniquePtr synchronizer_; //!< Object that tracks the latest common timestamp of //!< multiple variables - std::unique_ptr tf_buffer_; //!< TF2 object that supports querying transforms by - //!< time and frame id + std::unique_ptr tf_buffer_; //!< TF2 object that supports querying transforms by + //!< time and frame id std::unique_ptr tf_listener_; //!< TF2 object that subscribes to the //!< tf topics and inserts the received //!< transforms into the tf buffer @@ -219,11 +210,11 @@ class Pose2DPublisher : public fuse_core::AsyncPublisher //!< Publish the map->odom or map->base transform to the tf system std::shared_ptr tf_publisher_ = nullptr; - rclcpp::TimerBase::SharedPtr tf_publish_timer_; //!< Timer that publishes tf messages to ensure - //!< the tf transform doesn't get stale - rclcpp::Duration tf_timeout_; //!< The max time to wait for a tf transform to become available + rclcpp::TimerBase::SharedPtr tf_publish_timer_; //!< Timer that publishes tf messages to ensure + //!< the tf transform doesn't get stale + rclcpp::Duration tf_timeout_; //!< The max time to wait for a tf transform to become available geometry_msgs::msg::TransformStamped tf_transform_; //!< The transform to be published to tf - bool use_tf_lookup_; //!< Internal flag indicating that a tf frame lookup is required + bool use_tf_lookup_; //!< Internal flag indicating that a tf frame lookup is required }; } // namespace fuse_publishers diff --git a/fuse_publishers/include/fuse_publishers/serialized_publisher.hpp b/fuse_publishers/include/fuse_publishers/serialized_publisher.hpp index 764aac037..fc62ba01a 100644 --- a/fuse_publishers/include/fuse_publishers/serialized_publisher.hpp +++ b/fuse_publishers/include/fuse_publishers/serialized_publisher.hpp @@ -46,7 +46,6 @@ #include #include - namespace fuse_publishers { @@ -71,9 +70,8 @@ class SerializedPublisher : public fuse_core::AsyncPublisher /** * @brief Shadowing extension to the AsyncPublisher::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; /** * @brief Perform any required post-construction initialization, such as advertising publishers or @@ -89,19 +87,15 @@ class SerializedPublisher : public fuse_core::AsyncPublisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) override; + void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; protected: - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncPublisher interfaces_ + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncPublisher interfaces_ /** * @brief Publish the serialized graph @@ -110,17 +104,14 @@ class SerializedPublisher : public fuse_core::AsyncPublisher * whenever needed * @param[in] stamp A rclcpp::Time stamp used for the serialized graph message published */ - void graphPublisherCallback( - fuse_core::Graph::ConstSharedPtr graph, - const rclcpp::Time & stamp) const; + void graphPublisherCallback(fuse_core::Graph::ConstSharedPtr graph, const rclcpp::Time& stamp) const; std::string frame_id_; //!< The name of the frame for the serialized graph and transaction //!< messages published rclcpp::Publisher::SharedPtr graph_publisher_; rclcpp::Publisher::SharedPtr transaction_publisher_; - using GraphPublisherCallback = - std::function; + using GraphPublisherCallback = std::function; using GraphPublisherThrottledCallback = fuse_core::ThrottledCallback; GraphPublisherThrottledCallback graph_publisher_throttled_callback_; //!< The graph publisher //!< throttled callback diff --git a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h index e1c240cad..5f390ea09 100644 --- a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h +++ b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h @@ -35,8 +35,7 @@ #ifndef FUSE_PUBLISHERS__STAMPED_VARIABLE_SYNCHRONIZER_H_ #define FUSE_PUBLISHERS__STAMPED_VARIABLE_SYNCHRONIZER_H_ -#warning \ - This header is obsolete, please include fuse_publishers/stamped_variable_synchronizer.hpp instead +#warning This header is obsolete, please include fuse_publishers/stamped_variable_synchronizer.hpp instead #include diff --git a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp index 92cca72b1..0de70671a 100644 --- a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp +++ b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.hpp @@ -45,7 +45,6 @@ #include - namespace fuse_publishers { @@ -65,7 +64,7 @@ namespace fuse_publishers * * This class only functions with variables derived from the fuse_variables::Stamped base class. */ -template +template class StampedVariableSynchronizer { public: @@ -76,7 +75,7 @@ class StampedVariableSynchronizer * * @param[in] device_id The device id to use for all variable types */ - explicit StampedVariableSynchronizer(const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit StampedVariableSynchronizer(const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Find the latest timestamp for which variables of all the specified template types exist @@ -87,12 +86,10 @@ class StampedVariableSynchronizer * @param[in] graph The complete graph * @return The latest timestamp shared by all requested variable types */ - rclcpp::Time findLatestCommonStamp( - const fuse_core::Transaction & transaction, - const fuse_core::Graph & graph); + rclcpp::Time findLatestCommonStamp(const fuse_core::Transaction& transaction, const fuse_core::Graph& graph); private: - fuse_core::UUID device_id_; //!< The device_id to use with the Stamped classes + fuse_core::UUID device_id_; //!< The device_id to use with the Stamped classes rclcpp::Time latest_common_stamp_; //!< The previously discovered common stamp /** @@ -103,8 +100,8 @@ class StampedVariableSynchronizer * @param[in] graph The complete graph, used to verify that all requested variables exist * for a given time */ - template - void updateTime(const VariableRange & variable_range, const fuse_core::Graph & graph); + template + void updateTime(const VariableRange& variable_range, const fuse_core::Graph& graph); }; namespace detail @@ -113,79 +110,79 @@ namespace detail /** * @brief Some helper structs for testing properties of all types in a template parameter pack */ -template +template struct bool_pack; -template +template using all_true_helper = std::is_same, bool_pack>; /** * @brief Test if a property is true for all types in a template parameter pack. This is a type. */ -template +template using all_true = all_true_helper; /** * @brief Test if a property is true for all types in a template parameter pack. This is a boolean * value. */ -template +template constexpr bool allTrue = all_true::value; /** * @brief Test if a class is derived from the fuse_variables::Stamped base class. This is a type. */ -template +template using is_stamped = std::is_base_of; /** * @brief Test if a class is derived from the fuse_variables::Stamped base class. This is a boolean * value. */ -template +template constexpr bool isStamped = is_stamped::value; /** * @brief Test if a class is derived from the fuse_core::Variable base class. This is a type. */ -template +template using is_variable = std::is_base_of; /** * @brief Test if a class is derived from the fuse_core::Variable base class. This is a boolean * value. */ -template +template constexpr bool isVariable = is_variable::value; /** * @brief Test if a class is derived from both the fuse_core::Variable base class and the * fuse_variables::Stamped base class. This is a type. */ -template +template struct is_stamped_variable { - static constexpr bool value = isStamped&& isVariable; + static constexpr bool value = isStamped && isVariable; }; /** * @brief Test if a class is derived from both the fuse_core::Variable base class and the * fuse_variables::Stamped base class. This is a boolean value. */ -template +template constexpr bool isStampedVariable = is_stamped_variable::value; /** * @brief Test if all of the template parameter pack types are fuse_core::Variable and * fuse_variables::Stamped. This is a type. */ -template +template using all_stamped_variables = all_true...>; /** * @brief Test if all of the template parameter pack types are fuse_core::Variable and * fuse_variables::Stamped. This is a boolean value. */ -template +template constexpr bool allStampedVariables = all_stamped_variables::value; /** @@ -201,12 +198,11 @@ constexpr bool allStampedVariables = all_stamped_variables::value; * @param[in] device_id The device id used to construct all variable types * @return True if all variables exist, false otherwise */ -template +template struct all_variables_exist { - static bool value( - const fuse_core::Graph & /*graph*/, const rclcpp::Time & /*stamp*/, - const fuse_core::UUID & /*device_id*/) + static bool value(const fuse_core::Graph& /*graph*/, const rclcpp::Time& /*stamp*/, + const fuse_core::UUID& /*device_id*/) { return true; } @@ -223,12 +219,10 @@ struct all_variables_exist * @param[in] device_id The device id used to construct all variable types * @return True if all variables exist, false otherwise */ -template +template struct all_variables_exist { - static bool value( - const fuse_core::Graph & graph, const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) + static bool value(const fuse_core::Graph& graph, const rclcpp::Time& stamp, const fuse_core::UUID& device_id) { return graph.variableExists(T(stamp, device_id).uuid()) && all_variables_exist::value(graph, stamp, device_id); @@ -246,10 +240,10 @@ struct all_variables_exist * @param[in] variable The variable to check against the template parameter pack * @return True if the variable's type is part of the template parameter pack, false otherwise */ -template +template struct is_variable_in_pack { - static bool value(const fuse_core::Variable & /*variable*/) + static bool value(const fuse_core::Variable& /*variable*/) { return false; } @@ -264,65 +258,62 @@ struct is_variable_in_pack * @param[in] variable The variable to check against the template parameter pack * @return True if the variable's type is part of the template parameter pack, false otherwise */ -template +template struct is_variable_in_pack { - static bool value(const fuse_core::Variable & variable) + static bool value(const fuse_core::Variable& variable) { - auto derived = dynamic_cast(&variable); - return static_cast(derived) || - is_variable_in_pack::value(variable); + auto derived = dynamic_cast(&variable); + return static_cast(derived) || is_variable_in_pack::value(variable); } }; } // namespace detail -template -StampedVariableSynchronizer::StampedVariableSynchronizer(const fuse_core::UUID & device_id) -: device_id_(device_id), +template +StampedVariableSynchronizer::StampedVariableSynchronizer(const fuse_core::UUID& device_id) + : device_id_(device_id) + , // NOTE(CH3): Uninitialized, for getting latest We use RCL_ROS_TIME so time comparisons are // consistent latest_common_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { - static_assert( - detail::allStampedVariables, "All synchronized types must be derived from both " - "fuse_core::Variable and fuse_variable::Stamped."); + static_assert(detail::allStampedVariables, "All synchronized types must be derived from both " + "fuse_core::Variable and fuse_variable::Stamped."); static_assert(sizeof...(Ts) > 0, "At least one type must be specified."); } -template -rclcpp::Time StampedVariableSynchronizer::findLatestCommonStamp( - const fuse_core::Transaction & transaction, - const fuse_core::Graph & graph) +template +rclcpp::Time StampedVariableSynchronizer::findLatestCommonStamp(const fuse_core::Transaction& transaction, + const fuse_core::Graph& graph) { // Clear the previous stamp if the variable was deleted if (0u != latest_common_stamp_.nanoseconds() && - !detail::all_variables_exist::value(graph, latest_common_stamp_, device_id_)) + !detail::all_variables_exist::value(graph, latest_common_stamp_, device_id_)) { latest_common_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); } // Search the transaction for more recent variables updateTime(transaction.addedVariables(), graph); // If no common timestamp was found, search the whole graph for the most recent variable set - if (0u == latest_common_stamp_.nanoseconds()) { + if (0u == latest_common_stamp_.nanoseconds()) + { updateTime(graph.getVariables(), graph); } return latest_common_stamp_; } -template -template -void StampedVariableSynchronizer::updateTime( - const VariableRange & variable_range, - const fuse_core::Graph & graph) +template +template +void StampedVariableSynchronizer::updateTime(const VariableRange& variable_range, const fuse_core::Graph& graph) { - for (const auto & candidate_variable : variable_range) { - if (detail::is_variable_in_pack::value(candidate_variable)) { - const auto & stamped_variable = - dynamic_cast(candidate_variable); - if ((stamped_variable.stamp() > latest_common_stamp_) && - (stamped_variable.deviceId() == device_id_) && - (detail::all_variables_exist::value(graph, stamped_variable.stamp(), device_id_))) + for (const auto& candidate_variable : variable_range) + { + if (detail::is_variable_in_pack::value(candidate_variable)) + { + const auto& stamped_variable = dynamic_cast(candidate_variable); + if ((stamped_variable.stamp() > latest_common_stamp_) && (stamped_variable.deviceId() == device_id_) && + (detail::all_variables_exist::value(graph, stamped_variable.stamp(), device_id_))) { latest_common_stamp_ = stamped_variable.stamp(); } diff --git a/fuse_publishers/package.xml b/fuse_publishers/package.xml index 1b58db8b3..c2d3a7f00 100644 --- a/fuse_publishers/package.xml +++ b/fuse_publishers/package.xml @@ -38,8 +38,6 @@ tf2_ros ament_cmake_gtest - ament_lint_auto - ament_lint_common fuse_constraints fuse_graphs rclcpp diff --git a/fuse_publishers/src/path_2d_publisher.cpp b/fuse_publishers/src/path_2d_publisher.cpp index 69c5be8f4..1b9675ae1 100644 --- a/fuse_publishers/src/path_2d_publisher.cpp +++ b/fuse_publishers/src/path_2d_publisher.cpp @@ -58,16 +58,12 @@ PLUGINLIB_EXPORT_CLASS(fuse_publishers::Path2DPublisher, fuse_core::Publisher); namespace fuse_publishers { -Path2DPublisher::Path2DPublisher() -: fuse_core::AsyncPublisher(1), - device_id_(fuse_core::uuid::NIL), - frame_id_("map") +Path2DPublisher::Path2DPublisher() : fuse_core::AsyncPublisher(1), device_id_(fuse_core::uuid::NIL), frame_id_("map") { } -void Path2DPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void Path2DPublisher::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -77,103 +73,99 @@ void Path2DPublisher::onInit() { // Configure the publisher std::string device_str; - device_str = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "device_id"), device_str); - if (device_str != "") { + device_str = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "device_id"), device_str); + if (device_str != "") + { device_id_ = fuse_core::uuid::from_string(device_str); - } else { - device_str = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "device_name"), device_str); - if (device_str != "") { + } + else + { + device_str = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "device_name"), device_str); + if (device_str != "") + { device_id_ = fuse_core::uuid::generate(device_str); } } - frame_id_ = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "frame_id"), frame_id_); + frame_id_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "frame_id"), frame_id_); // Advertise the topic rclcpp::PublisherOptions pub_options; pub_options.callback_group = cb_group_; - path_publisher_ = rclcpp::create_publisher( - interfaces_, fuse_core::joinTopicName(name_, "path"), 1, pub_options); + path_publisher_ = rclcpp::create_publisher(interfaces_, fuse_core::joinTopicName(name_, "path"), + 1, pub_options); pose_array_publisher_ = rclcpp::create_publisher( - interfaces_, fuse_core::joinTopicName(name_, "pose_array"), 1, pub_options); + interfaces_, fuse_core::joinTopicName(name_, "pose_array"), 1, pub_options); } -void Path2DPublisher::notifyCallback( - fuse_core::Transaction::ConstSharedPtr /*transaction*/, - fuse_core::Graph::ConstSharedPtr graph) +void Path2DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr /*transaction*/, + fuse_core::Graph::ConstSharedPtr graph) { // Exit early if no one is listening - if ((path_publisher_->get_subscription_count() == 0) && - (pose_array_publisher_->get_subscription_count() == 0)) + if ((path_publisher_->get_subscription_count() == 0) && (pose_array_publisher_->get_subscription_count() == 0)) { return; } // Extract all of the 2D pose variables to the path std::vector poses; - for (const auto & variable : graph->getVariables()) { - auto orientation = dynamic_cast(&variable); - if (orientation && - (orientation->deviceId() == device_id_)) + for (const auto& variable : graph->getVariables()) + { + auto orientation = dynamic_cast(&variable); + if (orientation && (orientation->deviceId() == device_id_)) { - const auto & stamp = orientation->stamp(); + const auto& stamp = orientation->stamp(); auto position_uuid = fuse_variables::Position2DStamped(stamp, device_id_).uuid(); - if (!graph->variableExists(position_uuid)) { + if (!graph->variableExists(position_uuid)) + { continue; } - auto position = - dynamic_cast(&graph->getVariable(position_uuid)); + auto position = dynamic_cast(&graph->getVariable(position_uuid)); geometry_msgs::msg::PoseStamped pose; pose.header.stamp = stamp; pose.header.frame_id = frame_id_; pose.pose.position.x = position->x(); pose.pose.position.y = position->y(); pose.pose.position.z = 0.0; - pose.pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation->yaw())); + pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation->yaw())); poses.push_back(std::move(pose)); } } // Exit if there are no poses - if (poses.empty()) { + if (poses.empty()) + { return; } // Sort the poses by timestamp - auto compare_stamps = - [](const geometry_msgs::msg::PoseStamped & pose1, const geometry_msgs::msg::PoseStamped & pose2) + auto compare_stamps = [](const geometry_msgs::msg::PoseStamped& pose1, const geometry_msgs::msg::PoseStamped& pose2) { + if (pose1.header.stamp.sec == pose2.header.stamp.sec) { - if (pose1.header.stamp.sec == pose2.header.stamp.sec) { - return pose1.header.stamp.nanosec < pose2.header.stamp.nanosec; - } else { - return pose1.header.stamp.sec < pose2.header.stamp.sec; - } - }; + return pose1.header.stamp.nanosec < pose2.header.stamp.nanosec; + } + else + { + return pose1.header.stamp.sec < pose2.header.stamp.sec; + } + }; std::sort(poses.begin(), poses.end(), compare_stamps); // Define the header for the aggregate message std_msgs::msg::Header header; header.stamp = poses.back().header.stamp; header.frame_id = frame_id_; // Convert the sorted poses into a Path msg - if (path_publisher_->get_subscription_count() > 0) { + if (path_publisher_->get_subscription_count() > 0) + { nav_msgs::msg::Path path_msg; path_msg.header = header; path_msg.poses = poses; path_publisher_->publish(path_msg); } // Convert the sorted poses into a PoseArray msg - if (pose_array_publisher_->get_subscription_count() > 0) { + if (pose_array_publisher_->get_subscription_count() > 0) + { geometry_msgs::msg::PoseArray pose_array_msg; pose_array_msg.header = header; - std::transform( - poses.begin(), - poses.end(), - std::back_inserter(pose_array_msg.poses), - [](const geometry_msgs::msg::PoseStamped & pose) - { - return pose.pose; - }); // NOLINT(whitespace/braces) + std::transform(poses.begin(), poses.end(), std::back_inserter(pose_array_msg.poses), + [](const geometry_msgs::msg::PoseStamped& pose) { return pose.pose; }); // NOLINT(whitespace/braces) pose_array_publisher_->publish(pose_array_msg); } } diff --git a/fuse_publishers/src/pose_2d_publisher.cpp b/fuse_publishers/src/pose_2d_publisher.cpp index fe5c88dcc..5291c1aa8 100644 --- a/fuse_publishers/src/pose_2d_publisher.cpp +++ b/fuse_publishers/src/pose_2d_publisher.cpp @@ -61,38 +61,32 @@ PLUGINLIB_EXPORT_CLASS(fuse_publishers::Pose2DPublisher, fuse_core::Publisher); namespace { -bool findPose( - rclcpp::Logger logger, - rclcpp::Clock clock, - const fuse_core::Graph & graph, - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id, - fuse_core::UUID & orientation_uuid, - fuse_core::UUID & position_uuid, - geometry_msgs::msg::Pose & pose -) +bool findPose(rclcpp::Logger logger, rclcpp::Clock clock, const fuse_core::Graph& graph, const rclcpp::Time& stamp, + const fuse_core::UUID& device_id, fuse_core::UUID& orientation_uuid, fuse_core::UUID& position_uuid, + geometry_msgs::msg::Pose& pose) { - try { + try + { orientation_uuid = fuse_variables::Orientation2DStamped(stamp, device_id).uuid(); - auto orientation_variable = dynamic_cast( - graph.getVariable(orientation_uuid)); + auto orientation_variable = + dynamic_cast(graph.getVariable(orientation_uuid)); position_uuid = fuse_variables::Position2DStamped(stamp, device_id).uuid(); - auto position_variable = dynamic_cast( - graph.getVariable(position_uuid)); + auto position_variable = dynamic_cast(graph.getVariable(position_uuid)); pose.position.x = position_variable.x(); pose.position.y = position_variable.y(); pose.position.z = 0.0; - pose.orientation = - tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation_variable.yaw())); - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM_THROTTLE( - logger, clock, 10.0 * 1000, - "Failed to find a pose at time " << stamp.nanoseconds() << ". Error" << e.what()); + pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation_variable.yaw())); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger, clock, 10.0 * 1000, + "Failed to find a pose at time " << stamp.nanoseconds() << ". Error" << e.what()); return false; - } catch (...) { - RCLCPP_WARN_STREAM_THROTTLE( - logger, clock, 10.0 * 1000, - "Failed to find a pose at time " << stamp.nanoseconds() << ". Error: unknown"); + } + catch (...) + { + RCLCPP_WARN_STREAM_THROTTLE(logger, clock, 10.0 * 1000, + "Failed to find a pose at time " << stamp.nanoseconds() << ". Error: unknown"); return false; } return true; @@ -104,18 +98,17 @@ namespace fuse_publishers { Pose2DPublisher::Pose2DPublisher() -: fuse_core::AsyncPublisher(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - publish_to_tf_(false), - tf_timeout_(rclcpp::Duration(0, 0)), - use_tf_lookup_(false) + : fuse_core::AsyncPublisher(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , publish_to_tf_(false) + , tf_timeout_(rclcpp::Duration(0, 0)) + , use_tf_lookup_(false) { } -void Pose2DPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void Pose2DPublisher::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -129,71 +122,66 @@ void Pose2DPublisher::onInit() tf_publisher_ = std::make_shared(interfaces_); // Read configuration from the parameter server - base_frame_ = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "base_frame"), std::string("base_link")); - map_frame_ = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "map_frame"), std::string("map")); - odom_frame_ = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "odom_frame"), std::string("odom")); + base_frame_ = + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "base_frame"), std::string("base_link")); + map_frame_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "map_frame"), std::string("map")); + odom_frame_ = + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "odom_frame"), std::string("odom")); std::string device_str; - device_str = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "device_id"), device_str); - if (device_str != "") { + device_str = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "device_id"), device_str); + if (device_str != "") + { device_id_ = fuse_core::uuid::from_string(device_str); - } else { - device_str = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "device_name"), device_str); - if (device_str != "") { + } + else + { + device_str = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "device_name"), device_str); + if (device_str != "") + { device_id_ = fuse_core::uuid::generate(device_str); } } - publish_to_tf_ = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "publish_to_tf"), false); + publish_to_tf_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "publish_to_tf"), false); // Configure tf, if requested - if (publish_to_tf_) { + if (publish_to_tf_) + { use_tf_lookup_ = (!odom_frame_.empty() && (odom_frame_ != base_frame_)); - if (use_tf_lookup_) { + if (use_tf_lookup_) + { double tf_cache_time; double default_tf_cache_time = 10.0; - tf_cache_time = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "tf_cache_time"), default_tf_cache_time); - if (tf_cache_time <= 0) { - RCLCPP_WARN_STREAM( - logger_, - "The requested tf_cache_time is <= 0. Using the default value (" - << default_tf_cache_time << "s) instead."); + tf_cache_time = + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "tf_cache_time"), default_tf_cache_time); + if (tf_cache_time <= 0) + { + RCLCPP_WARN_STREAM(logger_, "The requested tf_cache_time is <= 0. Using the default value (" + << default_tf_cache_time << "s) instead."); tf_cache_time = default_tf_cache_time; } double tf_timeout; double default_tf_timeout = 0.1; - tf_timeout = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "tf_timeout"), default_tf_timeout); - if (tf_timeout <= 0) { - RCLCPP_WARN_STREAM( - logger_, - "The requested tf_timeout is <= 0. Using the default value (" - << default_tf_timeout << "s) instead."); + tf_timeout = + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "tf_timeout"), default_tf_timeout); + if (tf_timeout <= 0) + { + RCLCPP_WARN_STREAM(logger_, "The requested tf_timeout is <= 0. Using the default value (" << default_tf_timeout + << "s) instead."); tf_timeout = default_tf_timeout; } tf_timeout_ = rclcpp::Duration::from_seconds(tf_timeout); tf_buffer_ = std::make_unique( - clock_, - rclcpp::Duration::from_seconds(tf_cache_time) - .to_chrono() - // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h - // TODO(methylDragon): See above ^ // NOLINT - ); - 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() + clock_, rclcpp::Duration::from_seconds(tf_cache_time).to_chrono() + // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h + // TODO(methylDragon): See above ^ // NOLINT ); + 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()); } } @@ -202,10 +190,9 @@ void Pose2DPublisher::onInit() pub_options.callback_group = cb_group_; pose_publisher_ = rclcpp::create_publisher( - interfaces_, fuse_core::joinTopicName(name_, "pose"), 1, pub_options); - pose_with_covariance_publisher_ = - rclcpp::create_publisher( - interfaces_, fuse_core::joinTopicName(name_, "pose_with_covariance"), 1, pub_options); + interfaces_, fuse_core::joinTopicName(name_, "pose"), 1, pub_options); + pose_with_covariance_publisher_ = rclcpp::create_publisher( + interfaces_, fuse_core::joinTopicName(name_, "pose_with_covariance"), 1, pub_options); } void Pose2DPublisher::onStart() @@ -215,64 +202,58 @@ void Pose2DPublisher::onStart() // Clear the synchronizer synchronizer_ = Synchronizer::make_unique(device_id_); // Start the tf timer - if (publish_to_tf_) { + if (publish_to_tf_) + { double tf_publish_frequency; double default_tf_publish_frequency = 10.0; // We pull the param again on each start so the publisher can technically get set to a different // publish frequency - tf_publish_frequency = fuse_core::getParam( - interfaces_, fuse_core::joinParameterName(name_, "tf_publish_frequency"), - default_tf_publish_frequency); - if (tf_publish_frequency <= 0) { - RCLCPP_WARN_STREAM( - logger_, - "The requested tf_publish_frequency is <= 0. Using the default value (" << - default_tf_publish_frequency << "hz) instead."); + tf_publish_frequency = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "tf_publish_frequency"), + default_tf_publish_frequency); + if (tf_publish_frequency <= 0) + { + RCLCPP_WARN_STREAM(logger_, "The requested tf_publish_frequency is <= 0. Using the default value (" + << default_tf_publish_frequency << "hz) instead."); tf_publish_frequency = default_tf_publish_frequency; } - tf_publish_timer_ = rclcpp::create_timer( - interfaces_, - clock_, - std::chrono::duration(1.0 / tf_publish_frequency), - std::move(std::bind(&Pose2DPublisher::tfPublishTimerCallback, this)), - cb_group_ - ); + tf_publish_timer_ = + rclcpp::create_timer(interfaces_, clock_, std::chrono::duration(1.0 / tf_publish_frequency), + std::move(std::bind(&Pose2DPublisher::tfPublishTimerCallback, this)), cb_group_); } } void Pose2DPublisher::onStop() { // Stop the tf timer - if (publish_to_tf_) { + if (publish_to_tf_) + { tf_publish_timer_->cancel(); } } -void Pose2DPublisher::notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) +void Pose2DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) { auto latest_stamp = synchronizer_->findLatestCommonStamp(*transaction, *graph); - if (0u == latest_stamp.nanoseconds()) { // If uninitialized - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a matching set of stamped pose variables with device id '" - << device_id_ << "'."); + if (0u == latest_stamp.nanoseconds()) + { // If uninitialized + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a matching set of stamped pose variables with device id '" << device_id_ + << "'."); return; } // Get the pose values associated with the selected timestamp fuse_core::UUID orientation_uuid; fuse_core::UUID position_uuid; geometry_msgs::msg::Pose pose; - if (!findPose( - logger_, *clock_, *graph, latest_stamp, device_id_, orientation_uuid, position_uuid, - pose)) + if (!findPose(logger_, *clock_, *graph, latest_stamp, device_id_, orientation_uuid, position_uuid, pose)) { return; } // Publish the various message types - if (publish_to_tf_) { + if (publish_to_tf_) + { // Create a 3D ROS Transform message from the current 2D pose geometry_msgs::msg::TransformStamped map_to_base; map_to_base.header.stamp = latest_stamp; @@ -284,37 +265,42 @@ void Pose2DPublisher::notifyCallback( map_to_base.transform.translation.z = pose.position.z; map_to_base.transform.rotation = pose.orientation; // If we are suppose to publish the map->odom frame instead, do that transformation now - if (use_tf_lookup_) { + if (use_tf_lookup_) + { // We need to lookup the base->odom frame first, so we can compute the map->odom transform // from the map->base transform - try { - auto base_to_odom = tf_buffer_->lookupTransform( - base_frame_, odom_frame_, latest_stamp, - tf_timeout_); + try + { + auto base_to_odom = tf_buffer_->lookupTransform(base_frame_, odom_frame_, latest_stamp, tf_timeout_); geometry_msgs::msg::TransformStamped map_to_odom; tf2::doTransform(base_to_odom, map_to_odom, map_to_base); map_to_odom.child_frame_id = odom_frame_; // The child frame is not populated for some // reason tf_transform_ = map_to_odom; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 2.0 * 1000, - "Could not lookup the transform " << base_frame_ << "->" << odom_frame_ << ". Error: " - << e.what()); } - } else { + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 2.0 * 1000, + "Could not lookup the transform " << base_frame_ << "->" << odom_frame_ + << ". Error: " << e.what()); + } + } + else + { // Simple. No intermediate frame. Just use the optimized map->base transform. tf_transform_ = map_to_base; } } - if (pose_publisher_->get_subscription_count() > 0) { + if (pose_publisher_->get_subscription_count() > 0) + { geometry_msgs::msg::PoseStamped msg; msg.header.stamp = latest_stamp; msg.header.frame_id = map_frame_; msg.pose = pose; pose_publisher_->publish(msg); } - if (pose_with_covariance_publisher_->get_subscription_count() > 0) { + if (pose_with_covariance_publisher_->get_subscription_count() > 0) + { // Get the covariance from the graph std::vector> requests; requests.emplace_back(position_uuid, position_uuid); @@ -343,7 +329,8 @@ void Pose2DPublisher::tfPublishTimerCallback() { // The tf_transform_ is updated in a separate thread, so we must guard the read/write operations. // Only publish if the tf transform is valid - if (rclcpp::Time(tf_transform_.header.stamp).nanoseconds()) { + if (rclcpp::Time(tf_transform_.header.stamp).nanoseconds()) + { // Update the timestamp of the transform so the tf tree will continue to be valid tf_transform_.header.stamp = clock_->now(); tf_publisher_->sendTransform(tf_transform_); diff --git a/fuse_publishers/src/serialized_publisher.cpp b/fuse_publishers/src/serialized_publisher.cpp index 64d2b196f..5dad52ea4 100644 --- a/fuse_publishers/src/serialized_publisher.cpp +++ b/fuse_publishers/src/serialized_publisher.cpp @@ -50,17 +50,15 @@ namespace fuse_publishers { SerializedPublisher::SerializedPublisher() -: fuse_core::AsyncPublisher(1), - frame_id_("map"), - graph_publisher_throttled_callback_( - std::bind(&SerializedPublisher::graphPublisherCallback, this, std::placeholders::_1, - std::placeholders::_2)) + : fuse_core::AsyncPublisher(1) + , frame_id_("map") + , graph_publisher_throttled_callback_( + std::bind(&SerializedPublisher::graphPublisherCallback, this, std::placeholders::_1, std::placeholders::_2)) { } void SerializedPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -69,63 +67,52 @@ void SerializedPublisher::initialize( void SerializedPublisher::onInit() { // Configure the publisher - frame_id_ = fuse_core::getParam( - interfaces_, - fuse_core::joinParameterName(name_, "frame_id"), frame_id_); + frame_id_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "frame_id"), frame_id_); bool latch = false; - latch = fuse_core::getParam( - interfaces_, - fuse_core::joinParameterName(name_, "latch"), latch); + latch = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "latch"), latch); - rclcpp::Duration graph_throttle_period{0, 0}; - fuse_core::getPositiveParam( - interfaces_, - fuse_core::joinParameterName(name_, "graph_throttle_period"), - graph_throttle_period, false); + rclcpp::Duration graph_throttle_period{ 0, 0 }; + fuse_core::getPositiveParam(interfaces_, fuse_core::joinParameterName(name_, "graph_throttle_period"), + graph_throttle_period, false); - bool graph_throttle_use_wall_time{false}; + bool graph_throttle_use_wall_time{ false }; graph_throttle_use_wall_time = fuse_core::getParam( - interfaces_, - fuse_core::joinParameterName(name_, "graph_throttle_use_wall_time"), - graph_throttle_use_wall_time); + interfaces_, fuse_core::joinParameterName(name_, "graph_throttle_use_wall_time"), graph_throttle_use_wall_time); graph_publisher_throttled_callback_.setThrottlePeriod(graph_throttle_period); - if (!graph_throttle_use_wall_time) { - graph_publisher_throttled_callback_.setClock( - interfaces_.get_node_clock_interface()->get_clock()); + if (!graph_throttle_use_wall_time) + { + graph_publisher_throttled_callback_.setClock(interfaces_.get_node_clock_interface()->get_clock()); } // Advertise the topics rclcpp::QoS qos(1); // Queue size of 1 - if (latch) { + if (latch) + { qos.transient_local(); } rclcpp::PublisherOptions pub_options; pub_options.callback_group = cb_group_; - graph_publisher_ = - rclcpp::create_publisher( - interfaces_, "graph", qos, - pub_options); + graph_publisher_ = rclcpp::create_publisher(interfaces_, "graph", qos, pub_options); transaction_publisher_ = - rclcpp::create_publisher( - interfaces_, "transaction", qos, - pub_options); + rclcpp::create_publisher(interfaces_, "transaction", qos, pub_options); } -void SerializedPublisher::notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) +void SerializedPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) { - const auto & stamp = transaction->stamp(); - if (graph_publisher_->get_subscription_count() > 0) { + const auto& stamp = transaction->stamp(); + if (graph_publisher_->get_subscription_count() > 0) + { graph_publisher_throttled_callback_(graph, stamp); } - if (transaction_publisher_->get_subscription_count() > 0) { + if (transaction_publisher_->get_subscription_count() > 0) + { fuse_msgs::msg::SerializedTransaction msg; msg.header.stamp = stamp; msg.header.frame_id = frame_id_; @@ -134,8 +121,7 @@ void SerializedPublisher::notifyCallback( } } -void SerializedPublisher::graphPublisherCallback( - fuse_core::Graph::ConstSharedPtr graph, const rclcpp::Time & stamp) const +void SerializedPublisher::graphPublisherCallback(fuse_core::Graph::ConstSharedPtr graph, const rclcpp::Time& stamp) const { fuse_msgs::msg::SerializedGraph msg; msg.header.stamp = stamp; diff --git a/fuse_publishers/test/CMakeLists.txt b/fuse_publishers/test/CMakeLists.txt index 374182980..03c7b2f4d 100644 --- a/fuse_publishers/test/CMakeLists.txt +++ b/fuse_publishers/test/CMakeLists.txt @@ -2,24 +2,20 @@ find_package(fuse_constraints REQUIRED) find_package(fuse_graphs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -# CORE GTESTS ====================================================================================== -ament_add_gtest("test_stamped_variable_synchronizer" "test_stamped_variable_synchronizer.cpp") -target_link_libraries("test_stamped_variable_synchronizer" - ${PROJECT_NAME} - fuse_constraints::fuse_constraints - fuse_graphs::fuse_graphs -) +# CORE GTESTS +# ====================================================================================== +ament_add_gtest("test_stamped_variable_synchronizer" + "test_stamped_variable_synchronizer.cpp") +target_link_libraries( + "test_stamped_variable_synchronizer" ${PROJECT_NAME} + fuse_constraints::fuse_constraints fuse_graphs::fuse_graphs) ament_add_gtest("test_path_2d_publisher" "test_path_2d_publisher.cpp") -target_link_libraries("test_path_2d_publisher" - ${PROJECT_NAME} - fuse_constraints::fuse_constraints - fuse_graphs::fuse_graphs -) +target_link_libraries( + "test_path_2d_publisher" ${PROJECT_NAME} fuse_constraints::fuse_constraints + fuse_graphs::fuse_graphs) ament_add_gtest("test_pose_2d_publisher" "test_pose_2d_publisher.cpp") -target_link_libraries("test_pose_2d_publisher" - ${PROJECT_NAME} - fuse_constraints::fuse_constraints - fuse_graphs::fuse_graphs -) +target_link_libraries( + "test_pose_2d_publisher" ${PROJECT_NAME} fuse_constraints::fuse_constraints + fuse_graphs::fuse_graphs) diff --git a/fuse_publishers/test/test_path_2d_publisher.cpp b/fuse_publishers/test/test_path_2d_publisher.cpp index 1c2026d3c..05ebcd059 100644 --- a/fuse_publishers/test/test_path_2d_publisher.cpp +++ b/fuse_publishers/test/test_path_2d_publisher.cpp @@ -53,7 +53,6 @@ #include #include - /** * @brief Test fixture for the Path2DPublisher * @@ -64,39 +63,33 @@ class Path2DPublisherTestFixture : public ::testing::Test { public: Path2DPublisherTestFixture() - : graph_(fuse_graphs::HashGraph::make_shared()), - transaction_(fuse_core::Transaction::make_shared()), - received_path_msg_(false), - received_pose_array_msg_(false) + : graph_(fuse_graphs::HashGraph::make_shared()) + , transaction_(fuse_core::Transaction::make_shared()) + , received_path_msg_(false) + , received_pose_array_msg_(false) { // Add a few pose variables - auto position1 = - fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); + auto position1 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); position1->x() = 1.01; position1->y() = 2.01; - auto orientation1 = - fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); + auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); orientation1->yaw() = 3.01; - auto position2 = - fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); + auto position2 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); position2->x() = 1.02; position2->y() = 2.02; - auto orientation2 = - fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); + auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); orientation2->yaw() = 3.02; - auto position3 = - fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); + auto position3 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); position3->x() = 1.03; position3->y() = 2.03; - auto orientation3 = - fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); + auto orientation3 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); orientation3->yaw() = 3.03; - auto position4 = fuse_variables::Position2DStamped::make_shared( - rclcpp::Time(1235, 11, RCL_ROS_TIME), fuse_core::uuid::generate("kitt")); + auto position4 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 11, RCL_ROS_TIME), + fuse_core::uuid::generate("kitt")); position4->x() = 1.04; position4->y() = 2.04; - auto orientation4 = fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time(1235, 11, RCL_ROS_TIME), fuse_core::uuid::generate("kitt")); + auto orientation4 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 11, RCL_ROS_TIME), + fuse_core::uuid::generate("kitt")); orientation4->yaw() = 3.04; transaction_->addInvolvedStamp(position1->stamp()); @@ -120,38 +113,34 @@ class Path2DPublisherTestFixture : public ::testing::Test mean1 << 1.01, 2.01, 3.01; fuse_core::Matrix3d cov1; /* *INDENT-OFF* */ - cov1 << 1.01, 0.0, 0.0, 0.0, 2.01, 0.0, 0.0, 0.0, 3.01; + cov1 << 1.01, 0.0, 0.0, 0.0, 2.01, 0.0, 0.0, 0.0, 3.01; /* *INDENT-ON* */ auto constraint1 = - fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position1, *orientation1, mean1, cov1); + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position1, *orientation1, mean1, cov1); fuse_core::Vector3d mean2; mean2 << 1.02, 2.02, 3.02; fuse_core::Matrix3d cov2; /* *INDENT-OFF* */ - cov2 << 1.02, 0.0, 0.0, 0.0, 2.02, 0.0, 0.0, 0.0, 3.02; + cov2 << 1.02, 0.0, 0.0, 0.0, 2.02, 0.0, 0.0, 0.0, 3.02; /* *INDENT-ON* */ auto constraint2 = - fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position2, *orientation2, mean2, cov2); + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position2, *orientation2, mean2, cov2); fuse_core::Vector3d mean3; mean3 << 1.03, 2.03, 3.03; fuse_core::Matrix3d cov3; /* *INDENT-OFF* */ - cov3 << 1.03, 0.0, 0.0, 0.0, 2.03, 0.0, 0.0, 0.0, 3.03; + cov3 << 1.03, 0.0, 0.0, 0.0, 2.03, 0.0, 0.0, 0.0, 3.03; /* *INDENT-ON* */ auto constraint3 = - fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position3, *orientation3, mean3, cov3); + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position3, *orientation3, mean3, cov3); fuse_core::Vector3d mean4; mean4 << 1.04, 2.04, 3.04; fuse_core::Matrix3d cov4; /* *INDENT-OFF* */ - cov4 << 1.04, 0.0, 0.0, 0.0, 2.04, 0.0, 0.0, 0.0, 3.04; + cov4 << 1.04, 0.0, 0.0, 0.0, 2.04, 0.0, 0.0, 0.0, 3.04; /* *INDENT-ON* */ auto constraint4 = - fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position4, *orientation4, mean4, cov4); + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position4, *orientation4, mean4, cov4); transaction_->addConstraint(constraint1); transaction_->addConstraint(constraint2); transaction_->addConstraint(constraint3); @@ -166,35 +155,33 @@ class Path2DPublisherTestFixture : public ::testing::Test { rclcpp::init(0, nullptr); executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); rclcpp::shutdown(); } - void pathCallback(const nav_msgs::msg::Path & msg) + void pathCallback(const nav_msgs::msg::Path& msg) { path_msg_ = msg; received_path_msg_ = true; } - void poseArrayCallback(const geometry_msgs::msg::PoseArray & msg) + void poseArrayCallback(const geometry_msgs::msg::PoseArray& msg) { pose_array_msg_ = msg; received_pose_array_msg_ = true; } - std::thread spinner_; //!< Internal thread for spinning the executor + std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; protected: @@ -210,10 +197,7 @@ TEST_F(Path2DPublisherTestFixture, PublishPath) { // Test that the expected PoseStamped message is published rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "test_publisher.frame_id:=test_map"}); + options.arguments({ "--ros-args", "-p", "test_publisher.frame_id:=test_map" }); auto node = rclcpp::Node::make_shared("test_path_2d_publisher_node", options); executor_->add_node(node); @@ -226,20 +210,20 @@ TEST_F(Path2DPublisherTestFixture, PublishPath) // Subscribe to the "path" topic auto subscriber1 = node->create_subscription( - "/test_publisher/path", 1, - std::bind(&Path2DPublisherTestFixture::pathCallback, this, std::placeholders::_1)); + "/test_publisher/path", 1, std::bind(&Path2DPublisherTestFixture::pathCallback, this, std::placeholders::_1)); // Subscribe to the "pose_array" topic auto subscriber2 = node->create_subscription( - "/test_publisher/pose_array", 1, - std::bind(&Path2DPublisherTestFixture::poseArrayCallback, this, std::placeholders::_1)); + "/test_publisher/pose_array", 1, + std::bind(&Path2DPublisherTestFixture::poseArrayCallback, this, std::placeholders::_1)); // Send the graph to the Publisher to trigger message publishing publisher.notify(transaction_, graph_); // Verify the subscriber received the expected pose rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_path_msg_) && (node->now() < timeout)) { + while ((!received_path_msg_) && (node->now() < timeout)) + { rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } diff --git a/fuse_publishers/test/test_pose_2d_publisher.cpp b/fuse_publishers/test/test_pose_2d_publisher.cpp index 947255cff..ee04675c8 100644 --- a/fuse_publishers/test/test_pose_2d_publisher.cpp +++ b/fuse_publishers/test/test_pose_2d_publisher.cpp @@ -55,7 +55,6 @@ #include - /** * @brief Test fixture for the LatestStampedPose2DPublisher * @@ -66,42 +65,34 @@ class Pose2DPublisherTestFixture : public ::testing::Test { public: Pose2DPublisherTestFixture() - : graph_(fuse_graphs::HashGraph::make_shared()), - transaction_(fuse_core::Transaction::make_shared()), - received_pose_msg_(false), - received_pose_with_covariance_msg_(false), - received_tf_msg_(false) + : graph_(fuse_graphs::HashGraph::make_shared()) + , transaction_(fuse_core::Transaction::make_shared()) + , received_pose_msg_(false) + , received_pose_with_covariance_msg_(false) + , received_tf_msg_(false) { // Add a few pose variables - auto position1 = - fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); + auto position1 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); position1->x() = 1.01; position1->y() = 2.01; - auto orientation1 = - fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); + auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); orientation1->yaw() = 3.01; - auto position2 = - fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); + auto position2 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); position2->x() = 1.02; position2->y() = 2.02; - auto orientation2 = - fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); + auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); orientation2->yaw() = 3.02; - auto position3 = - fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); + auto position3 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); position3->x() = 1.03; position3->y() = 2.03; - auto orientation3 = - fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); + auto orientation3 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); orientation3->yaw() = 3.03; - auto position4 = fuse_variables::Position2DStamped::make_shared( - rclcpp::Time(1235, 11, RCL_ROS_TIME), - fuse_core::uuid::generate("kitt")); + auto position4 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 11, RCL_ROS_TIME), + fuse_core::uuid::generate("kitt")); position4->x() = 1.04; position4->y() = 2.04; - auto orientation4 = fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time(1235, 11, RCL_ROS_TIME), - fuse_core::uuid::generate("kitt")); + auto orientation4 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 11, RCL_ROS_TIME), + fuse_core::uuid::generate("kitt")); orientation4->yaw() = 3.04; transaction_->addInvolvedStamp(position1->stamp()); @@ -125,26 +116,26 @@ class Pose2DPublisherTestFixture : public ::testing::Test mean1 << 1.01, 2.01, 3.01; fuse_core::Matrix3d cov1; cov1 << 1.01, 0.0, 0.0, 0.0, 2.01, 0.0, 0.0, 0.0, 3.01; - auto constraint1 = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position1, *orientation1, mean1, cov1); + auto constraint1 = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position1, *orientation1, mean1, cov1); fuse_core::Vector3d mean2; mean2 << 1.02, 2.02, 3.02; fuse_core::Matrix3d cov2; cov2 << 1.02, 0.0, 0.0, 0.0, 2.02, 0.0, 0.0, 0.0, 3.02; - auto constraint2 = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position2, *orientation2, mean2, cov2); + auto constraint2 = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position2, *orientation2, mean2, cov2); fuse_core::Vector3d mean3; mean3 << 1.03, 2.03, 3.03; fuse_core::Matrix3d cov3; cov3 << 1.03, 0.0, 0.0, 0.0, 2.03, 0.0, 0.0, 0.0, 3.03; - auto constraint3 = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position3, *orientation3, mean3, cov3); + auto constraint3 = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position3, *orientation3, mean3, cov3); fuse_core::Vector3d mean4; mean4 << 1.04, 2.04, 3.04; fuse_core::Matrix3d cov4; cov4 << 1.04, 0.0, 0.0, 0.0, 2.04, 0.0, 0.0, 0.0, 3.04; - auto constraint4 = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position4, *orientation4, mean4, cov4); + auto constraint4 = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared("test", *position4, *orientation4, mean4, cov4); transaction_->addConstraint(constraint1); transaction_->addConstraint(constraint2); transaction_->addConstraint(constraint3); @@ -172,41 +163,39 @@ class Pose2DPublisherTestFixture : public ::testing::Test rclcpp::init(0, nullptr); executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); rclcpp::shutdown(); } - void poseCallback(const geometry_msgs::msg::PoseStamped & msg) + void poseCallback(const geometry_msgs::msg::PoseStamped& msg) { received_pose_msg_ = true; pose_msg_ = msg; } - void poseWithCovarianceCallback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg) + void poseWithCovarianceCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { received_pose_with_covariance_msg_ = true; pose_with_covariance_msg_ = msg; } - void tfCallback(const tf2_msgs::msg::TFMessage & msg) + void tfCallback(const tf2_msgs::msg::TFMessage& msg) { received_tf_msg_ = true; tf_msg_ = msg; } - std::thread spinner_; //!< Internal thread for spinning the executor + std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; protected: @@ -225,13 +214,9 @@ TEST_F(Pose2DPublisherTestFixture, PublishPose) { // Test that the expected PoseStamped message is published rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "test_publisher.map_frame:=test_map", - "-p", "test_publisher.odom_frame:=test_odom", - "-p", "test_publisher.base_frame:=test_base", - "-p", "test_publisher.publish_to_tf:=false"}); + options.arguments({ "--ros-args", "-p", "test_publisher.map_frame:=test_map", "-p", + "test_publisher.odom_frame:=test_odom", "-p", "test_publisher.base_frame:=test_base", "-p", + "test_publisher.publish_to_tf:=false" }); auto node = rclcpp::Node::make_shared("test_pose_2d_publisher_node", options); executor_->add_node(node); @@ -242,15 +227,15 @@ TEST_F(Pose2DPublisherTestFixture, PublishPose) // Subscribe to the "pose" topic auto subscriber1 = node->create_subscription( - "/test_publisher/pose", 1, - std::bind(&Pose2DPublisherTestFixture::poseCallback, this, std::placeholders::_1)); + "/test_publisher/pose", 1, std::bind(&Pose2DPublisherTestFixture::poseCallback, this, std::placeholders::_1)); // Send the graph to the Publisher to trigger message publishing publisher.notify(transaction_, graph_); // Verify the subscriber received the expected pose rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_pose_msg_) && (node->now() < timeout)) { + while ((!received_pose_msg_) && (node->now() < timeout)) + { rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } @@ -267,13 +252,9 @@ TEST_F(Pose2DPublisherTestFixture, PublishPoseWithCovariance) { // Test that the expected PoseWithCovarianceStamped message is published rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "test_publisher.map_frame:=test_map", - "-p", "test_publisher.odom_frame:=test_odom", - "-p", "test_publisher.base_frame:=test_base", - "-p", "test_publisher.publish_to_tf:=false"}); + options.arguments({ "--ros-args", "-p", "test_publisher.map_frame:=test_map", "-p", + "test_publisher.odom_frame:=test_odom", "-p", "test_publisher.base_frame:=test_base", "-p", + "test_publisher.publish_to_tf:=false" }); auto node = rclcpp::Node::make_shared("test_pose_2d_publisher_node", options); executor_->add_node(node); @@ -284,17 +265,16 @@ TEST_F(Pose2DPublisherTestFixture, PublishPoseWithCovariance) // Subscribe to the "pose_with_covariance" topic auto subscriber1 = node->create_subscription( - "/test_publisher/pose_with_covariance", 1, - std::bind( - &Pose2DPublisherTestFixture::poseWithCovarianceCallback, this, - std::placeholders::_1)); + "/test_publisher/pose_with_covariance", 1, + std::bind(&Pose2DPublisherTestFixture::poseWithCovarianceCallback, this, std::placeholders::_1)); // Send the graph to the Publisher to trigger message publishing publisher.notify(transaction_, graph_); // Verify the subscriber received the expected pose rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_pose_with_covariance_msg_) && (node->now() < timeout)) { + while ((!received_pose_with_covariance_msg_) && (node->now() < timeout)) + { rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } @@ -305,18 +285,14 @@ TEST_F(Pose2DPublisherTestFixture, PublishPoseWithCovariance) EXPECT_NEAR(2.02, pose_with_covariance_msg_.pose.pose.position.y, 1.0e-9); EXPECT_NEAR(0.00, pose_with_covariance_msg_.pose.pose.position.z, 1.0e-9); EXPECT_NEAR(3.02, tf2::getYaw(pose_with_covariance_msg_.pose.pose.orientation), 1.0e-9); - std::vector expected_covariance = - { + std::vector expected_covariance = { /* *INDENT-OFF* */ - 1.02, 0.00, 0.00, 0.00, 0.00, 0.00, - 0.00, 2.02, 0.00, 0.00, 0.00, 0.00, - 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, - 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, - 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, - 0.00, 0.00, 0.00, 0.00, 0.00, 3.02 + 1.02, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.02, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, + 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 3.02 /* *INDENT-ON* */ }; - for (size_t i = 0; i < 36; ++i) { + for (size_t i = 0; i < 36; ++i) + { EXPECT_NEAR(expected_covariance[i], pose_with_covariance_msg_.pose.covariance[i], 1.0e-9); } } @@ -325,13 +301,9 @@ TEST_F(Pose2DPublisherTestFixture, PublishTfWithoutOdom) { // Test that the expected TFMessage is published rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "test_publisher.map_frame:=test_map", - "-p", "test_publisher.odom_frame:=test_base", - "-p", "test_publisher.base_frame:=test_base", - "-p", "test_publisher.publish_to_tf:=true"}); + options.arguments({ "--ros-args", "-p", "test_publisher.map_frame:=test_map", "-p", + "test_publisher.odom_frame:=test_base", "-p", "test_publisher.base_frame:=test_base", "-p", + "test_publisher.publish_to_tf:=true" }); auto node = rclcpp::Node::make_shared("test_pose_2d_publisher_node", options); executor_->add_node(node); @@ -342,8 +314,7 @@ TEST_F(Pose2DPublisherTestFixture, PublishTfWithoutOdom) // Subscribe to the "pose" topic auto subscriber1 = node->create_subscription( - "/tf", 1, - std::bind(&Pose2DPublisherTestFixture::tfCallback, this, std::placeholders::_1)); + "/tf", 1, std::bind(&Pose2DPublisherTestFixture::tfCallback, this, std::placeholders::_1)); // Create a publisher and send it the graph fuse_publishers::Pose2DPublisher publisher; @@ -355,7 +326,8 @@ TEST_F(Pose2DPublisherTestFixture, PublishTfWithoutOdom) // Verify the subscriber received the expected pose rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_tf_msg_) && (node->now() < timeout)) { + while ((!received_tf_msg_) && (node->now() < timeout)) + { rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } @@ -373,20 +345,15 @@ TEST_F(Pose2DPublisherTestFixture, PublishTfWithOdom) { // Test that the expected TFMessage is published rclcpp::NodeOptions options; - options.arguments( - { - "--ros-args", - "-p", "test_publisher.map_frame:=test_map", - "-p", "test_publisher.odom_frame:=test_odom", - "-p", "test_publisher.base_frame:=test_base", - "-p", "test_publisher.publish_to_tf:=true"}); + options.arguments({ "--ros-args", "-p", "test_publisher.map_frame:=test_map", "-p", + "test_publisher.odom_frame:=test_odom", "-p", "test_publisher.base_frame:=test_base", "-p", + "test_publisher.publish_to_tf:=true" }); auto node = rclcpp::Node::make_shared("test_pose_2d_publisher_node", options); executor_->add_node(node); // Subscribe to the "pose" topic auto subscriber1 = node->create_subscription( - "/tf", 1, - std::bind(&Pose2DPublisherTestFixture::tfCallback, this, std::placeholders::_1)); + "/tf", 1, std::bind(&Pose2DPublisherTestFixture::tfCallback, this, std::placeholders::_1)); auto tf_node_ = rclcpp::Node::make_shared("tf_pub_node"); executor_->add_node(tf_node_); @@ -403,7 +370,8 @@ TEST_F(Pose2DPublisherTestFixture, PublishTfWithOdom) // Verify the subscriber received the expected pose rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_tf_msg_) && (node->now() < timeout)) { + while ((!received_tf_msg_) && (node->now() < timeout)) + { rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } diff --git a/fuse_publishers/test/test_stamped_variable_synchronizer.cpp b/fuse_publishers/test/test_stamped_variable_synchronizer.cpp index 03acb1a6d..4a2716ad3 100644 --- a/fuse_publishers/test/test_stamped_variable_synchronizer.cpp +++ b/fuse_publishers/test/test_stamped_variable_synchronizer.cpp @@ -68,47 +68,25 @@ TEST(StampedVariableSynchronizer, FullSearch) // timestamp has been found before // Create the synchronizer - auto sync = - StampedVariableSynchronizer(generate("blank")); + auto sync = StampedVariableSynchronizer(generate("blank")); // Define the transaction and graph auto transaction = fuse_core::Transaction(); auto graph = fuse_graphs::HashGraph(); graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 10, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 10, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 20, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 20, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("Dadblank"))); - graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("Dadblank"))); - graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 40, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("Dadblank"))); + graph.addVariable( + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("Dadblank"))); + graph.addVariable( + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(40, 0, RCL_ROS_TIME), generate("blank"))); // Use the synchronizer auto actual = sync.findLatestCommonStamp(transaction, graph); @@ -124,32 +102,19 @@ TEST(StampedVariableSynchronizer, Update) // Perform an initial search, then use the transaction to perform an incremental update // Create the synchronizer - auto sync = - StampedVariableSynchronizer(generate("blank")); + auto sync = StampedVariableSynchronizer(generate("blank")); // Define the first transaction and graph auto transaction1 = fuse_core::Transaction(); auto graph = fuse_graphs::HashGraph(); graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 10, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 10, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 20, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 20, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); // Use the synchronizer auto actual1 = sync.findLatestCommonStamp(transaction1, graph); @@ -158,35 +123,17 @@ TEST(StampedVariableSynchronizer, Update) // Create an incremental transaction update auto transaction2 = fuse_core::Transaction(); transaction2.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); transaction2.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); transaction2.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 40, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("blank"))); - graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 40, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(40, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable( + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(40, 0, RCL_ROS_TIME), generate("blank"))); // Use the synchronizer auto actual2 = sync.findLatestCommonStamp(transaction2, graph); @@ -197,42 +144,23 @@ TEST(StampedVariableSynchronizer, Remove) { // Perform an initial search, then use the transaction to remove the latest variables // Create the synchronizer - auto sync = - StampedVariableSynchronizer(generate("blank")); + auto sync = StampedVariableSynchronizer(generate("blank")); // Define the first transaction and graph auto transaction1 = fuse_core::Transaction(); auto graph = fuse_graphs::HashGraph(); graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 10, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 10, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 20, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 20, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Orientation2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); graph.addVariable( - fuse_variables::Position2DStamped::make_shared( - rclcpp::Time( - 30, 0, - RCL_ROS_TIME), generate("blank"))); + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); // Use the synchronizer auto actual1 = sync.findLatestCommonStamp(transaction1, graph); @@ -241,13 +169,8 @@ TEST(StampedVariableSynchronizer, Remove) // Create an incremental transaction that removes one of the latest variables auto transaction2 = fuse_core::Transaction(); transaction2.removeVariable( - fuse_variables::Position2DStamped( - rclcpp::Time(30, 0, RCL_ROS_TIME), - generate("blank")).uuid()); - graph.removeVariable( - fuse_variables::Position2DStamped( - rclcpp::Time(30, 0, RCL_ROS_TIME), - generate("blank")).uuid()); + fuse_variables::Position2DStamped(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank")).uuid()); + graph.removeVariable(fuse_variables::Position2DStamped(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank")).uuid()); // Use the synchronizer auto actual2 = sync.findLatestCommonStamp(transaction2, graph); diff --git a/fuse_tutorials/CMakeLists.txt b/fuse_tutorials/CMakeLists.txt index 8dd86a0da..c45b22c37 100644 --- a/fuse_tutorials/CMakeLists.txt +++ b/fuse_tutorials/CMakeLists.txt @@ -20,65 +20,47 @@ find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) -########### -## Build ## -########### - -## fuse_tutorial library -add_library(${PROJECT_NAME} - src/beacon_publisher.cpp - src/range_constraint.cpp - src/range_sensor_model.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$" -) -target_link_libraries(${PROJECT_NAME} PUBLIC - fuse_constraints::fuse_constraints - fuse_core::fuse_core - fuse_models::fuse_models - fuse_variables::fuse_variables - ${nav_msgs_TARGETS} - rclcpp::rclcpp - ${sensor_msgs_TARGETS} -) +# ############################################################################## +# Build ## +# ############################################################################## + +# fuse_tutorial library +add_library(${PROJECT_NAME} src/beacon_publisher.cpp src/range_constraint.cpp + src/range_sensor_model.cpp) +target_include_directories( + ${PROJECT_NAME} + PUBLIC "$" + "$") +target_link_libraries( + ${PROJECT_NAME} + PUBLIC fuse_constraints::fuse_constraints + fuse_core::fuse_core + fuse_models::fuse_models + fuse_variables::fuse_variables + ${nav_msgs_TARGETS} + rclcpp::rclcpp + ${sensor_msgs_TARGETS}) # tutorial_sim executable add_executable(range_sensor_simulator src/range_sensor_simulator.cpp) target_link_libraries(range_sensor_simulator ${PROJECT_NAME}) -############# -## Testing ## -############# - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## -install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-export ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) -install(TARGETS - TARGETS range_sensor_simulator - DESTINATION lib/${PROJECT_NAME} -) +install(TARGETS range_sensor_simulator DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY launch config data - DESTINATION share/${PROJECT_NAME} -) +install(DIRECTORY launch config data DESTINATION share/${PROJECT_NAME}) pluginlib_export_plugin_description_file(fuse_core fuse_plugins.xml) @@ -91,7 +73,6 @@ ament_export_dependencies( fuse_variables nav_msgs rclcpp - sensor_msgs -) + sensor_msgs) ament_package() diff --git a/fuse_tutorials/include/fuse_tutorials/beacon_publisher.hpp b/fuse_tutorials/include/fuse_tutorials/beacon_publisher.hpp index 475407f28..226782169 100644 --- a/fuse_tutorials/include/fuse_tutorials/beacon_publisher.hpp +++ b/fuse_tutorials/include/fuse_tutorials/beacon_publisher.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_tutorials { /** @@ -121,8 +120,9 @@ class BeaconPublisher : public fuse_core::AsyncPublisher * queue. Generally this will be 1, unless you have a good reason to use a multi-threaded * executor. */ - BeaconPublisher() - : fuse_core::AsyncPublisher(1) {} + BeaconPublisher() : fuse_core::AsyncPublisher(1) + { + } /** * @brief Shadowing extension to the AsyncPublisher::initialize call @@ -130,9 +130,8 @@ class BeaconPublisher : public fuse_core::AsyncPublisher * This allows you to bind more node interfaces to the publisher than the ones supported by the * base AsyncPublisher class. */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; /** * @brief Perform any required initialization for the publisher @@ -152,7 +151,9 @@ class BeaconPublisher : public fuse_core::AsyncPublisher * so there is nothing to do here. If this was not a tutorial, I would have omitted the override * entirely. */ - void onStart() override {} + void onStart() override + { + } /** * @brief Perform any required operations to stop publications @@ -162,7 +163,9 @@ class BeaconPublisher : public fuse_core::AsyncPublisher * cleanup, so there is nothing to do here. If this was not a tutorial, I would have omitted the * override entirely. */ - void onStop() override {} + void onStop() override + { + } /** * @brief Called when a newly optimized graph has been generated by the optimizer @@ -179,18 +182,14 @@ class BeaconPublisher : public fuse_core::AsyncPublisher * @param[in] graph - A read-only pointer to the graph object, allowing queries to be performed * whenever needed */ - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) override; + void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; protected: - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncPublisher interfaces_ + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncPublisher interfaces_ std::string map_frame_id_; //!< The name of the robot's map frame diff --git a/fuse_tutorials/include/fuse_tutorials/range_constraint.hpp b/fuse_tutorials/include/fuse_tutorials/range_constraint.hpp index f59f2a475..a9a6e6b64 100644 --- a/fuse_tutorials/include/fuse_tutorials/range_constraint.hpp +++ b/fuse_tutorials/include/fuse_tutorials/range_constraint.hpp @@ -48,7 +48,6 @@ #include #include - namespace fuse_tutorials { /** @@ -115,12 +114,8 @@ class RangeConstraint : public fuse_core::Constraint * @param[in] z - The distance measured between the robot and beacon by our new sensor * @param[in] sigma - The uncertainty of measured distance */ - RangeConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & robot_position, - const fuse_variables::Point2DLandmark & beacon_position, - const double z, - const double sigma); + RangeConstraint(const std::string& source, const fuse_variables::Position2DStamped& robot_position, + const fuse_variables::Point2DLandmark& beacon_position, const double z, const double sigma); /** * @brief Print a human-readable description of the constraint to the provided stream. @@ -130,7 +125,7 @@ class RangeConstraint : public fuse_core::Constraint * * @param[out] stream - The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -146,7 +141,7 @@ class RangeConstraint : public fuse_core::Constraint * CostFunction object when it is done. Also note that the fuse project guarantees the * derived Constraint object will outlive the Ceres Solver CostFunction object. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; private: // Allow Boost Serialization access to private methods and members @@ -167,16 +162,16 @@ class RangeConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & sigma_; - archive & z_; + archive& boost::serialization::base_object(*this); + archive& sigma_; + archive& z_; } - double sigma_ {0.0}; //!< The standard deviation of the range measurement - double z_ {0.0}; //!< The measured range to the beacon + double sigma_{ 0.0 }; //!< The standard deviation of the range measurement + double z_{ 0.0 }; //!< The measured range to the beacon }; } // namespace fuse_tutorials diff --git a/fuse_tutorials/include/fuse_tutorials/range_cost_functor.hpp b/fuse_tutorials/include/fuse_tutorials/range_cost_functor.hpp index 4da4f2c06..3a2920796 100644 --- a/fuse_tutorials/include/fuse_tutorials/range_cost_functor.hpp +++ b/fuse_tutorials/include/fuse_tutorials/range_cost_functor.hpp @@ -89,8 +89,9 @@ class RangeCostFunctor * @param[in] z The measured range to the beacon * @param[in] sigma The standard deviation of the range measurement */ - RangeCostFunctor(const double z, const double sigma) - : sigma_(sigma), z_(z) {} + RangeCostFunctor(const double z, const double sigma) : sigma_(sigma), z_(z) + { + } /** * @brief Compute the costs using the provided stored measurement details and the provided @@ -120,10 +121,8 @@ class RangeCostFunctor * only a single value. * @return True if the cost was computed successfully, false otherwise */ - template - bool operator()( - const T * const robot_position, const T * const beacon_position, - T * residuals) const + template + bool operator()(const T* const robot_position, const T* const beacon_position, T* residuals) const { // Implement our mathematic measurement model: // z_hat = sqrt( (x_robot - x_beacon)^2 + (y_robot - y_beacon)^2 ) @@ -135,10 +134,13 @@ class RangeCostFunctor auto dx = robot_position[0] - beacon_position[0]; auto dy = robot_position[1] - beacon_position[1]; auto norm_sq = dx * dx + dy * dy; - if (norm_sq > 0.0) { + if (norm_sq > 0.0) + { auto z_hat = ceres::sqrt(norm_sq); residuals[0] = (T(z_) - z_hat) / T(sigma_); - } else { + } + else + { residuals[0] = T(z_) / T(sigma_); } return true; @@ -146,7 +148,7 @@ class RangeCostFunctor private: double sigma_; //!< The standard deviation of the range measurement - double z_; //!< The measured range to the beacon + double z_; //!< The measured range to the beacon }; } // namespace fuse_tutorials diff --git a/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp b/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp index 1e903dead..52e5baa99 100644 --- a/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp +++ b/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp @@ -43,7 +43,6 @@ #include #include - namespace fuse_tutorials { /** @@ -129,16 +128,15 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel * optimizer node. We do, however, specify the number of threads to use to spin the callback * queue. Generally this will be 1, unless you have a good reason to use a multi-threaded spinner. */ - RangeSensorModel() - : fuse_core::AsyncSensorModel(1), logger_(rclcpp::get_logger("uninitialized")) {} + RangeSensorModel() : fuse_core::AsyncSensorModel(1), logger_(rclcpp::get_logger("uninitialized")) + { + } /** * @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; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Receives the set of known beacon positions @@ -151,7 +149,7 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel * * @param[in] msg - Message containing the database of known but noisy beacon positions. */ - void priorBeaconsCallback(const sensor_msgs::msg::PointCloud2 & msg); + void priorBeaconsCallback(const sensor_msgs::msg::PointCloud2& msg); /** * @brief Callback for range measurement messages @@ -162,7 +160,7 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel * * @param[in] msg - The range message to process */ - void rangesCallback(const sensor_msgs::msg::PointCloud2 & msg); + void rangesCallback(const sensor_msgs::msg::PointCloud2& msg); protected: /** @@ -192,12 +190,10 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel double sigma; }; - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Logger logger_; //!< The sensor model's logger @@ -205,7 +201,7 @@ class RangeSensorModel : public fuse_core::AsyncSensorModel //!< ROS subscription for the database of prior beacon positions rclcpp::Subscription::SharedPtr beacon_sub_; - bool initialized_ {false}; //!< Flag indicating the initial beacon positions have been processed + bool initialized_{ false }; //!< Flag indicating the initial beacon positions have been processed //!< ROS subscription for the range sensor measurements rclcpp::Subscription::SharedPtr sub_; diff --git a/fuse_tutorials/launch/fuse_simple_tutorial.launch.py b/fuse_tutorials/launch/fuse_simple_tutorial.launch.py index 46a3308e2..f0a7d0414 100755 --- a/fuse_tutorials/launch/fuse_simple_tutorial.launch.py +++ b/fuse_tutorials/launch/fuse_simple_tutorial.launch.py @@ -22,31 +22,46 @@ def generate_launch_description(): - pkg_dir = FindPackageShare('fuse_tutorials') + pkg_dir = FindPackageShare("fuse_tutorials") - return LaunchDescription([ - ExecuteProcess( - cmd=['ros2', 'bag', 'play', - PathJoinSubstitution([pkg_dir, 'data', 'turtlebot3.bag']), - '--clock', '-l', '-d', '3'], - output='screen' - ), - - SetParameter(name='use_sim_time', value=True), - Node( - package='fuse_optimizers', - executable='fixed_lag_smoother_node', - name='state_estimator', - parameters=[PathJoinSubstitution([ - pkg_dir, 'config', 'fuse_simple_tutorial.yaml' - ])] - ), - Node( - package='rviz2', - executable='rviz2', - name='rviz', - arguments=[ - '-d', [PathJoinSubstitution([pkg_dir, 'config', 'fuse_simple_tutorial.rviz'])] - ] - ) - ]) + return LaunchDescription( + [ + ExecuteProcess( + cmd=[ + "ros2", + "bag", + "play", + PathJoinSubstitution([pkg_dir, "data", "turtlebot3.bag"]), + "--clock", + "-l", + "-d", + "3", + ], + output="screen", + ), + SetParameter(name="use_sim_time", value=True), + Node( + package="fuse_optimizers", + executable="fixed_lag_smoother_node", + name="state_estimator", + parameters=[ + PathJoinSubstitution( + [pkg_dir, "config", "fuse_simple_tutorial.yaml"] + ) + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz", + arguments=[ + "-d", + [ + PathJoinSubstitution( + [pkg_dir, "config", "fuse_simple_tutorial.rviz"] + ) + ], + ], + ), + ] + ) diff --git a/fuse_tutorials/launch/range_sensor_tutorial.launch.py b/fuse_tutorials/launch/range_sensor_tutorial.launch.py index 2b084629e..0612a5afc 100755 --- a/fuse_tutorials/launch/range_sensor_tutorial.launch.py +++ b/fuse_tutorials/launch/range_sensor_tutorial.launch.py @@ -21,34 +21,45 @@ def generate_launch_description(): - pkg_dir = FindPackageShare('fuse_tutorials') + pkg_dir = FindPackageShare("fuse_tutorials") - return LaunchDescription([ - Node(package='tf2_ros', - executable='static_transform_publisher', - arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']), - Node( - package='fuse_tutorials', - executable='range_sensor_simulator', - name='range_sensor_simulator', - output='screen' - ), - Node( - package='fuse_optimizers', - executable='fixed_lag_smoother_node', - name='state_estimation', - output='screen', - parameters=[PathJoinSubstitution([ - pkg_dir, 'config', 'range_sensor_tutorial.yaml' - ])] - ), - Node( - package='rviz2', - executable='rviz2', - name='rviz', - output='screen', - arguments=[ - '-d', [PathJoinSubstitution([pkg_dir, 'config', 'range_sensor_tutorial.rviz'])] - ] - ) - ]) + return LaunchDescription( + [ + Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0", "0", "0", "0", "0", "0", "map", "odom"], + ), + Node( + package="fuse_tutorials", + executable="range_sensor_simulator", + name="range_sensor_simulator", + output="screen", + ), + Node( + package="fuse_optimizers", + executable="fixed_lag_smoother_node", + name="state_estimation", + output="screen", + parameters=[ + PathJoinSubstitution( + [pkg_dir, "config", "range_sensor_tutorial.yaml"] + ) + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz", + output="screen", + arguments=[ + "-d", + [ + PathJoinSubstitution( + [pkg_dir, "config", "range_sensor_tutorial.rviz"] + ) + ], + ], + ), + ] + ) diff --git a/fuse_tutorials/package.xml b/fuse_tutorials/package.xml index 828304a6a..90d9b8105 100644 --- a/fuse_tutorials/package.xml +++ b/fuse_tutorials/package.xml @@ -34,9 +34,6 @@ rviz2 sensor_msgs - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/fuse_tutorials/src/beacon_publisher.cpp b/fuse_tutorials/src/beacon_publisher.cpp index b68bd35a8..4317e096b 100644 --- a/fuse_tutorials/src/beacon_publisher.cpp +++ b/fuse_tutorials/src/beacon_publisher.cpp @@ -49,12 +49,10 @@ // Register this publisher with ROS as a plugin. PLUGINLIB_EXPORT_CLASS(fuse_tutorials::BeaconPublisher, fuse_core::Publisher); - namespace fuse_tutorials { -void BeaconPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void BeaconPublisher::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -72,20 +70,21 @@ void BeaconPublisher::onInit() pub_options.callback_group = cb_group_; beacon_publisher_ = rclcpp::create_publisher( - interfaces_, fuse_core::joinTopicName(name_, "beacons"), 1, pub_options); + interfaces_, fuse_core::joinTopicName(name_, "beacons"), 1, pub_options); } -void BeaconPublisher::notifyCallback( - fuse_core::Transaction::ConstSharedPtr /* transaction */, - fuse_core::Graph::ConstSharedPtr graph) +void BeaconPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr /* transaction */, + fuse_core::Graph::ConstSharedPtr graph) { // This is where all of the processing happens in this publisher implementation. All of the // beacons are represented as fuse_variables::Point2DLandmark objects. We loop through the // variables in the graph and keep a pointer to the variables that are the correct type. - auto beacons = std::vector(); - for (const auto & variable : graph->getVariables()) { - const auto beacon = dynamic_cast(&variable); - if (beacon) { + auto beacons = std::vector(); + for (const auto& variable : graph->getVariables()) + { + const auto beacon = dynamic_cast(&variable); + if (beacon) + { beacons.push_back(beacon); } } @@ -110,8 +109,9 @@ void BeaconPublisher::notifyCallback( sensor_msgs::PointCloud2Iterator y_it(msg, "y"); sensor_msgs::PointCloud2Iterator z_it(msg, "z"); sensor_msgs::PointCloud2Iterator id_it(msg, "id"); - for (auto id = 0u; id < beacons.size(); ++id) { - const auto & beacon = beacons.at(id); + for (auto id = 0u; id < beacons.size(); ++id) + { + const auto& beacon = beacons.at(id); *x_it = static_cast(beacon->x()); *y_it = static_cast(beacon->y()); *z_it = 0.0f; diff --git a/fuse_tutorials/src/range_constraint.cpp b/fuse_tutorials/src/range_constraint.cpp index 9760bfcb9..de4f8887f 100644 --- a/fuse_tutorials/src/range_constraint.cpp +++ b/fuse_tutorials/src/range_constraint.cpp @@ -51,19 +51,17 @@ namespace fuse_tutorials // variable order defined in the RangeCostFunctor must match the variable order provided to the base // class Constraint constructor. In this case, robot position, then the beacon position // fuse_core::Constraint(source, { robot_position.uuid(), beacon_position.uuid() }) -RangeConstraint::RangeConstraint( - const std::string & source, - const fuse_variables::Position2DStamped & robot_position, - const fuse_variables::Point2DLandmark & beacon_position, - const double z, - const double sigma) -: fuse_core::Constraint(source, {robot_position.uuid(), beacon_position.uuid()}), // NOLINT - sigma_(sigma), - z_(z) +RangeConstraint::RangeConstraint(const std::string& source, const fuse_variables::Position2DStamped& robot_position, + const fuse_variables::Point2DLandmark& beacon_position, const double z, + const double sigma) + : fuse_core::Constraint(source, { robot_position.uuid(), beacon_position.uuid() }) + , // NOLINT + sigma_(sigma) + , z_(z) { } -void RangeConstraint::print(std::ostream & stream) const +void RangeConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -74,7 +72,7 @@ void RangeConstraint::print(std::ostream & stream) const << " range sigma: " << sigma_ << "\n"; } -ceres::CostFunction * RangeConstraint::costFunction() const +ceres::CostFunction* RangeConstraint::costFunction() const { // Here we use the Ceres Solver AutoDiffCostFunction class to generate a derived CostFunction // object from our RangeCostFunctor. The AutoDiffCostFunction requires the cost functor as the @@ -87,8 +85,7 @@ ceres::CostFunction * RangeConstraint::costFunction() const // is also 2. // If there were additional involved variables, the size of each variable would appear here in // order. - return new ceres::AutoDiffCostFunction( - new RangeCostFunctor(z_, sigma_)); + return new ceres::AutoDiffCostFunction(new RangeCostFunctor(z_, sigma_)); } } // namespace fuse_tutorials diff --git a/fuse_tutorials/src/range_sensor_model.cpp b/fuse_tutorials/src/range_sensor_model.cpp index 9991adc62..89305d04d 100644 --- a/fuse_tutorials/src/range_sensor_model.cpp +++ b/fuse_tutorials/src/range_sensor_model.cpp @@ -48,16 +48,14 @@ PLUGINLIB_EXPORT_CLASS(fuse_tutorials::RangeSensorModel, fuse_core::SensorModel) namespace fuse_tutorials { -void RangeSensorModel::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void RangeSensorModel::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 RangeSensorModel::priorBeaconsCallback(const sensor_msgs::msg::PointCloud2 & msg) +void RangeSensorModel::priorBeaconsCallback(const sensor_msgs::msg::PointCloud2& msg) { // Store a copy of the beacon database. We use a map to allow efficient lookups by ID number. sensor_msgs::PointCloud2ConstIterator x_it(msg, "x"); @@ -65,8 +63,9 @@ void RangeSensorModel::priorBeaconsCallback(const sensor_msgs::msg::PointCloud2 sensor_msgs::PointCloud2ConstIterator z_it(msg, "z"); sensor_msgs::PointCloud2ConstIterator sigma_it(msg, "sigma"); sensor_msgs::PointCloud2ConstIterator id_it(msg, "id"); - for (; x_it != x_it.end(); ++x_it, ++y_it, ++z_it, ++sigma_it, ++id_it) { - beacon_db_[*id_it] = Beacon {*x_it, *y_it, *sigma_it}; + for (; x_it != x_it.end(); ++x_it, ++y_it, ++z_it, ++sigma_it, ++id_it) + { + beacon_db_[*id_it] = Beacon{ *x_it, *y_it, *sigma_it }; } RCLCPP_INFO_STREAM(logger_, "Updated Beacon Database."); } @@ -86,14 +85,8 @@ void RangeSensorModel::onInit() latched_qos.transient_local(); beacon_sub_ = rclcpp::create_subscription( - interfaces_, - "prior_beacons", - latched_qos, - std::bind( - &RangeSensorModel::priorBeaconsCallback, this, std::placeholders::_1 - ), - sub_options - ); + interfaces_, "prior_beacons", latched_qos, + std::bind(&RangeSensorModel::priorBeaconsCallback, this, std::placeholders::_1), sub_options); } void RangeSensorModel::onStart() @@ -112,14 +105,7 @@ void RangeSensorModel::onStart() sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - "ranges", - 10, - std::bind( - &RangeSensorModel::rangesCallback, this, std::placeholders::_1 - ), - sub_options - ); + interfaces_, "ranges", 10, std::bind(&RangeSensorModel::rangesCallback, this, std::placeholders::_1), sub_options); } void RangeSensorModel::onStop() @@ -131,7 +117,7 @@ void RangeSensorModel::onStop() sub_.reset(); } -void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2 & msg) +void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2& msg) { // We received a new message for our sensor. This is where most of the processing happens for our // sensor model. We take the published ROS message and transform it into one or more Constraints, @@ -139,7 +125,8 @@ void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2 & msg) // However, if we have not received the prior beacon positions yet, we cannot process the range // messages. - if (beacon_db_.empty()) { + if (beacon_db_.empty()) + { return; } @@ -169,7 +156,8 @@ void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2 & msg) sensor_msgs::PointCloud2ConstIterator id_it(msg, "id"); sensor_msgs::PointCloud2ConstIterator range_it(msg, "range"); sensor_msgs::PointCloud2ConstIterator sigma_it(msg, "sigma"); - for (; id_it != id_it.end(); ++id_it, ++range_it, ++sigma_it) { + for (; id_it != id_it.end(); ++id_it, ++range_it, ++sigma_it) + { // Each measure range will involve a different observed beacon. Construct a variable for this // measurement's beacon. auto beacon = beacon_db_[*id_it]; @@ -180,12 +168,8 @@ void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2 & msg) // Now that we have the involved variables defined, create a constraint for this sensor // measurement - auto constraint = fuse_tutorials::RangeConstraint::make_shared( - this->name(), - *robot_position, - *beacon_position, - *range_it, - *sigma_it); + auto constraint = fuse_tutorials::RangeConstraint::make_shared(this->name(), *robot_position, *beacon_position, + *range_it, *sigma_it); transaction->addConstraint(constraint); // If this is the very first measurement, add a prior position constraint on all of the beacons @@ -196,15 +180,14 @@ void RangeSensorModel::rangesCallback(const sensor_msgs::msg::PointCloud2 & msg) // beacons could be tracked over multiple samples. Once the beacon has been observed with enough // parallax, then all measurements of that beacon could be added at once. Some type of delayed // initialization scheme is common in vision-based odometry and SLAM systems. - if (!initialized_) { + if (!initialized_) + { auto mean = fuse_core::Vector2d(beacon.x, beacon.y); auto cov = fuse_core::Matrix2d(); cov << beacon.sigma * beacon.sigma, 0.0, 0.0, beacon.sigma * beacon.sigma; /* *INDENT-OFF* */ - auto prior = - fuse_constraints::AbsoluteConstraint::make_shared( - this->name(), *beacon_position, mean, cov - ); + auto prior = fuse_constraints::AbsoluteConstraint::make_shared( + this->name(), *beacon_position, mean, cov); /* *INDENT-ON* */ transaction->addConstraint(prior); } diff --git a/fuse_tutorials/src/range_sensor_simulator.cpp b/fuse_tutorials/src/range_sensor_simulator.cpp index ba25807f2..89cb88d52 100644 --- a/fuse_tutorials/src/range_sensor_simulator.cpp +++ b/fuse_tutorials/src/range_sensor_simulator.cpp @@ -45,25 +45,25 @@ #include #include -static constexpr double SITE_WIDTH = 100.0; //!< The width/length of the test area in meters -static constexpr double BEACON_SPACING = 20.0; //!< The distance between each range beacon -static constexpr double BEACON_SIGMA = 4.0; //!< Std dev used to create the database of noisy - //!< beacon positions +static constexpr double SITE_WIDTH = 100.0; //!< The width/length of the test area in meters +static constexpr double BEACON_SPACING = 20.0; //!< The distance between each range beacon +static constexpr double BEACON_SIGMA = 4.0; //!< Std dev used to create the database of noisy + //!< beacon positions static constexpr double ROBOT_PATH_RADIUS = 0.35 * SITE_WIDTH; //!< The radius of the simulated //!< robot's path -static constexpr double ROBOT_VELOCITY = 10.0; //!< The forward velocity of our simulated robot -static constexpr char BASELINK_FRAME[] = "base_link"; //!< The base_link frame id used when - //!< publishing sensor data -static constexpr char ODOM_FRAME[] = "odom"; //!< The odom frame id used when publishing wheel - //!< odometry 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 double ODOM_VX_SIGMA = 0.5; //!< Std dev of simulated Odometry linear velocity - //!< measurement noise -static constexpr double ODOM_VYAW_SIGMA = 0.5; //!< Std dev of simulated Odometry angular velocity - //!< measurement noise -static constexpr double RANGE_SIGMA = 0.5; //!< Std dev of simulated beacon range measurement noise +static constexpr double ROBOT_VELOCITY = 10.0; //!< The forward velocity of our simulated robot +static constexpr char BASELINK_FRAME[] = "base_link"; //!< The base_link frame id used when + //!< publishing sensor data +static constexpr char ODOM_FRAME[] = "odom"; //!< The odom frame id used when publishing wheel + //!< odometry 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 double ODOM_VX_SIGMA = 0.5; //!< Std dev of simulated Odometry linear velocity + //!< measurement noise +static constexpr double ODOM_VYAW_SIGMA = 0.5; //!< Std dev of simulated Odometry angular velocity + //!< measurement noise +static constexpr double RANGE_SIGMA = 0.5; //!< Std dev of simulated beacon range measurement noise /** * @brief The position of a "range beacon" @@ -94,9 +94,11 @@ struct Robot std::vector createBeacons() { auto beacons = std::vector(); - for (auto x = -SITE_WIDTH / 2; x <= SITE_WIDTH / 2; x += BEACON_SPACING) { - for (auto y = -SITE_WIDTH / 2; y <= SITE_WIDTH / 2; y += BEACON_SPACING) { - beacons.push_back({x, y}); // NOLINT[whitespace/braces] + for (auto x = -SITE_WIDTH / 2; x <= SITE_WIDTH / 2; x += BEACON_SPACING) + { + for (auto y = -SITE_WIDTH / 2; y <= SITE_WIDTH / 2; y += BEACON_SPACING) + { + beacons.push_back({ x, y }); // NOLINT[whitespace/braces] } } return beacons; @@ -105,15 +107,16 @@ std::vector createBeacons() /** * @brief Create a noisy set of beacon priors from the true set of beacons */ -std::vector createNoisyBeacons(const std::vector & beacons) +std::vector createNoisyBeacons(const std::vector& beacons) { static std::random_device rd{}; - static std::mt19937 generator{rd()}; - static std::normal_distribution<> noise{0.0, BEACON_SIGMA}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> noise{ 0.0, BEACON_SIGMA }; auto noisy_beacons = std::vector(); - for (const auto & beacon : beacons) { - noisy_beacons.push_back({beacon.x + noise(generator), beacon.y + noise(generator)}); // NOLINT + for (const auto& beacon : beacons) + { + noisy_beacons.push_back({ beacon.x + noise(generator), beacon.y + noise(generator) }); // NOLINT } return noisy_beacons; } @@ -121,36 +124,37 @@ std::vector createNoisyBeacons(const std::vector & beacons) /** * @brief Convert the set of beacons into a pointcloud for visualization purposes */ -sensor_msgs::msg::PointCloud2::SharedPtr beaconsToPointcloud( - const std::vector & beacons, - const rclcpp::Clock & clock) +sensor_msgs::msg::PointCloud2::SharedPtr beaconsToPointcloud(const std::vector& beacons, + const rclcpp::Clock& clock) { auto msg = std::make_shared(); msg->header.stamp = clock.now(); msg->header.frame_id = MAP_FRAME; sensor_msgs::PointCloud2Modifier modifier(*msg); - modifier.setPointCloud2Fields( - 5, "x", 1, sensor_msgs::msg::PointField::FLOAT32, - "y", 1, sensor_msgs::msg::PointField::FLOAT32, - "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "sigma", 1, sensor_msgs::msg::PointField::FLOAT32, - "id", 1, sensor_msgs::msg::PointField::UINT32); + modifier.setPointCloud2Fields(5, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, + sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "sigma", 1, sensor_msgs::msg::PointField::FLOAT32, "id", 1, + sensor_msgs::msg::PointField::UINT32); modifier.resize(beacons.size()); sensor_msgs::PointCloud2Iterator x_it(*msg, "x"); sensor_msgs::PointCloud2Iterator y_it(*msg, "y"); sensor_msgs::PointCloud2Iterator z_it(*msg, "z"); sensor_msgs::PointCloud2Iterator sigma_it(*msg, "sigma"); sensor_msgs::PointCloud2Iterator id_it(*msg, "id"); - for (auto id = 0u; id < beacons.size(); ++id) { + for (auto id = 0u; id < beacons.size(); ++id) + { // Compute the distance to each beacon - const auto & beacon = beacons.at(id); + const auto& beacon = beacons.at(id); *x_it = static_cast(beacon.x); *y_it = static_cast(beacon.y); *z_it = 10.0f; *sigma_it = static_cast(BEACON_SIGMA); *id_it = id; - ++x_it; ++y_it; ++z_it; ++sigma_it, ++id_it; + ++x_it; + ++y_it; + ++z_it; + ++sigma_it, ++id_it; } return msg; } @@ -158,7 +162,7 @@ sensor_msgs::msg::PointCloud2::SharedPtr beaconsToPointcloud( /** * @brief Convert the robot state into a ground truth odometry message */ -nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot & state) +nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot& state) { auto msg = std::make_shared(); msg->header.stamp = state.stamp; @@ -197,11 +201,8 @@ nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot & state) * * The state estimator will not run until it has been sent a starting pose. */ -void initializeStateEstimation( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const Robot & state, - const rclcpp::Clock::SharedPtr & clock, - const rclcpp::Logger & logger) +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(); @@ -220,29 +221,26 @@ void initializeStateEstimation( srv->pose.pose.covariance[28] = 1.0; srv->pose.pose.covariance[35] = 1.0; - auto client = rclcpp::create_client( - interfaces.get_node_base_interface(), - interfaces.get_node_graph_interface(), - interfaces.get_node_services_interface(), - "/state_estimation/set_pose", - rclcpp::ServicesQoS() - ); + auto client = rclcpp::create_client(interfaces.get_node_base_interface(), + interfaces.get_node_graph_interface(), + interfaces.get_node_services_interface(), + "/state_estimation/set_pose", rclcpp::ServicesQoS()); while (!client->wait_for_service(std::chrono::seconds(30)) && - interfaces.get_node_base_interface()->get_context()->is_valid()) + interfaces.get_node_base_interface()->get_context()->is_valid()) { - RCLCPP_WARN_STREAM( - logger, "Waiting for '" << client->get_service_name() << "' service to become avaiable."); + RCLCPP_WARN_STREAM(logger, "Waiting for '" << client->get_service_name() << "' service to become available."); } auto success = false; - while (!success) { + 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) != - rclcpp::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR(logger, "service call failed :("); client->remove_pending_request(result_future); @@ -255,7 +253,7 @@ void initializeStateEstimation( /** * @brief Compute the next robot state given the current robot state and a simulated step time */ -Robot simulateRobotMotion(const Robot & previous_state, const rclcpp::Time & now) +Robot simulateRobotMotion(const Robot& previous_state, const rclcpp::Time& now) { auto dt = (now - previous_state.stamp).seconds(); auto theta = std::atan2(previous_state.y, previous_state.x) + (dt * previous_state.vyaw); @@ -273,11 +271,11 @@ Robot simulateRobotMotion(const Robot & previous_state, const rclcpp::Time & now /** * @brief Create a simulated Imu measurement from the current state */ -sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot & robot) +sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot& robot) { static std::random_device rd{}; - static std::mt19937 generator{rd()}; - static std::normal_distribution<> noise{0.0, IMU_SIGMA}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> noise{ 0.0, IMU_SIGMA }; auto msg = std::make_shared(); msg->header.stamp = robot.stamp; @@ -292,12 +290,12 @@ sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot & robot) /** * @brief Create a simulated Odometry measurement from the current state */ -nav_msgs::msg::Odometry::SharedPtr simulateWheelOdometry(const Robot & robot) +nav_msgs::msg::Odometry::SharedPtr simulateWheelOdometry(const Robot& robot) { static std::random_device rd{}; - static std::mt19937 generator{rd()}; - static std::normal_distribution<> vx_noise{0.0, ODOM_VX_SIGMA}; - static std::normal_distribution<> vyaw_noise{0.0, ODOM_VYAW_SIGMA}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> vx_noise{ 0.0, ODOM_VX_SIGMA }; + static std::normal_distribution<> vyaw_noise{ 0.0, ODOM_VYAW_SIGMA }; auto msg = std::make_shared(); msg->header.stamp = robot.stamp; @@ -312,13 +310,11 @@ nav_msgs::msg::Odometry::SharedPtr simulateWheelOdometry(const Robot & robot) return msg; } -sensor_msgs::msg::PointCloud2::SharedPtr simulateRangeSensor( - const Robot & robot, - const std::vector & beacons) +sensor_msgs::msg::PointCloud2::SharedPtr simulateRangeSensor(const Robot& robot, const std::vector& beacons) { static std::random_device rd{}; - static std::mt19937 generator{rd()}; - static std::normal_distribution<> noise{0.0, RANGE_SIGMA}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> noise{ 0.0, RANGE_SIGMA }; auto msg = std::make_shared(); msg->header.stamp = robot.stamp; @@ -326,19 +322,19 @@ sensor_msgs::msg::PointCloud2::SharedPtr simulateRangeSensor( // Configure the pointcloud to have the following fields: id, range, sigma sensor_msgs::PointCloud2Modifier modifier(*msg); - modifier.setPointCloud2Fields( - 3, "id", 1, sensor_msgs::msg::PointField::UINT32, - "range", 1, sensor_msgs::msg::PointField::FLOAT64, - "sigma", 1, sensor_msgs::msg::PointField::FLOAT64); + modifier.setPointCloud2Fields(3, "id", 1, sensor_msgs::msg::PointField::UINT32, "range", 1, + sensor_msgs::msg::PointField::FLOAT64, "sigma", 1, + sensor_msgs::msg::PointField::FLOAT64); // Generate the simulated range to each known beacon modifier.resize(beacons.size()); sensor_msgs::PointCloud2Iterator id_it(*msg, "id"); sensor_msgs::PointCloud2Iterator range_it(*msg, "range"); sensor_msgs::PointCloud2Iterator sigma_it(*msg, "sigma"); - for (auto id = 0u; id < beacons.size(); ++id) { + for (auto id = 0u; id < beacons.size(); ++id) + { // Compute the distance to each beacon - const auto & beacon = beacons.at(id); + const auto& beacon = beacons.at(id); auto dx = robot.x - beacon.x; auto dy = robot.y - beacon.y; auto range = std::sqrt(dx * dx + dy * dy) + noise(generator); @@ -346,7 +342,9 @@ sensor_msgs::msg::PointCloud2::SharedPtr simulateRangeSensor( *id_it = id; *range_it = range; *sigma_it = RANGE_SIGMA; - ++id_it; ++range_it; ++sigma_it; + ++id_it; + ++range_it; + ++sigma_it; } return msg; } @@ -374,7 +372,7 @@ sensor_msgs::msg::PointCloud2::SharedPtr simulateRangeSensor( * - The true position and velocity of the robot is published as a nav_msgs::msg::Odometry message * on the /ground_truth topic at 10Hz */ -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("range_sensor_simulator"); @@ -384,18 +382,12 @@ int main(int argc, char ** argv) auto latched_qos = rclcpp::QoS(1); latched_qos.transient_local(); - auto imu_publisher = - node->create_publisher("imu", 1); - auto true_beacons_publisher = - node->create_publisher("true_beacons", latched_qos); - auto prior_beacons_publisher = - node->create_publisher("prior_beacons", latched_qos); - auto wheel_odom_publisher = - node->create_publisher("wheel_odom", 1); - auto ground_truth_publisher = - node->create_publisher("ground_truth", 1); - auto range_publisher = - node->create_publisher("ranges", 1); + auto imu_publisher = node->create_publisher("imu", 1); + auto true_beacons_publisher = node->create_publisher("true_beacons", latched_qos); + auto prior_beacons_publisher = node->create_publisher("prior_beacons", latched_qos); + auto wheel_odom_publisher = node->create_publisher("wheel_odom", 1); + auto ground_truth_publisher = node->create_publisher("ground_truth", 1); + auto range_publisher = node->create_publisher("ranges", 1); // Create the true set of range beacons auto beacons = createBeacons(); @@ -420,7 +412,8 @@ int main(int argc, char ** argv) // Simulate the robot traveling in a circular path auto rate = rclcpp::Rate(10.0); - while (rclcpp::ok()) { + while (rclcpp::ok()) + { // Simulate the robot motion auto new_state = simulateRobotMotion(state, node->now()); // Publish the new ground truth diff --git a/fuse_variables/CMakeLists.txt b/fuse_variables/CMakeLists.txt index d1d845747..845de66ab 100644 --- a/fuse_variables/CMakeLists.txt +++ b/fuse_variables/CMakeLists.txt @@ -17,12 +17,13 @@ find_package(pluginlib REQUIRED) find_package(Ceres REQUIRED) -########### -## Build ## -########### +# ############################################################################## +# Build ## +# ############################################################################## -## fuse_variables library -add_library(${PROJECT_NAME} +# fuse_variables library +add_library( + ${PROJECT_NAME} src/acceleration_angular_2d_stamped.cpp src/acceleration_angular_3d_stamped.cpp src/acceleration_linear_2d_stamped.cpp @@ -39,51 +40,38 @@ add_library(${PROJECT_NAME} src/velocity_angular_2d_stamped.cpp src/velocity_angular_3d_stamped.cpp src/velocity_linear_2d_stamped.cpp - src/velocity_linear_3d_stamped.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$" -) -target_link_libraries(${PROJECT_NAME} PUBLIC - Ceres::ceres - fuse_core::fuse_core - pluginlib::pluginlib -) + src/velocity_linear_3d_stamped.cpp) +target_include_directories( + ${PROJECT_NAME} + PUBLIC "$" + "$") +target_link_libraries(${PROJECT_NAME} PUBLIC Ceres::ceres fuse_core::fuse_core + pluginlib::pluginlib) -############# -## Testing ## -############# +# ############################################################################## +# Testing ## +# ############################################################################## if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - add_subdirectory(test) endif() -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## -install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-export ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) pluginlib_export_plugin_description_file(fuse_core fuse_plugins.xml) ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET) -ament_export_dependencies( - ament_cmake_ros - fuse_core - pluginlib - Ceres -) +ament_export_dependencies(ament_cmake_ros fuse_core pluginlib Ceres) ament_package() diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h index b9d9e407c..e4e32b8a6 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__ACCELERATION_ANGULAR_2D_STAMPED_H_ #define FUSE_VARIABLES__ACCELERATION_ANGULAR_2D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/acceleration_angular_2d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/acceleration_angular_2d_stamped.hpp instead #include 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 bc9f1714b..efaeeb0f9 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.hpp @@ -86,26 +86,31 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit AccelerationAngular2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit AccelerationAngular2DStamped(const rclcpp::Time& stamp, + const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the angular acceleration. */ - double & yaw() {return data_[YAW];} + double& yaw() + { + return data_[YAW]; + } /** * @brief Read-only access to the angular acceleration. */ - const double & yaw() const {return data_[YAW];} + const double& yaw() const + { + return data_[YAW]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -113,7 +118,10 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -127,11 +135,11 @@ class AccelerationAngular2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + 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); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h index 6c5b6fba1..26c2d3346 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__ACCELERATION_ANGULAR_3D_STAMPED_H_ #define FUSE_VARIABLES__ACCELERATION_ANGULAR_3D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/acceleration_angular_3d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/acceleration_angular_3d_stamped.hpp instead #include 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 b43683cfc..b91b1e1b5 100644 --- a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.hpp @@ -88,46 +88,63 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit AccelerationAngular3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit AccelerationAngular3DStamped(const rclcpp::Time& stamp, + const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the roll (X-axis) angular acceleration. */ - double & roll() {return data_[ROLL];} + double& roll() + { + return data_[ROLL]; + } /** * @brief Read-only access to the roll (X-axis) angular acceleration. */ - const double & roll() const {return data_[ROLL];} + const double& roll() const + { + return data_[ROLL]; + } /** * @brief Read-write access to the pitch (Y-axis) angular acceleration. */ - double & pitch() {return data_[PITCH];} + double& pitch() + { + return data_[PITCH]; + } /** * @brief Read-only access to the pitch (Y-axis) angular acceleration. */ - const double & pitch() const {return data_[PITCH];} + const double& pitch() const + { + return data_[PITCH]; + } /** * @brief Read-write access to the yaw (Z-axis) angular acceleration. */ - double & yaw() {return data_[YAW];} + double& yaw() + { + return data_[YAW]; + } /** * @brief Read-only access to the yaw (Z-axis) angular acceleration. */ - const double & yaw() const {return data_[YAW];} + const double& yaw() const + { + return data_[YAW]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -135,7 +152,10 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -149,11 +169,11 @@ class AccelerationAngular3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + 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); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h index f587aa099..52287c600 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__ACCELERATION_LINEAR_2D_STAMPED_H_ #define FUSE_VARIABLES__ACCELERATION_LINEAR_2D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/acceleration_linear_2d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/acceleration_linear_2d_stamped.hpp instead #include 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 ae7a539a8..cbdf503cd 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.hpp @@ -87,36 +87,47 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit AccelerationLinear2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit AccelerationLinear2DStamped(const rclcpp::Time& stamp, + const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis linear acceleration. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis linear acceleration. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis linear acceleration. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis linear acceleration. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -124,7 +135,10 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -138,11 +152,11 @@ class AccelerationLinear2DStamped : public FixedSizeVariable<2>, public Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + 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); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h index 6808f6b0f..b330ecfd8 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__ACCELERATION_LINEAR_3D_STAMPED_H_ #define FUSE_VARIABLES__ACCELERATION_LINEAR_3D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/acceleration_linear_3d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/acceleration_linear_3d_stamped.hpp instead #include 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 c48e45a81..11c71edaa 100644 --- a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.hpp @@ -88,46 +88,63 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit AccelerationLinear3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit AccelerationLinear3DStamped(const rclcpp::Time& stamp, + const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis linear acceleration. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis linear acceleration. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis linear acceleration. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis linear acceleration. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-write access to the Z-axis linear acceleration. */ - double & z() {return data_[Z];} + double& z() + { + return data_[Z]; + } /** * @brief Read-only access to the Z-axis linear acceleration. */ - const double & z() const {return data_[Z];} + const double& z() const + { + return data_[Z]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -135,7 +152,10 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -149,11 +169,11 @@ class AccelerationLinear3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + 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); + 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 e2451569c..1d4e86f66 100644 --- a/fuse_variables/include/fuse_variables/fixed_size_variable.hpp +++ b/fuse_variables/include/fuse_variables/fixed_size_variable.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_variables { @@ -60,7 +59,7 @@ namespace fuse_variables * of typical variable types (points, poses, calibration parameters) are all known at design/compile * time. */ -template +template class FixedSizeVariable : public fuse_core::Variable { public: @@ -79,10 +78,10 @@ class FixedSizeVariable : public fuse_core::Variable /** * @brief Constructor */ - explicit FixedSizeVariable(const fuse_core::UUID & uuid) - : fuse_core::Variable(uuid), - data_{} // zero-initialize the data array - {} + explicit FixedSizeVariable(const fuse_core::UUID& uuid) + : fuse_core::Variable(uuid), data_{} // zero-initialize the data array + { + } /** * @brief Destructor @@ -95,27 +94,42 @@ class FixedSizeVariable : public fuse_core::Variable * The number of scalar values contained by this variable type is defined by the class template * parameter \p N. */ - size_t size() const override {return N;} + size_t size() const override + { + return N; + } /** * @brief Read-only access to the variable data */ - const double * data() const override {return data_.data();} + const double* data() const override + { + return data_.data(); + } /** * @brief Read-write access to the variable data */ - double * data() override {return data_.data();} + double* data() override + { + return data_.data(); + } /** * @brief Read-only access to the variable data as a std::array */ - const std::array & array() const {return data_;} + const std::array& array() const + { + return data_; + } /** * @brief Read-write access to the variable data as a std::array */ - std::array & array() {return data_;} + std::array& array() + { + return data_; + } protected: std::array data_; //!< Fixed-sized, contiguous memory for holding the variable data @@ -131,16 +145,16 @@ class FixedSizeVariable : public fuse_core::Variable * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & data_; + archive& boost::serialization::base_object(*this); + archive& data_; } }; // Define the constant that was declared above -template +template constexpr size_t FixedSizeVariable::SIZE; } // namespace fuse_variables diff --git a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp index 987d64712..6393926f0 100644 --- a/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_2d_stamped.hpp @@ -78,37 +78,27 @@ class Orientation2DLocalParameterization : public fuse_core::LocalParameterizati return 1; } - bool Plus( - const double * x, - const double * delta, - double * x_plus_delta) const override + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { // Compute the angle increment as a linear update, and handle the 2*Pi rollover x_plus_delta[0] = fuse_core::wrapAngle2D(x[0] + delta[0]); return true; } - bool ComputeJacobian( - const double * /*x*/, - double * jacobian) const override + bool ComputeJacobian(const double* /*x*/, double* jacobian) const override { jacobian[0] = 1.0; return true; } - bool Minus( - const double * x, - const double * y, - double * y_minus_x) const override + bool Minus(const double* x, const double* y, double* y_minus_x) const override { // Compute the difference from x to y, and handle the 2*Pi rollover y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); return true; } - bool ComputeMinusJacobian( - const double * /*x*/, - double * jacobian) const override + bool ComputeMinusJacobian(const double* /*x*/, double* jacobian) const override { jacobian[0] = 1.0; return true; @@ -125,10 +115,10 @@ class Orientation2DLocalParameterization : public fuse_core::LocalParameterizati * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; @@ -145,31 +135,37 @@ class Orientation2DManifold : public fuse_core::Manifold public: FUSE_SMART_PTR_DEFINITIONS(Orientation2DManifold) - int AmbientSize() const override {return 1;} + int AmbientSize() const override + { + return 1; + } - int TangentSize() const override {return 1;} + int TangentSize() const override + { + return 1; + } - bool Plus(const double * x, const double * delta, double * x_plus_delta) const override + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { // Compute the angle increment as a linear update, and handle the 2*Pi rollover x_plus_delta[0] = fuse_core::wrapAngle2D(x[0] + delta[0]); return true; } - bool PlusJacobian(const double * /*x*/, double * jacobian) const override + bool PlusJacobian(const double* /*x*/, double* jacobian) const override { jacobian[0] = 1.0; return true; } - bool Minus(const double * y, const double * x, double * y_minus_x) const override + bool Minus(const double* y, const double* x, double* y_minus_x) const override { // Compute the difference from y to x, and handle the 2*Pi rollover y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); return true; } - bool MinusJacobian(const double * /*x*/, double * jacobian) const override + bool MinusJacobian(const double* /*x*/, double* jacobian) const override { jacobian[0] = 1.0; return true; @@ -185,10 +181,10 @@ class Orientation2DManifold : public fuse_core::Manifold * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; #endif @@ -226,26 +222,30 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit Orientation2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit Orientation2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the heading angle. */ - double & yaw() {return data_[YAW];} + double& yaw() + { + return data_[YAW]; + } /** * @brief Read-only access to the heading angle. */ - const double & yaw() const {return data_[YAW];} + const double& yaw() const + { + return data_[YAW]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Returns the number of elements of the local parameterization space. @@ -253,7 +253,10 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * Since we are overriding the \p localParameterization() method, it is good practice to override * the \p localSize() method as well. */ - size_t localSize() const override {return 1u;} + size_t localSize() const override + { + return 1u; + } /** * @brief Create a new Ceres local parameterization object to apply to updates of this variable @@ -263,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; + fuse_core::LocalParameterization* localParameterization() const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -274,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; + fuse_core::Manifold* manifold() const override; #endif private: @@ -288,11 +291,11 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + 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); + 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 9e7b0ed3b..1781597c1 100644 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/orientation_3d_stamped.hpp @@ -63,7 +63,7 @@ namespace fuse_variables * * ceres/rotation.h is missing this function for some reason. */ -template +template inline static void QuaternionInverse(const T in[4], T out[4]) { out[0] = in[0]; @@ -75,7 +75,7 @@ inline static void QuaternionInverse(const T in[4], T out[4]) /** * @brief A LocalParameterization class for 3D Orientations. * - * 3D orientations add and subtract nonlinearly. Additionally, the typcial 3D orientation + * 3D orientations add and subtract nonlinearly. Additionally, the typical 3D orientation * representation is a quaternion, which has 4 degrees of freedom to parameterize a 3D space. This * local parameterization uses the Rodrigues/angle-axis formulas to combine 3D rotations, along with * the appropriate "analytic" derivatives. @@ -85,14 +85,17 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati public: FUSE_SMART_PTR_DEFINITIONS(Orientation3DLocalParameterization) - int GlobalSize() const override {return 4;} + int GlobalSize() const override + { + return 4; + } - int LocalSize() const override {return 3;} + int LocalSize() const override + { + return 3; + } - bool Plus( - const double * x, - const double * delta, - double * x_plus_delta) const override + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { double q_delta[4]; ceres::AngleAxisToQuaternion(delta, q_delta); @@ -100,27 +103,30 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati return true; } - bool ComputeJacobian( - const double * x, - double * jacobian) const override + bool ComputeJacobian(const double* x, double* jacobian) const override { double x0 = x[0] / 2; double x1 = x[1] / 2; double x2 = x[2] / 2; double x3 = x[3] / 2; /* *INDENT-OFF* */ - jacobian[0] = -x1; jacobian[1] = -x2; jacobian[2] = -x3; // NOLINT - jacobian[3] = x0; jacobian[4] = -x3; jacobian[5] = x2; // NOLINT - jacobian[6] = x3; jacobian[7] = x0; jacobian[8] = -x1; // NOLINT - jacobian[9] = -x2; jacobian[10] = x1; jacobian[11] = x0; // NOLINT + jacobian[0] = -x1; + jacobian[1] = -x2; + jacobian[2] = -x3; // NOLINT + jacobian[3] = x0; + jacobian[4] = -x3; + jacobian[5] = x2; // NOLINT + jacobian[6] = x3; + jacobian[7] = x0; + jacobian[8] = -x1; // NOLINT + jacobian[9] = -x2; + jacobian[10] = x1; + jacobian[11] = x0; // NOLINT /* *INDENT-ON* */ return true; } - bool Minus( - const double * x, - const double * y, - double * y_minus_x) const override + bool Minus(const double* x, const double* y, double* y_minus_x) const override { double x_inverse[4]; QuaternionInverse(x, x_inverse); @@ -130,18 +136,25 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati return true; } - bool ComputeMinusJacobian( - const double * x, - double * jacobian) const override + bool ComputeMinusJacobian(const double* x, double* jacobian) const override { double x0 = x[0] * 2; double x1 = x[1] * 2; double x2 = x[2] * 2; double x3 = x[3] * 2; /* *INDENT-OFF* */ - jacobian[0] = -x1; jacobian[1] = x0; jacobian[2] = x3; jacobian[3] = -x2; // NOLINT - jacobian[4] = -x2; jacobian[5] = -x3; jacobian[6] = x0; jacobian[7] = x1; // NOLINT - jacobian[8] = -x3; jacobian[9] = x2; jacobian[10] = -x1; jacobian[11] = x0; // NOLINT + jacobian[0] = -x1; + jacobian[1] = x0; + jacobian[2] = x3; + jacobian[3] = -x2; // NOLINT + jacobian[4] = -x2; + jacobian[5] = -x3; + jacobian[6] = x0; + jacobian[7] = x1; // NOLINT + jacobian[8] = -x3; + jacobian[9] = x2; + jacobian[10] = -x1; + jacobian[11] = x0; // NOLINT /* *INDENT-ON* */ return true; } @@ -157,10 +170,10 @@ class Orientation3DLocalParameterization : public fuse_core::LocalParameterizati * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; @@ -177,11 +190,17 @@ class Orientation3DManifold : public fuse_core::Manifold public: FUSE_SMART_PTR_DEFINITIONS(Orientation3DManifold) - int AmbientSize() const override {return 4;} + int AmbientSize() const override + { + return 4; + } - int TangentSize() const override {return 3;} + int TangentSize() const override + { + return 3; + } - bool Plus(const double * x, const double * delta, double * x_plus_delta) const override + bool Plus(const double* x, const double* delta, double* x_plus_delta) const override { double q_delta[4]; ceres::AngleAxisToQuaternion(delta, q_delta); @@ -189,22 +208,30 @@ class Orientation3DManifold : public fuse_core::Manifold return true; } - bool PlusJacobian(const double * x, double * jacobian) const override + bool PlusJacobian(const double* x, double* jacobian) const override { double x0 = x[0] / 2; double x1 = x[1] / 2; double x2 = x[2] / 2; double x3 = x[3] / 2; /* *INDENT-OFF* */ - jacobian[0] = -x1; jacobian[1] = -x2; jacobian[2] = -x3; // NOLINT - jacobian[3] = x0; jacobian[4] = -x3; jacobian[5] = x2; // NOLINT - jacobian[6] = x3; jacobian[7] = x0; jacobian[8] = -x1; // NOLINT - jacobian[9] = -x2; jacobian[10] = x1; jacobian[11] = x0; // NOLINT + jacobian[0] = -x1; + jacobian[1] = -x2; + jacobian[2] = -x3; // NOLINT + jacobian[3] = x0; + jacobian[4] = -x3; + jacobian[5] = x2; // NOLINT + jacobian[6] = x3; + jacobian[7] = x0; + jacobian[8] = -x1; // NOLINT + jacobian[9] = -x2; + jacobian[10] = x1; + jacobian[11] = x0; // NOLINT /* *INDENT-ON* */ return true; } - bool Minus(const double * y, const double * x, double * y_minus_x) const override + bool Minus(const double* y, const double* x, double* y_minus_x) const override { double x_inverse[4]; QuaternionInverse(x, x_inverse); @@ -214,16 +241,25 @@ class Orientation3DManifold : public fuse_core::Manifold return true; } - bool MinusJacobian(const double * x, double * jacobian) const override + bool MinusJacobian(const double* x, double* jacobian) const override { double x0 = x[0] * 2; double x1 = x[1] * 2; double x2 = x[2] * 2; double x3 = x[3] * 2; /* *INDENT-OFF* */ - jacobian[0] = -x1; jacobian[1] = x0; jacobian[2] = x3; jacobian[3] = -x2; // NOLINT - jacobian[4] = -x2; jacobian[5] = -x3; jacobian[6] = x0; jacobian[7] = x1; // NOLINT - jacobian[8] = -x3; jacobian[9] = x2; jacobian[10] = -x1; jacobian[11] = x0; // NOLINT + jacobian[0] = -x1; + jacobian[1] = x0; + jacobian[2] = x3; + jacobian[3] = -x2; // NOLINT + jacobian[4] = -x2; + jacobian[5] = -x3; + jacobian[6] = x0; + jacobian[7] = x1; // NOLINT + jacobian[8] = -x3; + jacobian[9] = x2; + jacobian[10] = -x1; + jacobian[11] = x0; // NOLINT /* *INDENT-ON* */ return true; } @@ -238,10 +274,10 @@ class Orientation3DManifold : public fuse_core::Manifold * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); + archive& boost::serialization::base_object(*this); } }; #endif @@ -295,71 +331,102 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit Orientation3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit Orientation3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the quaternion w component */ - double & w() {return data_[W];} + double& w() + { + return data_[W]; + } /** * @brief Read-only access to the quaternion w component */ - const double & w() const {return data_[W];} + const double& w() const + { + return data_[W]; + } /** * @brief Read-write access to the quaternion x component */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the quaternion x component */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the quaternion y component */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the quaternion y component */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-write access to the quaternion z component */ - double & z() {return data_[Z];} + double& z() + { + return data_[Z]; + } /** * @brief Read-only access to the quaternion z component */ - const double & z() const {return data_[Z];} + const double& z() const + { + return data_[Z]; + } /** * @brief Read-only access to quaternion's Euler roll angle component */ - double roll() {return fuse_core::getRoll(w(), x(), y(), z());} + double roll() + { + return fuse_core::getRoll(w(), x(), y(), z()); + } /** * @brief Read-only access to quaternion's Euler pitch angle component */ - double pitch() {return fuse_core::getPitch(w(), x(), y(), z());} + double pitch() + { + return fuse_core::getPitch(w(), x(), y(), z()); + } /** * @brief Read-only access to quaternion's Euler yaw angle component */ - double yaw() {return fuse_core::getYaw(w(), x(), y(), z());} + double yaw() + { + return fuse_core::getYaw(w(), x(), y(), z()); + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Returns the number of elements of the local parameterization space. @@ -367,7 +434,10 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * While a quaternion has 4 parameters, a 3D rotation only has 3 degrees of freedom. Hence, the * local parameterization space is only size 3. */ - size_t localSize() const override {return 3u;} + size_t localSize() const override + { + return 3u; + } /** * @brief Provides a Ceres local parameterization for the quaternion @@ -375,7 +445,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; + fuse_core::LocalParameterization* localParameterization() const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -383,7 +453,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; + fuse_core::Manifold* manifold() const override; #endif private: @@ -397,11 +467,11 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + 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); + archive& boost::serialization::base_object(*this); } }; 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 869d29943..623af71ac 100644 --- a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.hpp @@ -46,7 +46,6 @@ #include #include - namespace fuse_variables { /** @@ -80,39 +79,54 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> * * @param[in] landmark_id The id associated to a landmark */ - explicit Point2DFixedLandmark(const uint64_t & landmark_id); + explicit Point2DFixedLandmark(const uint64_t& landmark_id); /** * @brief Read-write access to the X-axis position. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis position. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis position. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis position. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-only access to the id */ - const uint64_t & id() const {return id_;} + const uint64_t& id() const + { + return id_; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Specifies if the value of the variable should not be changed during optimization @@ -125,13 +139,16 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; - uint64_t id_ {0}; + uint64_t id_{ 0 }; /** * @brief The Boost Serialize method that serializes all of the data members in to/out of the @@ -140,11 +157,11 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & id_; + 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 9f6673654..292d4b372 100644 --- a/fuse_variables/include/fuse_variables/point_2d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_2d_landmark.hpp @@ -46,7 +46,6 @@ #include #include - namespace fuse_variables { /** @@ -80,39 +79,54 @@ class Point2DLandmark : public FixedSizeVariable<2> * * @param[in] landmark_id The id associated to a landmark */ - explicit Point2DLandmark(const uint64_t & landmark_id); + explicit Point2DLandmark(const uint64_t& landmark_id); /** * @brief Read-write access to the X-axis position. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis position. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis position. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis position. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-only access to the id */ - const uint64_t & id() const {return id_;} + const uint64_t& id() const + { + return id_; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -120,13 +134,16 @@ class Point2DLandmark : public FixedSizeVariable<2> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; - uint64_t id_ {0}; + uint64_t id_{ 0 }; /** * @brief The Boost Serialize method that serializes all of the data members in to/out of the @@ -135,11 +152,11 @@ class Point2DLandmark : public FixedSizeVariable<2> * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & id_; + 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 0d78b1959..699eb95fa 100644 --- a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.hpp @@ -46,7 +46,6 @@ #include #include - namespace fuse_variables { /** @@ -81,49 +80,70 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> * * @param[in] landmark_id The id associated to a landmark */ - explicit Point3DFixedLandmark(const uint64_t & landmark_id); + explicit Point3DFixedLandmark(const uint64_t& landmark_id); /** * @brief Read-write access to the X-axis position. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis position. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis position. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis position. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-write access to the Z-axis position. */ - double & z() {return data_[Z];} + double& z() + { + return data_[Z]; + } /** * @brief Read-only access to the Z-axis position. */ - const double & z() const {return data_[Z];} + const double& z() const + { + return data_[Z]; + } /** * @brief Read-only access to the id */ - const uint64_t & id() const {return id_;} + const uint64_t& id() const + { + return id_; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Specifies if the value of the variable should not be changed during optimization @@ -136,13 +156,16 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; - uint64_t id_ {0}; + uint64_t id_{ 0 }; /** * @brief The Boost Serialize method that serializes all of the data members in to/out of the @@ -151,11 +174,11 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & id_; + 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 9b184e699..4cd048e0e 100644 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.hpp +++ b/fuse_variables/include/fuse_variables/point_3d_landmark.hpp @@ -49,7 +49,6 @@ #include #include - namespace fuse_variables { /** @@ -84,49 +83,70 @@ class Point3DLandmark : public FixedSizeVariable<3> * * @param[in] landmark_id The id associated to a landmark */ - explicit Point3DLandmark(const uint64_t & landmark_id); + explicit Point3DLandmark(const uint64_t& landmark_id); /** * @brief Read-write access to the X-axis position. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis position. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis position. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis position. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-write access to the Z-axis position. */ - double & z() {return data_[Z];} + double& z() + { + return data_[Z]; + } /** * @brief Read-only access to the Z-axis position. */ - const double & z() const {return data_[Z];} + const double& z() const + { + return data_[Z]; + } /** * @brief Read-only access to the id */ - const uint64_t & id() const {return id_;} + const uint64_t& id() const + { + return id_; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -134,13 +154,16 @@ class Point3DLandmark : public FixedSizeVariable<3> * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; - uint64_t id_ {0}; + uint64_t id_{ 0 }; /** * @brief The Boost Serialize method that serializes all of the data members in to/out of the @@ -149,11 +172,11 @@ class Point3DLandmark : public FixedSizeVariable<3> * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); - archive & id_; + 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 38496d206..af349ce34 100644 --- a/fuse_variables/include/fuse_variables/position_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_2d_stamped.hpp @@ -88,36 +88,46 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped * robots or devices * */ - explicit Position2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit Position2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis position. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis position. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis position. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis position. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -125,7 +135,10 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -139,11 +152,11 @@ class Position2DStamped : public FixedSizeVariable<2>, public Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + 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); + 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 b2d22e5b8..0e7bfd09a 100644 --- a/fuse_variables/include/fuse_variables/position_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/position_3d_stamped.hpp @@ -88,46 +88,62 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit Position3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit Position3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis position. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis position. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis position. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis position. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-write access to the Z-axis position. */ - double & z() {return data_[Z];} + double& z() + { + return data_[Z]; + } /** * @brief Read-only access to the Z-axis position. */ - const double & z() const {return data_[Z];} + const double& z() const + { + return data_[Z]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -135,7 +151,10 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -149,11 +168,11 @@ class Position3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + 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); + 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 5fcf2a7f1..4f2da197d 100644 --- a/fuse_variables/include/fuse_variables/stamped.hpp +++ b/fuse_variables/include/fuse_variables/stamped.hpp @@ -67,12 +67,10 @@ class Stamped /** * @brief Constructor */ - explicit Stamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL) - : device_id_(device_id), - stamp_(stamp) - {} + explicit Stamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL) + : device_id_(device_id), stamp_(stamp) + { + } /** * @brief Destructor @@ -82,16 +80,22 @@ class Stamped /** * @brief Read-only access to the associated timestamp. */ - const rclcpp::Time & stamp() const {return stamp_;} + const rclcpp::Time& stamp() const + { + return stamp_; + } /** * @brief Read-only access to the associated device ID. */ - const fuse_core::UUID & deviceId() const {return device_id_;} + const fuse_core::UUID& deviceId() const + { + return device_id_; + } private: - 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 + 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 // Allow Boost Serialization access to private methods @@ -104,11 +108,11 @@ class Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & device_id_; - archive & stamp_; + archive& device_id_; + archive& stamp_; } }; @@ -133,8 +137,8 @@ class Stamped * @param[in] interfaces The node interfaces used to load parameters * @return A device UUID */ -fuse_core::UUID loadDeviceId( - fuse_core::node_interfaces::NodeInterfaces interfaces); +fuse_core::UUID +loadDeviceId(fuse_core::node_interfaces::NodeInterfaces interfaces); } // namespace fuse_variables diff --git a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h index 2445fdbc4..7f8ad356d 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__VELOCITY_ANGULAR_2D_STAMPED_H_ #define FUSE_VARIABLES__VELOCITY_ANGULAR_2D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/velocity_angular_2d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/velocity_angular_2d_stamped.hpp instead #include 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 91aa62a2b..1050bb7c1 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.hpp @@ -86,26 +86,30 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit VelocityAngular2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit VelocityAngular2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the angular velocity. */ - double & yaw() {return data_[YAW];} + double& yaw() + { + return data_[YAW]; + } /** * @brief Read-only access to the angular velocity. */ - const double & yaw() const {return data_[YAW];} + const double& yaw() const + { + return data_[YAW]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -113,7 +117,10 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -127,11 +134,11 @@ class VelocityAngular2DStamped : public FixedSizeVariable<1>, public Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + 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); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h index 199deab25..0a5dfe2d8 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__VELOCITY_ANGULAR_3D_STAMPED_H_ #define FUSE_VARIABLES__VELOCITY_ANGULAR_3D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/velocity_angular_3d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/velocity_angular_3d_stamped.hpp instead #include 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 312e58d59..000207d9f 100644 --- a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.hpp @@ -88,46 +88,62 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in] device_id An optional device id, for use when variables originate from multiple * robots or devices */ - explicit VelocityAngular3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit VelocityAngular3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the roll (X-axis) angular velocity. */ - double & roll() {return data_[ROLL];} + double& roll() + { + return data_[ROLL]; + } /** * @brief Read-only access to the roll (X-axis) angular velocity. */ - const double & roll() const {return data_[ROLL];} + const double& roll() const + { + return data_[ROLL]; + } /** * @brief Read-write access to the pitch (Y-axis) angular velocity. */ - double & pitch() {return data_[PITCH];} + double& pitch() + { + return data_[PITCH]; + } /** * @brief Read-only access to the pitch (Y-axis) angular velocity. */ - const double & pitch() const {return data_[PITCH];} + const double& pitch() const + { + return data_[PITCH]; + } /** * @brief Read-write access to the yaw (Z-axis) angular velocity. */ - double & yaw() {return data_[YAW];} + double& yaw() + { + return data_[YAW]; + } /** * @brief Read-only access to the yaw (Z-axis) angular velocity. */ - const double & yaw() const {return data_[YAW];} + const double& yaw() const + { + return data_[YAW]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -135,7 +151,10 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -149,11 +168,11 @@ class VelocityAngular3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + 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); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h index 5641b6f59..04aaf0413 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__VELOCITY_LINEAR_2D_STAMPED_H_ #define FUSE_VARIABLES__VELOCITY_LINEAR_2D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/velocity_linear_2d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/velocity_linear_2d_stamped.hpp instead #include 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 19ab2de72..9565bf33f 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.hpp @@ -88,36 +88,46 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped * robots or devices * */ - explicit VelocityLinear2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id = fuse_core::uuid::NIL); + explicit VelocityLinear2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis linear velocity. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis linear velocity. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis linear velocity. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis linear velocity. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -125,7 +135,10 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -139,11 +152,11 @@ class VelocityLinear2DStamped : public FixedSizeVariable<2>, public Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + 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); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h index d091e4ef6..cc8048e16 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h @@ -35,8 +35,7 @@ #ifndef FUSE_VARIABLES__VELOCITY_LINEAR_3D_STAMPED_H_ #define FUSE_VARIABLES__VELOCITY_LINEAR_3D_STAMPED_H_ -#warning \ - This header is obsolete, please include fuse_variables/velocity_linear_3d_stamped.hpp instead +#warning This header is obsolete, please include fuse_variables/velocity_linear_3d_stamped.hpp instead #include 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 a86c0e1cb..9c0c8b35b 100644 --- a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp +++ b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.hpp @@ -89,46 +89,62 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped * robots or devices * */ - explicit VelocityLinear3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_ids = fuse_core::uuid::NIL); + explicit VelocityLinear3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_ids = fuse_core::uuid::NIL); /** * @brief Read-write access to the X-axis linear velocity. */ - double & x() {return data_[X];} + double& x() + { + return data_[X]; + } /** * @brief Read-only access to the X-axis linear velocity. */ - const double & x() const {return data_[X];} + const double& x() const + { + return data_[X]; + } /** * @brief Read-write access to the Y-axis linear velocity. */ - double & y() {return data_[Y];} + double& y() + { + return data_[Y]; + } /** * @brief Read-only access to the Y-axis linear velocity. */ - const double & y() const {return data_[Y];} + const double& y() const + { + return data_[Y]; + } /** * @brief Read-write access to the Z-axis linear velocity. */ - double & z() {return data_[Z];} + double& z() + { + return data_[Z]; + } /** * @brief Read-only access to the Z-axis linear velocity. */ - const double & z() const {return data_[Z];} + const double& z() const + { + return data_[Z]; + } /** * @brief Print a human-readable description of the variable to the provided stream. * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; #if CERES_SUPPORTS_MANIFOLDS /** @@ -136,7 +152,10 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped * * Overriding the manifold() method prevents additional processing with the ManifoldAdapter */ - fuse_core::Manifold * manifold() const override {return nullptr;} + fuse_core::Manifold* manifold() const override + { + return nullptr; + } #endif private: @@ -150,11 +169,11 @@ class VelocityLinear3DStamped : public FixedSizeVariable<3>, public Stamped * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + 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); + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/package.xml b/fuse_variables/package.xml index 5430ba0bc..adba511ab 100644 --- a/fuse_variables/package.xml +++ b/fuse_variables/package.xml @@ -27,8 +27,6 @@ ament_cmake_gtest ament_cmake_pytest - ament_lint_auto - ament_lint_common rclcpp diff --git a/fuse_variables/src/acceleration_angular_2d_stamped.cpp b/fuse_variables/src/acceleration_angular_2d_stamped.cpp index 55d0b54c0..2cdb40ed6 100644 --- a/fuse_variables/src/acceleration_angular_2d_stamped.cpp +++ b/fuse_variables/src/acceleration_angular_2d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -AccelerationAngular2DStamped::AccelerationAngular2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable<1>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +AccelerationAngular2DStamped::AccelerationAngular2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable<1>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void AccelerationAngular2DStamped::print(std::ostream & stream) const +void AccelerationAngular2DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/acceleration_angular_3d_stamped.cpp b/fuse_variables/src/acceleration_angular_3d_stamped.cpp index 1e6f4ac9b..9e4f35401 100644 --- a/fuse_variables/src/acceleration_angular_3d_stamped.cpp +++ b/fuse_variables/src/acceleration_angular_3d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -AccelerationAngular3DStamped::AccelerationAngular3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable<3>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +AccelerationAngular3DStamped::AccelerationAngular3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable<3>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void AccelerationAngular3DStamped::print(std::ostream & stream) const +void AccelerationAngular3DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/acceleration_linear_2d_stamped.cpp b/fuse_variables/src/acceleration_linear_2d_stamped.cpp index 73a0ddad8..47d2c2ca2 100644 --- a/fuse_variables/src/acceleration_linear_2d_stamped.cpp +++ b/fuse_variables/src/acceleration_linear_2d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -AccelerationLinear2DStamped::AccelerationLinear2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +AccelerationLinear2DStamped::AccelerationLinear2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void AccelerationLinear2DStamped::print(std::ostream & stream) const +void AccelerationLinear2DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/acceleration_linear_3d_stamped.cpp b/fuse_variables/src/acceleration_linear_3d_stamped.cpp index df924541a..5018a0742 100644 --- a/fuse_variables/src/acceleration_linear_3d_stamped.cpp +++ b/fuse_variables/src/acceleration_linear_3d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -AccelerationLinear3DStamped::AccelerationLinear3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +AccelerationLinear3DStamped::AccelerationLinear3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void AccelerationLinear3DStamped::print(std::ostream & stream) const +void AccelerationLinear3DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/orientation_2d_stamped.cpp b/fuse_variables/src/orientation_2d_stamped.cpp index 3897d6f97..7b2f2d7a0 100644 --- a/fuse_variables/src/orientation_2d_stamped.cpp +++ b/fuse_variables/src/orientation_2d_stamped.cpp @@ -46,15 +46,12 @@ namespace fuse_variables { -Orientation2DStamped::Orientation2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +Orientation2DStamped::Orientation2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void Orientation2DStamped::print(std::ostream & stream) const +void Orientation2DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" @@ -65,13 +62,13 @@ void Orientation2DStamped::print(std::ostream & stream) const << " - yaw: " << yaw() << "\n"; } -fuse_core::LocalParameterization * Orientation2DStamped::localParameterization() const +fuse_core::LocalParameterization* Orientation2DStamped::localParameterization() const { return new Orientation2DLocalParameterization(); } #if CERES_SUPPORTS_MANIFOLDS -fuse_core::Manifold * Orientation2DStamped::manifold() const +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 4eb816a95..b72f73a19 100644 --- a/fuse_variables/src/orientation_3d_stamped.cpp +++ b/fuse_variables/src/orientation_3d_stamped.cpp @@ -46,15 +46,12 @@ namespace fuse_variables { -Orientation3DStamped::Orientation3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable<4>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +Orientation3DStamped::Orientation3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable<4>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void Orientation3DStamped::print(std::ostream & stream) const +void Orientation3DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" @@ -68,13 +65,13 @@ void Orientation3DStamped::print(std::ostream & stream) const << " - z: " << z() << "\n"; } -fuse_core::LocalParameterization * Orientation3DStamped::localParameterization() const +fuse_core::LocalParameterization* Orientation3DStamped::localParameterization() const { return new Orientation3DLocalParameterization(); } #if CERES_SUPPORTS_MANIFOLDS -fuse_core::Manifold * Orientation3DStamped::manifold() const +fuse_core::Manifold* Orientation3DStamped::manifold() const { return new Orientation3DManifold(); } diff --git a/fuse_variables/src/point_2d_fixed_landmark.cpp b/fuse_variables/src/point_2d_fixed_landmark.cpp index 48892c861..88886eb6f 100644 --- a/fuse_variables/src/point_2d_fixed_landmark.cpp +++ b/fuse_variables/src/point_2d_fixed_landmark.cpp @@ -42,13 +42,12 @@ namespace fuse_variables { -Point2DFixedLandmark::Point2DFixedLandmark(const uint64_t & landmark_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), - id_(landmark_id) +Point2DFixedLandmark::Point2DFixedLandmark(const uint64_t& landmark_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), id_(landmark_id) { } -void Point2DFixedLandmark::print(std::ostream & stream) const +void Point2DFixedLandmark::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/point_2d_landmark.cpp b/fuse_variables/src/point_2d_landmark.cpp index cbbeaedb1..aede91f22 100644 --- a/fuse_variables/src/point_2d_landmark.cpp +++ b/fuse_variables/src/point_2d_landmark.cpp @@ -42,13 +42,12 @@ namespace fuse_variables { -Point2DLandmark::Point2DLandmark(const uint64_t & landmark_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), - id_(landmark_id) +Point2DLandmark::Point2DLandmark(const uint64_t& landmark_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), id_(landmark_id) { } -void Point2DLandmark::print(std::ostream & stream) const +void Point2DLandmark::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/point_3d_fixed_landmark.cpp b/fuse_variables/src/point_3d_fixed_landmark.cpp index 52ae39680..47f553761 100644 --- a/fuse_variables/src/point_3d_fixed_landmark.cpp +++ b/fuse_variables/src/point_3d_fixed_landmark.cpp @@ -42,13 +42,12 @@ namespace fuse_variables { -Point3DFixedLandmark::Point3DFixedLandmark(const uint64_t & landmark_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), - id_(landmark_id) +Point3DFixedLandmark::Point3DFixedLandmark(const uint64_t& landmark_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), id_(landmark_id) { } -void Point3DFixedLandmark::print(std::ostream & stream) const +void Point3DFixedLandmark::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/point_3d_landmark.cpp b/fuse_variables/src/point_3d_landmark.cpp index 20e9ba840..0367b7920 100644 --- a/fuse_variables/src/point_3d_landmark.cpp +++ b/fuse_variables/src/point_3d_landmark.cpp @@ -42,13 +42,12 @@ namespace fuse_variables { -Point3DLandmark::Point3DLandmark(const uint64_t & landmark_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), - id_(landmark_id) +Point3DLandmark::Point3DLandmark(const uint64_t& landmark_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), id_(landmark_id) { } -void Point3DLandmark::print(std::ostream & stream) const +void Point3DLandmark::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/position_2d_stamped.cpp b/fuse_variables/src/position_2d_stamped.cpp index 9f5f7130e..3e8675a4c 100644 --- a/fuse_variables/src/position_2d_stamped.cpp +++ b/fuse_variables/src/position_2d_stamped.cpp @@ -44,13 +44,12 @@ namespace fuse_variables { -Position2DStamped::Position2DStamped(const rclcpp::Time & stamp, const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +Position2DStamped::Position2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void Position2DStamped::print(std::ostream & stream) const +void Position2DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/position_3d_stamped.cpp b/fuse_variables/src/position_3d_stamped.cpp index ae2689972..26e23aa42 100644 --- a/fuse_variables/src/position_3d_stamped.cpp +++ b/fuse_variables/src/position_3d_stamped.cpp @@ -44,13 +44,12 @@ namespace fuse_variables { -Position3DStamped::Position3DStamped(const rclcpp::Time & stamp, const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +Position3DStamped::Position3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void Position3DStamped::print(std::ostream & stream) const +void Position3DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/stamped.cpp b/fuse_variables/src/stamped.cpp index 87d5cd1a4..b5f8e4dd3 100644 --- a/fuse_variables/src/stamped.cpp +++ b/fuse_variables/src/stamped.cpp @@ -41,18 +41,20 @@ namespace fuse_variables { -fuse_core::UUID loadDeviceId( - fuse_core::node_interfaces::NodeInterfaces interfaces) +fuse_core::UUID +loadDeviceId(fuse_core::node_interfaces::NodeInterfaces interfaces) { std::string device_str; device_str = fuse_core::getParam(interfaces, "device_id", std::string()); - if (!device_str.empty()) { + if (!device_str.empty()) + { return fuse_core::uuid::from_string(device_str); } device_str = fuse_core::getParam(interfaces, "device_name", std::string()); - if (!device_str.empty()) { + if (!device_str.empty()) + { return fuse_core::uuid::generate(device_str); } diff --git a/fuse_variables/src/velocity_angular_2d_stamped.cpp b/fuse_variables/src/velocity_angular_2d_stamped.cpp index 9f91b35c7..d3a0a1f4f 100644 --- a/fuse_variables/src/velocity_angular_2d_stamped.cpp +++ b/fuse_variables/src/velocity_angular_2d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -VelocityAngular2DStamped::VelocityAngular2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +VelocityAngular2DStamped::VelocityAngular2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void VelocityAngular2DStamped::print(std::ostream & stream) const +void VelocityAngular2DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/velocity_angular_3d_stamped.cpp b/fuse_variables/src/velocity_angular_3d_stamped.cpp index 743aed502..25e8d3538 100644 --- a/fuse_variables/src/velocity_angular_3d_stamped.cpp +++ b/fuse_variables/src/velocity_angular_3d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -VelocityAngular3DStamped::VelocityAngular3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +VelocityAngular3DStamped::VelocityAngular3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void VelocityAngular3DStamped::print(std::ostream & stream) const +void VelocityAngular3DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/velocity_linear_2d_stamped.cpp b/fuse_variables/src/velocity_linear_2d_stamped.cpp index ae8fc35af..d6db6c014 100644 --- a/fuse_variables/src/velocity_linear_2d_stamped.cpp +++ b/fuse_variables/src/velocity_linear_2d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -VelocityLinear2DStamped::VelocityLinear2DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +VelocityLinear2DStamped::VelocityLinear2DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void VelocityLinear2DStamped::print(std::ostream & stream) const +void VelocityLinear2DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/src/velocity_linear_3d_stamped.cpp b/fuse_variables/src/velocity_linear_3d_stamped.cpp index e919b3380..1bb1d42d0 100644 --- a/fuse_variables/src/velocity_linear_3d_stamped.cpp +++ b/fuse_variables/src/velocity_linear_3d_stamped.cpp @@ -44,15 +44,12 @@ namespace fuse_variables { -VelocityLinear3DStamped::VelocityLinear3DStamped( - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id) -: FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), - Stamped(stamp, device_id) +VelocityLinear3DStamped::VelocityLinear3DStamped(const rclcpp::Time& stamp, const fuse_core::UUID& device_id) + : FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), Stamped(stamp, device_id) { } -void VelocityLinear3DStamped::print(std::ostream & stream) const +void VelocityLinear3DStamped::print(std::ostream& stream) const { stream << type() << ":\n" << " uuid: " << uuid() << "\n" diff --git a/fuse_variables/test/CMakeLists.txt b/fuse_variables/test/CMakeLists.txt index 3b425e309..7bdde53cb 100644 --- a/fuse_variables/test/CMakeLists.txt +++ b/fuse_variables/test/CMakeLists.txt @@ -1,24 +1,24 @@ -# CORE GTESTS ====================================================================================== +# CORE GTESTS +# ====================================================================================== set(TEST_TARGETS - test_acceleration_angular_2d_stamped - test_acceleration_angular_3d_stamped - test_acceleration_linear_2d_stamped - test_acceleration_linear_3d_stamped - test_fixed_size_variable - test_orientation_2d_stamped - test_orientation_3d_stamped - test_point_2d_fixed_landmark - test_point_2d_landmark - test_point_3d_fixed_landmark - test_point_3d_landmark - test_position_2d_stamped - test_position_3d_stamped - test_load_device_id - test_velocity_angular_2d_stamped - test_velocity_angular_3d_stamped - test_velocity_linear_2d_stamped - test_velocity_linear_3d_stamped -) + test_acceleration_angular_2d_stamped + test_acceleration_angular_3d_stamped + test_acceleration_linear_2d_stamped + test_acceleration_linear_3d_stamped + test_fixed_size_variable + test_orientation_2d_stamped + test_orientation_3d_stamped + test_point_2d_fixed_landmark + test_point_2d_landmark + test_point_3d_fixed_landmark + test_point_3d_landmark + test_position_2d_stamped + test_position_3d_stamped + test_load_device_id + test_velocity_angular_2d_stamped + test_velocity_angular_3d_stamped + test_velocity_linear_2d_stamped + test_velocity_linear_3d_stamped) foreach(test_name ${TEST_TARGETS}) ament_add_gtest("${test_name}" "${test_name}.cpp") diff --git a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp index c6f527ee8..f595a98f8 100644 --- a/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_angular_2d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::AccelerationAngular2DStamped; - TEST(AccelerationAngular2DStamped, Type) { AccelerationAngular2DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(AccelerationAngular2DStamped, UUID) AccelerationAngular2DStamped 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 variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ 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 variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + AccelerationAngular2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationAngular2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = AccelerationAngular2DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr 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)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(AccelerationAngular2DStamped, Stamped) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(2.7); return true; @@ -117,23 +115,18 @@ struct CostFunctor TEST(AccelerationAngular2DStamped, Optimization) { // Create a AccelerationAngular2DStamped - AccelerationAngular2DStamped acceleration( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationAngular2DStamped acceleration(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); acceleration.yaw() = 1.5; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(acceleration.data(), acceleration.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); - problem.AddResidualBlock( - cost_function, - nullptr, - parameter_blocks); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -147,8 +140,7 @@ TEST(AccelerationAngular2DStamped, Optimization) TEST(AccelerationAngular2DStamped, Serialization) { // Create a AccelerationAngular2DStamped - AccelerationAngular2DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationAngular2DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.yaw() = 1.5; // Serialize the variable into an archive diff --git a/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp b/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp index 39d416fc9..3f6a40061 100644 --- a/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_angular_3d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::AccelerationAngular3DStamped; - TEST(AccelerationAngular3DStamped, Type) { AccelerationAngular3DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(AccelerationAngular3DStamped, UUID) AccelerationAngular3DStamped 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 variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationAngular3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +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 variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + AccelerationAngular3DStamped 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 = AccelerationAngular3DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr 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)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(AccelerationAngular3DStamped, Stamped) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); @@ -119,20 +117,18 @@ struct CostFunctor TEST(AccelerationAngular3DStamped, Optimization) { // Create a AccelerationAngular3DStamped - AccelerationAngular3DStamped acceleration( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationAngular3DStamped acceleration(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); acceleration.roll() = 1.5; acceleration.pitch() = -3.0; acceleration.yaw() = 14.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(acceleration.data(), acceleration.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -150,8 +146,7 @@ TEST(AccelerationAngular3DStamped, Optimization) TEST(AccelerationAngular3DStamped, Serialization) { // Create a AccelerationAngular3DStamped - AccelerationAngular3DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationAngular3DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.roll() = 1.5; expected.pitch() = -3.0; expected.yaw() = 14.0; diff --git a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp index a1b4212e8..849a273fd 100644 --- a/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_linear_2d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::AccelerationLinear2DStamped; - TEST(AccelerationLinear2DStamped, Type) { AccelerationLinear2DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(AccelerationLinear2DStamped, UUID) AccelerationLinear2DStamped 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 variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ 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 variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + AccelerationLinear2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(AccelerationLinear2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = AccelerationLinear2DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr 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)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(AccelerationLinear2DStamped, Stamped) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); @@ -118,19 +116,17 @@ struct CostFunctor TEST(AccelerationLinear2DStamped, Optimization) { // Create a AccelerationLinear2DStamped - AccelerationLinear2DStamped acceleration( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationLinear2DStamped acceleration(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); acceleration.x() = 1.5; acceleration.y() = -3.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(acceleration.data(), acceleration.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -147,8 +143,7 @@ TEST(AccelerationLinear2DStamped, Optimization) TEST(AccelerationLinear2DStamped, Serialization) { // Create a AccelerationLinear2DStamped - AccelerationLinear2DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationLinear2DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.x() = 1.5; expected.y() = -3.0; diff --git a/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp b/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp index 83ac4093c..a67b3e8b5 100644 --- a/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp +++ b/fuse_variables/test/test_acceleration_linear_3d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::AccelerationLinear3DStamped; - TEST(AccelerationLinear3DStamped, Type) { AccelerationLinear3DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(AccelerationLinear3DStamped, UUID) AccelerationLinear3DStamped 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 variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + AccelerationLinear3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ 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 variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + AccelerationLinear3DStamped 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 = AccelerationLinear3DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr 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)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(AccelerationLinear3DStamped, Stamped) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); @@ -119,20 +117,18 @@ struct CostFunctor TEST(AccelerationLinear3DStamped, Optimization) { // Create a AccelerationLinear3DStamped - AccelerationLinear3DStamped acceleration( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationLinear3DStamped acceleration(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); acceleration.x() = 1.5; acceleration.y() = -3.0; acceleration.z() = 14.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(acceleration.data(), acceleration.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(acceleration.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -150,8 +146,7 @@ TEST(AccelerationLinear3DStamped, Optimization) TEST(AccelerationLinear3DStamped, Serialization) { // Create a AccelerationLinear3DStamped - AccelerationLinear3DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + AccelerationLinear3DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.x() = 1.5; expected.y() = -3.0; expected.z() = 14.0; diff --git a/fuse_variables/test/test_fixed_size_variable.cpp b/fuse_variables/test/test_fixed_size_variable.cpp index 2ea971938..239f702d3 100644 --- a/fuse_variables/test/test_fixed_size_variable.cpp +++ b/fuse_variables/test/test_fixed_size_variable.cpp @@ -44,12 +44,14 @@ class TestVariable : public fuse_variables::FixedSizeVariable<2> public: FUSE_VARIABLE_DEFINITIONS(TestVariable) - TestVariable() - : fuse_variables::FixedSizeVariable<2>(fuse_core::uuid::generate()) - {} + TestVariable() : fuse_variables::FixedSizeVariable<2>(fuse_core::uuid::generate()) + { + } virtual ~TestVariable() = default; - void print(std::ostream & /*stream = std::cout*/) const override {} + void print(std::ostream& /*stream = std::cout*/) const override + { + } private: // Allow Boost Serialization access to private methods @@ -62,19 +64,18 @@ class TestVariable : public fuse_variables::FixedSizeVariable<2> * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object>(*this); + archive& boost::serialization::base_object>(*this); } }; - TEST(FixedSizeVariable, Size) { // Verify the expected size is returned TestVariable variable; - EXPECT_EQ(2u, variable.size()); // base class interface + EXPECT_EQ(2u, variable.size()); // base class interface EXPECT_EQ(2u, TestVariable::SIZE); // static member variable } @@ -84,7 +85,7 @@ TEST(FixedSizeVariable, Data) TestVariable variable; EXPECT_NO_THROW(variable.data()[0] = 1.0); EXPECT_NO_THROW(variable.data()[1] = 2.0); - const TestVariable & const_variable = variable; + const TestVariable& const_variable = variable; bool success = true; EXPECT_NO_THROW(success = success && const_variable.data()[0] == 1.0); EXPECT_NO_THROW(success = success && const_variable.data()[1] == 2.0); @@ -99,7 +100,7 @@ TEST(FixedSizeVariable, Array) EXPECT_NO_THROW(variable.array().at(1) = 2.0); EXPECT_NO_THROW(variable.array().front() = 3.0); EXPECT_NO_THROW(variable.array().back() = 4.0); - const TestVariable & const_variable = variable; + const TestVariable& const_variable = variable; bool success = true; EXPECT_NO_THROW(success = success && const_variable.array()[0] == 3.0); EXPECT_NO_THROW(success = success && const_variable.array().at(1) == 4.0); diff --git a/fuse_variables/test/test_load_device_id.cpp b/fuse_variables/test/test_load_device_id.cpp index 4b8ab693d..64c084248 100644 --- a/fuse_variables/test/test_load_device_id.cpp +++ b/fuse_variables/test/test_load_device_id.cpp @@ -47,16 +47,14 @@ class TestLoadDeviceId : public ::testing::Test void SetUp() override { executor_ = std::make_shared(); - spinner_ = std::thread( - [&]() { - executor_->spin(); - }); + spinner_ = std::thread([&]() { executor_->spin(); }); } void TearDown() override { executor_->cancel(); - if (spinner_.joinable()) { + if (spinner_.joinable()) + { spinner_.join(); } executor_.reset(); @@ -73,84 +71,48 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) auto node = rclcpp::Node::make_shared("id1_node"); node->declare_parameter("device_id", std::string("01234567-89AB-CDEF-0123-456789ABCDEF")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x01, 0x23, 0x45, 0x67, - 0x89, 0xAB, - 0xCD, 0xEF, - 0x01, 0x23, - 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF - }; + fuse_core::UUID expected = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF }; EXPECT_EQ(expected, actual); } { auto node = rclcpp::Node::make_shared("id2_node"); node->declare_parameter("device_id", std::string("01234567-89ab-cdef-0123-456789abcdef")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x01, 0x23, 0x45, 0x67, - 0x89, 0xAB, - 0xCD, 0xEF, - 0x01, 0x23, - 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF - }; + fuse_core::UUID expected = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF }; EXPECT_EQ(expected, actual); } { auto node = rclcpp::Node::make_shared("id3_node"); node->declare_parameter("device_id", std::string("0123456789ABCDEF0123456789ABCDEF")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x01, 0x23, 0x45, 0x67, - 0x89, 0xAB, - 0xCD, 0xEF, - 0x01, 0x23, - 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF - }; + fuse_core::UUID expected = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF }; EXPECT_EQ(expected, actual); } { auto node = rclcpp::Node::make_shared("id4_node"); node->declare_parameter("device_id", std::string("0123456789abcdef0123456789abcdef")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x01, 0x23, 0x45, 0x67, - 0x89, 0xAB, - 0xCD, 0xEF, - 0x01, 0x23, - 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF - }; + fuse_core::UUID expected = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF }; EXPECT_EQ(expected, actual); } { auto node = rclcpp::Node::make_shared("id5_node"); node->declare_parameter("device_id", std::string("{01234567-89ab-cdef-0123-456789abcdef}")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x01, 0x23, 0x45, 0x67, - 0x89, 0xAB, - 0xCD, 0xEF, - 0x01, 0x23, - 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF - }; + fuse_core::UUID expected = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF }; EXPECT_EQ(expected, actual); } { auto node = rclcpp::Node::make_shared("id6_node"); node->declare_parameter("device_id", std::string("{01234567-89AB-CDEF-0123-456789ABCDEF}")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x01, 0x23, 0x45, 0x67, - 0x89, 0xAB, - 0xCD, 0xEF, - 0x01, 0x23, - 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF - }; + fuse_core::UUID expected = { 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF, + 0x01, 0x23, 0x45, 0x67, 0x89, 0xAB, 0xCD, 0xEF }; EXPECT_EQ(expected, actual); } { @@ -162,14 +124,8 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) auto node = rclcpp::Node::make_shared("name_node"); node->declare_parameter("device_name", std::string("Test")); fuse_core::UUID actual = fuse_variables::loadDeviceId(*node); - fuse_core::UUID expected = - { - 0x5B, 0x23, 0x43, 0x6D, - 0x8E, 0x7C, - 0x51, 0xCF, - 0x81, 0x62, - 0x5C, 0xD5, 0xFD, 0x37, 0x9E, 0xCF - }; + fuse_core::UUID expected = { 0x5B, 0x23, 0x43, 0x6D, 0x8E, 0x7C, 0x51, 0xCF, + 0x81, 0x62, 0x5C, 0xD5, 0xFD, 0x37, 0x9E, 0xCF }; EXPECT_EQ(expected, actual); } { @@ -181,9 +137,8 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) } } - // NOTE(CH3): This main is required because the test is manually run by a launch test -int main(int argc, char ** argv) +int main(int argc, char** argv) { rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); diff --git a/fuse_variables/test/test_orientation_2d_stamped.cpp b/fuse_variables/test/test_orientation_2d_stamped.cpp index 8e17d43bc..a20ff82f0 100644 --- a/fuse_variables/test/test_orientation_2d_stamped.cpp +++ b/fuse_variables/test/test_orientation_2d_stamped.cpp @@ -64,10 +64,8 @@ TEST(Orientation2DStamped, UUID) Orientation2DStamped 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 variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Orientation2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -83,18 +81,16 @@ 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 variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + Orientation2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(Orientation2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = Orientation2DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr 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)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -108,8 +104,8 @@ TEST(Orientation2DStamped, Stamped) struct Orientation2DPlus { - template - bool operator()(const T * x, const T * delta, T * x_plus_delta) const + template + bool operator()(const T* x, const T* delta, T* x_plus_delta) const { x_plus_delta[0] = fuse_core::wrapAngle2D(x[0] + delta[0]); return true; @@ -118,8 +114,8 @@ struct Orientation2DPlus struct Orientation2DMinus { - template - bool operator()(const T * x, const T * y, T * y_minus_x) const + template + bool operator()(const T* x, const T* y, T* y_minus_x) const { y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); return true; @@ -127,7 +123,7 @@ struct Orientation2DMinus }; using Orientation2DLocalParameterization = - fuse_core::AutoDiffLocalParameterization; + fuse_core::AutoDiffLocalParameterization; TEST(Orientation2DStamped, Plus) { @@ -135,9 +131,9 @@ TEST(Orientation2DStamped, Plus) // Simple test { - double x[1] = {1.0}; - double delta[1] = {0.5}; - double actual[1] = {0.0}; + double x[1] = { 1.0 }; + double delta[1] = { 0.5 }; + double actual[1] = { 0.0 }; bool success = parameterization->Plus(x, delta, actual); EXPECT_TRUE(success); @@ -146,9 +142,9 @@ TEST(Orientation2DStamped, Plus) // Check roll-over { - double x[1] = {2.0}; - double delta[1] = {3.0}; - double actual[1] = {0.0}; + double x[1] = { 2.0 }; + double delta[1] = { 3.0 }; + double actual[1] = { 0.0 }; bool success = parameterization->Plus(x, delta, actual); EXPECT_TRUE(success); @@ -163,13 +159,14 @@ TEST(Orientation2DStamped, PlusJacobian) 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}; - for (auto test_value : test_values) { - double x[1] = {test_value}; - double actual[1] = {0.0}; + auto test_values = std::vector{ -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; + for (auto test_value : test_values) + { + double x[1] = { test_value }; + double actual[1] = { 0.0 }; bool success = parameterization->ComputeJacobian(x, actual); - double expected[1] = {0.0}; + double expected[1] = { 0.0 }; reference.ComputeJacobian(x, expected); EXPECT_TRUE(success); @@ -185,9 +182,9 @@ TEST(Orientation2DStamped, Minus) // Simple test { - double x1[1] = {1.0}; - double x2[1] = {1.5}; - double actual[1] = {0.0}; + double x1[1] = { 1.0 }; + double x2[1] = { 1.5 }; + double actual[1] = { 0.0 }; bool success = parameterization->Minus(x1, x2, actual); EXPECT_TRUE(success); @@ -196,9 +193,9 @@ TEST(Orientation2DStamped, Minus) // Check roll-over { - double x1[1] = {2.0}; - double x2[1] = {5 - 2 * M_PI}; - double actual[1] = {0.0}; + double x1[1] = { 2.0 }; + double x2[1] = { 5 - 2 * M_PI }; + double actual[1] = { 0.0 }; bool success = parameterization->Minus(x1, x2, actual); EXPECT_TRUE(success); @@ -211,13 +208,14 @@ TEST(Orientation2DStamped, MinusJacobian) 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}; - for (auto test_value : test_values) { - double x[1] = {test_value}; - double actual[1] = {0.0}; + auto test_values = std::vector{ -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; + for (auto test_value : test_values) + { + double x[1] = { test_value }; + double actual[1] = { 0.0 }; bool success = parameterization->ComputeMinusJacobian(x, actual); - double expected[1] = {0.0}; + double expected[1] = { 0.0 }; reference.ComputeMinusJacobian(x, expected); EXPECT_TRUE(success); @@ -229,9 +227,12 @@ TEST(Orientation2DStamped, MinusJacobian) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); return true; @@ -241,28 +242,20 @@ struct CostFunctor TEST(Orientation2DStamped, Optimization) { // Create a Orientation2DStamped - Orientation2DStamped orientation( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + Orientation2DStamped orientation(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); orientation.yaw() = 1.5; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; #if !CERES_SUPPORTS_MANIFOLDS - problem.AddParameterBlock( - orientation.data(), - orientation.size(), - orientation.localParameterization()); + problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.localParameterization()); #else - problem.AddParameterBlock( - orientation.data(), - orientation.size(), - orientation.manifold()); + problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -278,8 +271,7 @@ TEST(Orientation2DStamped, Optimization) TEST(Orientation2DStamped, Serialization) { // Create a Orientation2DStamped - Orientation2DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + Orientation2DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.yaw() = 1.5; // Serialize the variable into an archive @@ -307,15 +299,15 @@ TEST(Orientation2DStamped, Serialization) struct Orientation2DFunctor { - template - bool Plus(const T * x, const T * delta, T * x_plus_delta) const + template + bool Plus(const T* x, const T* delta, T* x_plus_delta) const { x_plus_delta[0] = fuse_core::wrapAngle2D(x[0] + delta[0]); return true; } - template - bool Minus(const T * y, const T * x, T * y_minus_x) const + template + bool Minus(const T* y, const T* x, T* y_minus_x) const { y_minus_x[0] = fuse_core::wrapAngle2D(y[0] - x[0]); return true; @@ -330,9 +322,9 @@ TEST(Orientation2DStamped, ManifoldPlus) // Simple test { - double x[1] = {1.0}; - double delta[1] = {0.5}; - double actual[1] = {0.0}; + double x[1] = { 1.0 }; + double delta[1] = { 0.5 }; + double actual[1] = { 0.0 }; bool success = manifold->Plus(x, delta, actual); EXPECT_TRUE(success); @@ -341,9 +333,9 @@ TEST(Orientation2DStamped, ManifoldPlus) // Check roll-over { - double x[1] = {2.0}; - double delta[1] = {3.0}; - double actual[1] = {0.0}; + double x[1] = { 2.0 }; + double delta[1] = { 3.0 }; + double actual[1] = { 0.0 }; bool success = manifold->Plus(x, delta, actual); EXPECT_TRUE(success); @@ -358,13 +350,14 @@ TEST(Orientation2DStamped, ManifoldPlusJacobian) 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}; - for (auto test_value : test_values) { - double x[1] = {test_value}; - double actual[1] = {0.0}; + auto test_values = std::vector{ -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; + for (auto test_value : test_values) + { + double x[1] = { test_value }; + double actual[1] = { 0.0 }; bool success = manifold->PlusJacobian(x, actual); - double expected[1] = {0.0}; + double expected[1] = { 0.0 }; reference.PlusJacobian(x, expected); EXPECT_TRUE(success); @@ -380,9 +373,9 @@ TEST(Orientation2DStamped, ManifoldMinus) // Simple test { - double x1[1] = {1.0}; - double x2[1] = {1.5}; - double actual[1] = {0.0}; + double x1[1] = { 1.0 }; + double x2[1] = { 1.5 }; + double actual[1] = { 0.0 }; bool success = manifold->Minus(x2, x1, actual); EXPECT_TRUE(success); @@ -391,9 +384,9 @@ TEST(Orientation2DStamped, ManifoldMinus) // Check roll-over { - double x1[1] = {2.0}; - double x2[1] = {5 - 2 * M_PI}; - double actual[1] = {0.0}; + double x1[1] = { 2.0 }; + double x2[1] = { 5 - 2 * M_PI }; + double actual[1] = { 0.0 }; bool success = manifold->Minus(x2, x1, actual); EXPECT_TRUE(success); @@ -406,13 +399,14 @@ TEST(Orientation2DStamped, ManifoldMinusJacobian) 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}; - for (auto test_value : test_values) { - double x[1] = {test_value}; - double actual[1] = {0.0}; + auto test_values = std::vector{ -2 * M_PI, -1 * M_PI, -1.0, 0.0, 1.0, M_PI, 2 * M_PI }; + for (auto test_value : test_values) + { + double x[1] = { test_value }; + double actual[1] = { 0.0 }; bool success = manifold->MinusJacobian(x, actual); - double expected[1] = {0.0}; + double expected[1] = { 0.0 }; reference.MinusJacobian(x, expected); EXPECT_TRUE(success); diff --git a/fuse_variables/test/test_orientation_3d_stamped.cpp b/fuse_variables/test/test_orientation_3d_stamped.cpp index 10e04d0b8..c50926b28 100644 --- a/fuse_variables/test/test_orientation_3d_stamped.cpp +++ b/fuse_variables/test/test_orientation_3d_stamped.cpp @@ -110,7 +110,7 @@ TEST(Orientation3DStamped, UUID) } } -template +template inline static void QuaternionInverse(const T in[4], T out[4]) { out[0] = in[0]; @@ -121,8 +121,8 @@ inline static void QuaternionInverse(const T in[4], T out[4]) struct Orientation3DPlus { - template - bool operator()(const T * x, const T * delta, T * x_plus_delta) const + template + bool operator()(const T* x, const T* delta, T* x_plus_delta) const { T q_delta[4]; ceres::AngleAxisToQuaternion(delta, q_delta); @@ -133,8 +133,8 @@ struct Orientation3DPlus struct Orientation3DMinus { - template - bool operator()(const T * x, const T * y, T * y_minus_x) const + template + bool operator()(const T* x, const T* y, T* y_minus_x) const { T x_inverse[4]; QuaternionInverse(x, x_inverse); @@ -146,15 +146,15 @@ struct Orientation3DMinus }; using Orientation3DLocalParameterization = - fuse_core::AutoDiffLocalParameterization; + fuse_core::AutoDiffLocalParameterization; TEST(Orientation3DStamped, Plus) { 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}; + 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); EXPECT_TRUE(success); @@ -170,9 +170,9 @@ TEST(Orientation3DStamped, Minus) { 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}; + 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); EXPECT_TRUE(success); @@ -188,39 +188,35 @@ TEST(Orientation3DStamped, PlusJacobian) auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation3DLocalParameterization(); - 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) { + 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); - double x[4] = {qw, qx, qy, 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; + 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()); fuse_core::MatrixXd expected(4, 3); /* *INDENT-OFF* */ - expected << 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0; + expected << 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* */ reference.ComputeJacobian(x, expected.data()); EXPECT_TRUE(success); Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); - EXPECT_TRUE( - expected.isApprox( - actual, - 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" - << "Actual is:\n" << actual.format(clean) << "\n" - << "Difference is:\n" << (expected - actual).format(clean) - << "\n"; + EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" + << expected.format(clean) << "\n" + << "Actual is:\n" + << actual.format(clean) << "\n" + << "Difference is:\n" + << (expected - actual).format(clean) << "\n"; } } } @@ -233,39 +229,35 @@ TEST(Orientation3DStamped, MinusJacobian) auto parameterization = Orientation3DStamped(rclcpp::Time(0, 0)).localParameterization(); auto reference = Orientation3DLocalParameterization(); - 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) { + 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); - double x[4] = {qw, qx, qy, 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; + 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()); fuse_core::MatrixXd expected(3, 4); /* *INDENT-OFF* */ - expected << 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0; + expected << 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* */ reference.ComputeMinusJacobian(x, expected.data()); EXPECT_TRUE(success); Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); - EXPECT_TRUE( - expected.isApprox( - actual, - 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" - << "Actual is:\n" << actual.format(clean) << - "\n" - << "Difference is:\n" << - (expected - actual).format(clean) - << "\n"; + EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" + << expected.format(clean) << "\n" + << "Actual is:\n" + << actual.format(clean) << "\n" + << "Difference is:\n" + << (expected - actual).format(clean) << "\n"; } } } @@ -275,8 +267,8 @@ TEST(Orientation3DStamped, MinusJacobian) TEST(Orientation3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = Orientation3DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr 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)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -290,7 +282,7 @@ TEST(Orientation3DStamped, Stamped) struct QuaternionCostFunction { - explicit QuaternionCostFunction(double * observation) + explicit QuaternionCostFunction(double* observation) { observation_[0] = observation[0]; observation_[1] = observation[1]; @@ -298,24 +290,12 @@ struct QuaternionCostFunction observation_[3] = observation[3]; } - template - bool operator()(const T * quaternion, T * residual) const + template + bool operator()(const T* quaternion, T* residual) const { - T inverse_quaternion[4] = - { - quaternion[0], - -quaternion[1], - -quaternion[2], - -quaternion[3] - }; + T inverse_quaternion[4] = { quaternion[0], -quaternion[1], -quaternion[2], -quaternion[3] }; - T observation[4] = - { - T(observation_[0]), - T(observation_[1]), - T(observation_[2]), - T(observation_[3]) - }; + T observation[4] = { T(observation_[0]), T(observation_[1]), T(observation_[2]), T(observation_[3]) }; T output[4]; @@ -342,25 +322,18 @@ TEST(Orientation3DStamped, Optimization) orientation.z() = 0.239; // 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)); + double target_quat[4] = { 1.0, 0.0, 0.0, 0.0 }; + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction(new QuaternionCostFunction(target_quat)); // Build the problem. ceres::Problem problem; #if !CERES_SUPPORTS_MANIFOLDS - problem.AddParameterBlock( - orientation.data(), - orientation.size(), - orientation.localParameterization()); + problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.localParameterization()); #else - problem.AddParameterBlock( - orientation.data(), - orientation.size(), - orientation.manifold()); + problem.AddParameterBlock(orientation.data(), orientation.size(), orientation.manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(orientation.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -443,16 +416,16 @@ TEST(Orientation3DStamped, Serialization) struct Orientation3DFunctor { - template - bool Plus(const T * x, const T * delta, T * x_plus_delta) const + template + bool Plus(const T* x, const T* delta, T* x_plus_delta) const { T q_delta[4]; ceres::AngleAxisToQuaternion(delta, q_delta); ceres::QuaternionProduct(x, q_delta, x_plus_delta); return true; } - template - bool Minus(const T * y, const T * x, T * y_minus_x) const + template + bool Minus(const T* y, const T* x, T* y_minus_x) const { T x_inverse[4]; QuaternionInverse(x, x_inverse); @@ -469,9 +442,9 @@ TEST(Orientation3DStamped, ManifoldPlus) { auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); - 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}; + 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 = manifold->Plus(x, delta, result); EXPECT_TRUE(success); @@ -488,39 +461,35 @@ TEST(Orientation3DStamped, ManifoldPlusJacobian) auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation3DManifold(); - 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) { + 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); - double x[4] = {qw, qx, qy, 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; + 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()); fuse_core::MatrixXd expected(4, 3); /* *INDENT-OFF* */ - expected << 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0; + expected << 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* */ reference.PlusJacobian(x, expected.data()); EXPECT_TRUE(success); Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); - EXPECT_TRUE( - expected.isApprox( - actual, - 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" - << "Actual is:\n" << actual.format(clean) << "\n" - << "Difference is:\n" << (expected - actual).format(clean) - << "\n"; + EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" + << expected.format(clean) << "\n" + << "Actual is:\n" + << actual.format(clean) << "\n" + << "Difference is:\n" + << (expected - actual).format(clean) << "\n"; } } } @@ -530,9 +499,9 @@ TEST(Orientation3DStamped, ManifoldPlusJacobian) TEST(Orientation3DStamped, ManifoldMinus) { - 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}; + 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 }; auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); bool success = manifold->Minus(x2, x1, result); @@ -550,37 +519,35 @@ TEST(Orientation3DStamped, ManifoldMinusJacobian) auto manifold = Orientation3DStamped(rclcpp::Time(0, 0)).manifold(); auto reference = Orientation3DManifold(); - 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) { + 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); - double x[4] = {qw, qx, qy, 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; + 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()); fuse_core::MatrixXd expected(3, 4); /* *INDENT-OFF* */ - expected << 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0; + expected << 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* */ reference.MinusJacobian(x, expected.data()); EXPECT_TRUE(success); Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]"); - EXPECT_TRUE( - expected.isApprox( - actual, - 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n" - << "Actual is:\n" << actual.format(clean) << "\n" - << "Difference is:\n" << (expected - actual).format(clean) - << "\n"; + EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" + << expected.format(clean) << "\n" + << "Actual is:\n" + << actual.format(clean) << "\n" + << "Difference is:\n" + << (expected - actual).format(clean) << "\n"; } } } diff --git a/fuse_variables/test/test_point_2d_fixed_landmark.cpp b/fuse_variables/test/test_point_2d_fixed_landmark.cpp index 9aa369adf..45b13ffd7 100644 --- a/fuse_variables/test/test_point_2d_fixed_landmark.cpp +++ b/fuse_variables/test/test_point_2d_fixed_landmark.cpp @@ -46,7 +46,6 @@ using fuse_variables::Point2DFixedLandmark; - TEST(Point2DFixedLandmark, Type) { Point2DFixedLandmark variable(0); @@ -72,9 +71,12 @@ TEST(Point2DFixedLandmark, UUID) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); @@ -90,16 +92,16 @@ TEST(Point2DFixedLandmark, Optimization) position.y() = -3.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(position.data(), position.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); - if (position.holdConstant()) { + if (position.holdConstant()) + { problem.SetParameterBlockConstant(position.data()); } diff --git a/fuse_variables/test/test_point_2d_landmark.cpp b/fuse_variables/test/test_point_2d_landmark.cpp index 8ca6d5b99..0615470dd 100644 --- a/fuse_variables/test/test_point_2d_landmark.cpp +++ b/fuse_variables/test/test_point_2d_landmark.cpp @@ -46,7 +46,6 @@ using fuse_variables::Point2DLandmark; - TEST(Point2DLandmark, Type) { Point2DLandmark variable(0); @@ -72,9 +71,12 @@ TEST(Point2DLandmark, UUID) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); @@ -90,13 +92,12 @@ TEST(Point2DLandmark, Optimization) position.y() = -3.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(position.data(), position.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_point_3d_fixed_landmark.cpp b/fuse_variables/test/test_point_3d_fixed_landmark.cpp index 3376c0306..5bdf54a46 100644 --- a/fuse_variables/test/test_point_3d_fixed_landmark.cpp +++ b/fuse_variables/test/test_point_3d_fixed_landmark.cpp @@ -46,7 +46,6 @@ using fuse_variables::Point3DFixedLandmark; - TEST(Point3DFixedLandmark, Type) { Point3DFixedLandmark variable(0); @@ -72,9 +71,12 @@ TEST(Point3DFixedLandmark, UUID) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); @@ -92,16 +94,16 @@ TEST(Point3DFixedLandmark, Optimization) position.z() = 0.8; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(position.data(), position.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); - if (position.holdConstant()) { + if (position.holdConstant()) + { problem.SetParameterBlockConstant(position.data()); } diff --git a/fuse_variables/test/test_point_3d_landmark.cpp b/fuse_variables/test/test_point_3d_landmark.cpp index 287bb6d51..055b25331 100644 --- a/fuse_variables/test/test_point_3d_landmark.cpp +++ b/fuse_variables/test/test_point_3d_landmark.cpp @@ -46,7 +46,6 @@ using fuse_variables::Point3DLandmark; - TEST(Point3DLandmark, Type) { Point3DLandmark variable(0); @@ -72,9 +71,12 @@ TEST(Point3DLandmark, UUID) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); @@ -92,13 +94,12 @@ TEST(Point3DLandmark, Optimization) position.z() = 0.8; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(position.data(), position.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); diff --git a/fuse_variables/test/test_position_2d_stamped.cpp b/fuse_variables/test/test_position_2d_stamped.cpp index 584ea8ef4..ce35789db 100644 --- a/fuse_variables/test/test_position_2d_stamped.cpp +++ b/fuse_variables/test/test_position_2d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::Position2DStamped; - TEST(Position2DStamped, Type) { Position2DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(Position2DStamped, UUID) Position2DStamped 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 variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + Position2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ 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 variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + Position2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(Position2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = Position2DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr 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)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(Position2DStamped, Stamped) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); @@ -118,19 +116,17 @@ struct CostFunctor TEST(Position2DStamped, Optimization) { // Create a Position2DStamped - Position2DStamped position( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + Position2DStamped position(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); position.x() = 1.5; position.y() = -3.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(position.data(), position.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -147,8 +143,7 @@ TEST(Position2DStamped, Optimization) TEST(Position2DStamped, Serialization) { // Create a Position2DStamped - Position2DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + Position2DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.x() = 1.5; expected.y() = -3.0; diff --git a/fuse_variables/test/test_position_3d_stamped.cpp b/fuse_variables/test/test_position_3d_stamped.cpp index 112943f2a..c70746b53 100644 --- a/fuse_variables/test/test_position_3d_stamped.cpp +++ b/fuse_variables/test/test_position_3d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::Position3DStamped; - TEST(Position3DStamped, Type) { Position3DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -106,8 +105,8 @@ TEST(Position3DStamped, UUID) TEST(Position3DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = Position3DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr 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)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -121,9 +120,12 @@ TEST(Position3DStamped, Stamped) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); @@ -141,18 +143,14 @@ TEST(Position3DStamped, Optimization) position.z() = 0.8; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(position.data(), position.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position.data()); - problem.AddResidualBlock( - cost_function, - nullptr, - parameter_blocks); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -168,8 +166,7 @@ TEST(Position3DStamped, Optimization) TEST(Position3DStamped, Serialization) { // Create a Position3DStamped - Position3DStamped expected(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("hal9000")); + Position3DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.x() = 1.5; expected.y() = -3.0; expected.z() = 0.8; diff --git a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp index bae0a296a..029af9778 100644 --- a/fuse_variables/test/test_velocity_angular_2d_stamped.cpp +++ b/fuse_variables/test/test_velocity_angular_2d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::VelocityAngular2DStamped; - TEST(VelocityAngular2DStamped, Type) { VelocityAngular2DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(VelocityAngular2DStamped, UUID) VelocityAngular2DStamped 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 variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ 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 variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + VelocityAngular2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityAngular2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = VelocityAngular2DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr 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)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(VelocityAngular2DStamped, Stamped) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(2.7); return true; @@ -117,18 +115,16 @@ struct CostFunctor TEST(VelocityAngular2DStamped, Optimization) { // Create a VelocityAngular2DStamped - VelocityAngular2DStamped velocity(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("hal9000")); + VelocityAngular2DStamped velocity(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); velocity.yaw() = 1.5; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(velocity.data(), velocity.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); @@ -144,8 +140,7 @@ TEST(VelocityAngular2DStamped, Optimization) TEST(VelocityAngular2DStamped, Serialization) { // Create a VelocityAngular2DStamped - VelocityAngular2DStamped expected(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("hal9000")); + VelocityAngular2DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.yaw() = 1.5; // Serialize the variable into an archive diff --git a/fuse_variables/test/test_velocity_angular_3d_stamped.cpp b/fuse_variables/test/test_velocity_angular_3d_stamped.cpp index d1378aad1..5d269cb39 100644 --- a/fuse_variables/test/test_velocity_angular_3d_stamped.cpp +++ b/fuse_variables/test/test_velocity_angular_3d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::VelocityAngular3DStamped; - TEST(VelocityAngular3DStamped, Type) { VelocityAngular3DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(VelocityAngular3DStamped, UUID) VelocityAngular3DStamped 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 variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityAngular3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,19 +77,16 @@ 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 variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + VelocityAngular3DStamped 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 = VelocityAngular3DStamped::make_shared( - rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr 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)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -106,9 +100,12 @@ TEST(VelocityAngular3DStamped, Stamped) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); @@ -120,25 +117,20 @@ struct CostFunctor TEST(VelocityAngular3DStamped, Optimization) { // Create a VelocityAngular3DStamped - VelocityAngular3DStamped velocity(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("hal9000")); + VelocityAngular3DStamped velocity(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); velocity.roll() = 1.5; velocity.pitch() = -3.0; velocity.yaw() = 14.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(velocity.data(), velocity.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); - problem.AddResidualBlock( - cost_function, - nullptr, - parameter_blocks); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -154,8 +146,7 @@ TEST(VelocityAngular3DStamped, Optimization) TEST(VelocityAngular3DStamped, Serialization) { // Create a VelocityAngular3DStamped - VelocityAngular3DStamped expected(rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("hal9000")); + VelocityAngular3DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.roll() = 1.5; expected.pitch() = -3.0; expected.yaw() = 14.0; diff --git a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp index 914b13d32..ea8aef29e 100644 --- a/fuse_variables/test/test_velocity_linear_2d_stamped.cpp +++ b/fuse_variables/test/test_velocity_linear_2d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::VelocityLinear2DStamped; - TEST(VelocityLinear2DStamped, Type) { VelocityLinear2DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(VelocityLinear2DStamped, UUID) VelocityLinear2DStamped 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 variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityLinear2DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,18 +77,16 @@ 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 variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("r2d2")); + VelocityLinear2DStamped variable2(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("bb8")); EXPECT_NE(variable1.uuid(), variable2.uuid()); } } TEST(VelocityLinear2DStamped, Stamped) { - fuse_core::Variable::SharedPtr base = VelocityLinear2DStamped::make_shared( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr 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)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -105,9 +100,12 @@ TEST(VelocityLinear2DStamped, Stamped) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); @@ -118,24 +116,19 @@ struct CostFunctor TEST(VelocityLinear2DStamped, Optimization) { // Create a VelocityLinear2DStamped - VelocityLinear2DStamped velocity( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + VelocityLinear2DStamped velocity(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); velocity.x() = 1.5; velocity.y() = -3.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(velocity.data(), velocity.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); - problem.AddResidualBlock( - cost_function, - nullptr, - parameter_blocks); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -150,8 +143,7 @@ TEST(VelocityLinear2DStamped, Optimization) TEST(VelocityLinear2DStamped, Serialization) { // Create a VelocityLinear2DStamped - VelocityLinear2DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + VelocityLinear2DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.x() = 1.5; expected.y() = -3.0; diff --git a/fuse_variables/test/test_velocity_linear_3d_stamped.cpp b/fuse_variables/test/test_velocity_linear_3d_stamped.cpp index 59e6909d0..7bcca429d 100644 --- a/fuse_variables/test/test_velocity_linear_3d_stamped.cpp +++ b/fuse_variables/test/test_velocity_linear_3d_stamped.cpp @@ -46,7 +46,6 @@ using fuse_variables::VelocityLinear3DStamped; - TEST(VelocityLinear3DStamped, Type) { VelocityLinear3DStamped variable(rclcpp::Time(12345678, 910111213)); @@ -61,10 +60,8 @@ TEST(VelocityLinear3DStamped, UUID) VelocityLinear3DStamped 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 variable3(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); + VelocityLinear3DStamped variable4(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("c3po")); EXPECT_EQ(variable3.uuid(), variable4.uuid()); } @@ -80,19 +77,16 @@ 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 variable1(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("8d8")); + VelocityLinear3DStamped 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 = VelocityLinear3DStamped::make_shared( - rclcpp::Time(12345678, 910111213), - fuse_core::uuid::generate("mo")); + fuse_core::Variable::SharedPtr 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)); EXPECT_EQ(rclcpp::Time(12345678, 910111213), derived->stamp()); @@ -106,9 +100,12 @@ TEST(VelocityLinear3DStamped, Stamped) struct CostFunctor { - CostFunctor() {} + CostFunctor() + { + } - template bool operator()(const T * const x, T * residual) const + template + bool operator()(const T* const x, T* residual) const { residual[0] = x[0] - T(3.0); residual[1] = x[1] + T(8.0); @@ -120,25 +117,20 @@ struct CostFunctor TEST(VelocityLinear3DStamped, Optimization) { // Create a VelocityLinear3DStamped - VelocityLinear3DStamped velocity( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + VelocityLinear3DStamped velocity(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); velocity.x() = 1.5; velocity.y() = -3.0; velocity.z() = 14.0; // Create a simple a constraint - ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction( - new CostFunctor()); + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); // Build the problem. ceres::Problem problem; problem.AddParameterBlock(velocity.data(), velocity.size()); - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(velocity.data()); - problem.AddResidualBlock( - cost_function, - nullptr, - parameter_blocks); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -154,8 +146,7 @@ TEST(VelocityLinear3DStamped, Optimization) TEST(VelocityLinear3DStamped, Serialization) { // Create a VelocityLinear3DStamped - VelocityLinear3DStamped expected( - rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); + VelocityLinear3DStamped expected(rclcpp::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); expected.x() = 1.5; expected.y() = -3.0; expected.z() = 14.0; diff --git a/fuse_viz/CMakeLists.txt b/fuse_viz/CMakeLists.txt index 8f4f2bfb2..64694f847 100644 --- a/fuse_viz/CMakeLists.txt +++ b/fuse_viz/CMakeLists.txt @@ -25,18 +25,19 @@ find_package(Eigen3 REQUIRED) find_package(Qt5 COMPONENTS Core Widgets REQUIRED) -########### -## Build ## -########### +# ############################################################################## +# Build ## +# ############################################################################## -qt5_wrap_cpp(moc_files +qt5_wrap_cpp( + moc_files include/fuse_viz/mapped_covariance_property.hpp include/fuse_viz/pose_2d_stamped_property.hpp include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp - include/fuse_viz/serialized_graph_display.hpp -) + include/fuse_viz/serialized_graph_display.hpp) -add_library(${PROJECT_NAME} SHARED +add_library( + ${PROJECT_NAME} SHARED src/mapped_covariance_property.cpp src/mapped_covariance_visual.cpp src/pose_2d_stamped_property.cpp @@ -44,49 +45,37 @@ add_library(${PROJECT_NAME} SHARED src/relative_pose_2d_stamped_constraint_property.cpp src/relative_pose_2d_stamped_constraint_visual.cpp src/serialized_graph_display.cpp - ${moc_files} -) -target_link_libraries(${PROJECT_NAME} PRIVATE - ${fuse_msgs_TARGETS} - Eigen3::Eigen - fuse_constraints::fuse_constraints - fuse_core::fuse_core - fuse_variables::fuse_variables - ${geometry_msgs_TARGETS} - rviz_common::rviz_common - rviz_rendering::rviz_rendering - tf2_geometry_msgs::tf2_geometry_msgs -) - -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ - ${Qt5Widgets_INCLUDE_DIRS} -) - -############# -## Install ## -############# - -install(TARGETS ${PROJECT_NAME} + ${moc_files}) +target_link_libraries( + ${PROJECT_NAME} + PRIVATE ${fuse_msgs_TARGETS} + Eigen3::Eigen + fuse_constraints::fuse_constraints + fuse_core::fuse_core + fuse_variables::fuse_variables + ${geometry_msgs_TARGETS} + rviz_common::rviz_common + rviz_rendering::rviz_rendering + tf2_geometry_msgs::tf2_geometry_msgs) + +target_include_directories( + ${PROJECT_NAME} + PUBLIC $ + $ + ${Qt5Widgets_INCLUDE_DIRS}) + +# ############################################################################## +# Install ## +# ############################################################################## + +install( + TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) -install(DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) pluginlib_export_plugin_description_file(rviz_common rviz_plugins.xml) -############# -## Testing ## -############# - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/fuse_viz/include/fuse_viz/conversions.hpp b/fuse_viz/include/fuse_viz/conversions.hpp index 18428a628..63897a506 100644 --- a/fuse_viz/include/fuse_viz/conversions.hpp +++ b/fuse_viz/include/fuse_viz/conversions.hpp @@ -53,7 +53,6 @@ #include #include - namespace tf2 { @@ -62,8 +61,8 @@ namespace tf2 * @param[in] covariance 3x3 covariance matrix. * @param[out] msg 6x6 covariance message, which is stored as a plain array with 36 elements. */ -template -inline void toMsg(const Eigen::MatrixBase & covariance, std::array & msg) +template +inline void toMsg(const Eigen::MatrixBase& covariance, std::array& msg) { using Scalar = typename Derived::Scalar; using Matrix6 = Eigen::Matrix; @@ -83,41 +82,39 @@ inline void toMsg(const Eigen::MatrixBase & covariance, std::array(position.x()), static_cast(position.y()), - static_cast(position.z())}; + return { static_cast(position.x()), static_cast(position.y()), static_cast(position.z()) }; } -inline Ogre::Quaternion toOgre(const tf2::Quaternion & orientation) +inline Ogre::Quaternion toOgre(const tf2::Quaternion& orientation) { - return {static_cast(orientation.w()), static_cast(orientation.x()), // NOLINT - static_cast(orientation.y()), static_cast(orientation.z())}; + return { static_cast(orientation.w()), static_cast(orientation.x()), // NOLINT + static_cast(orientation.y()), static_cast(orientation.z()) }; } -inline Ogre::Vector3 toOgre(const fuse_variables::Position2DStamped & position) +inline Ogre::Vector3 toOgre(const fuse_variables::Position2DStamped& position) { - return {static_cast(position.x()), static_cast(position.y()), 0}; + return { static_cast(position.x()), static_cast(position.y()), 0 }; } -inline Ogre::Quaternion toOgre(const fuse_variables::Orientation2DStamped & orientation) +inline Ogre::Quaternion toOgre(const fuse_variables::Orientation2DStamped& orientation) { return toOgre(toTF(orientation)); } @@ -127,31 +124,28 @@ inline Ogre::Quaternion toOgre(const fuse_variables::Orientation2DStamped & orie namespace { -inline tf2::Transform getPose( - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation) +inline tf2::Transform getPose(const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation) { return tf2::Transform(toTF(orientation), toTF(position)); } -inline tf2::Transform getPose( - const fuse_core::Graph & graph, const fuse_core::UUID & position_uuid, - const fuse_core::UUID & orientation_uuid) +inline tf2::Transform getPose(const fuse_core::Graph& graph, const fuse_core::UUID& position_uuid, + const fuse_core::UUID& orientation_uuid) { - const auto position = - dynamic_cast(&graph.getVariable(position_uuid)); - if (!position) { - throw std::runtime_error( - "Failed to get variable " + fuse_core::uuid::to_string(position_uuid) + - " from graph as fuse_variables::Position2DStamped."); + const auto position = dynamic_cast(&graph.getVariable(position_uuid)); + if (!position) + { + throw std::runtime_error("Failed to get variable " + fuse_core::uuid::to_string(position_uuid) + + " from graph as fuse_variables::Position2DStamped."); } - const auto orientation = dynamic_cast( - &graph.getVariable(orientation_uuid)); - if (!orientation) { - throw std::runtime_error( - "Failed to get variable " + fuse_core::uuid::to_string(orientation_uuid) + - " from graph as fuse_variables::Orientation2DStamped."); + const auto orientation = + dynamic_cast(&graph.getVariable(orientation_uuid)); + if (!orientation) + { + throw std::runtime_error("Failed to get variable " + fuse_core::uuid::to_string(orientation_uuid) + + " from graph as fuse_variables::Orientation2DStamped."); } return getPose(*position, *orientation); diff --git a/fuse_viz/include/fuse_viz/mapped_covariance_property.hpp b/fuse_viz/include/fuse_viz/mapped_covariance_property.hpp index d3b9130c5..120a511df 100644 --- a/fuse_viz/include/fuse_viz/mapped_covariance_property.hpp +++ b/fuse_viz/include/fuse_viz/mapped_covariance_property.hpp @@ -50,7 +50,6 @@ #include #include - namespace Ogre { @@ -87,10 +86,9 @@ class MappedCovarianceProperty : public rviz_common::properties::BoolProperty RGB, }; - MappedCovarianceProperty( - const QString & name = "Covariance", bool default_value = false, - const QString & description = QString(), rviz_common::properties::Property * parent = 0, - const char * changed_slot = 0, QObject * receiver = 0); + MappedCovarianceProperty(const QString& name = "Covariance", bool default_value = false, + const QString& description = QString(), rviz_common::properties::Property* parent = 0, + const char* changed_slot = 0, QObject* receiver = 0); virtual ~MappedCovarianceProperty(); @@ -98,10 +96,9 @@ class MappedCovarianceProperty : public rviz_common::properties::BoolProperty bool getOrientationBool(); // Methods to manage the unordered map of Covariance Visuals - MappedCovarianceVisualPtr createAndInsertVisual( - const std::string & key, Ogre::SceneManager * scene_manager, - Ogre::SceneNode * parent_node); - void eraseVisual(const std::string & key); + MappedCovarianceVisualPtr createAndInsertVisual(const std::string& key, Ogre::SceneManager* scene_manager, + Ogre::SceneNode* parent_node); + void eraseVisual(const std::string& key); void clearVisual(); size_t sizeVisual(); @@ -114,23 +111,23 @@ private Q_SLOTS: void updateColorStyleChoice(); private: - void updateColorAndAlphaAndScaleAndOffset(const MappedCovarianceVisualPtr & visual); - void updateOrientationFrame(const MappedCovarianceVisualPtr & visual); - void updateVisibility(const MappedCovarianceVisualPtr & visual); + void updateColorAndAlphaAndScaleAndOffset(const MappedCovarianceVisualPtr& visual); + void updateOrientationFrame(const MappedCovarianceVisualPtr& visual); + void updateVisibility(const MappedCovarianceVisualPtr& visual); std::unordered_map covariances_; - rviz_common::properties::BoolProperty * position_property_; - rviz_common::properties::ColorProperty * position_color_property_; - rviz_common::properties::FloatProperty * position_alpha_property_; - rviz_common::properties::FloatProperty * position_scale_property_; - rviz_common::properties::BoolProperty * orientation_property_; - rviz_common::properties::EnumProperty * orientation_frame_property_; - rviz_common::properties::EnumProperty * orientation_colorstyle_property_; - rviz_common::properties::ColorProperty * orientation_color_property_; - rviz_common::properties::FloatProperty * orientation_alpha_property_; - rviz_common::properties::FloatProperty * orientation_offset_property_; - rviz_common::properties::FloatProperty * orientation_scale_property_; + rviz_common::properties::BoolProperty* position_property_; + rviz_common::properties::ColorProperty* position_color_property_; + rviz_common::properties::FloatProperty* position_alpha_property_; + rviz_common::properties::FloatProperty* position_scale_property_; + rviz_common::properties::BoolProperty* orientation_property_; + rviz_common::properties::EnumProperty* orientation_frame_property_; + rviz_common::properties::EnumProperty* orientation_colorstyle_property_; + rviz_common::properties::ColorProperty* orientation_color_property_; + rviz_common::properties::FloatProperty* orientation_alpha_property_; + rviz_common::properties::FloatProperty* orientation_offset_property_; + rviz_common::properties::FloatProperty* orientation_scale_property_; }; } // namespace fuse_viz diff --git a/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp b/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp index 484e727a6..b5f4d3dc1 100644 --- a/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp +++ b/fuse_viz/include/fuse_viz/mapped_covariance_visual.hpp @@ -46,7 +46,6 @@ #include - namespace Ogre { class SceneManager; @@ -96,10 +95,9 @@ class MappedCovarianceVisual : public rviz_rendering::Object * @param pos_scale Scale of the position covariance * @param ori_scale Scale of the orientation covariance */ - MappedCovarianceVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, bool is_local_rotation, - bool is_visible = true, float pos_scale = 1.0f, float ori_scale = 0.1f, - float ori_offset = 0.1f); + MappedCovarianceVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, bool is_local_rotation, + bool is_visible = true, float pos_scale = 1.0f, float ori_scale = 0.1f, + float ori_offset = 0.1f); public: ~MappedCovarianceVisual() override; @@ -123,7 +121,7 @@ class MappedCovarianceVisual : public rviz_rendering::Object * @param b Blue component */ virtual void setPositionColor(float r, float g, float b, float a); - void setPositionColor(const Ogre::ColourValue & color); + void setPositionColor(const Ogre::ColourValue& color); /** * \brief Set the color of the orientation covariance. Values are in the range [0, 1] @@ -133,7 +131,7 @@ class MappedCovarianceVisual : public rviz_rendering::Object * @param b Blue component */ virtual void setOrientationColor(float r, float g, float b, float a); - void setOrientationColor(const Ogre::ColourValue & color); + void setOrientationColor(const Ogre::ColourValue& color); void setOrientationColorToRGB(float a); /** @brief Set the covariance. @@ -141,16 +139,16 @@ class MappedCovarianceVisual : public rviz_rendering::Object * This effectively changes the orientation and scale of position and orientation * covariance shapes */ - virtual void setCovariance(const geometry_msgs::msg::PoseWithCovariance & pose); + virtual void setCovariance(const geometry_msgs::msg::PoseWithCovariance& pose); - virtual const Ogre::Vector3 & getPositionCovarianceScale(); - virtual const Ogre::Quaternion & getPositionCovarianceOrientation(); + virtual const Ogre::Vector3& getPositionCovarianceScale(); + virtual const Ogre::Quaternion& getPositionCovarianceOrientation(); /** * \brief Get the root scene node of the position part of this covariance * @return the root scene node of the position part of this covariance */ - Ogre::SceneNode * getPositionSceneNode() + Ogre::SceneNode* getPositionSceneNode() { return position_scale_node_; } @@ -159,7 +157,7 @@ class MappedCovarianceVisual : public rviz_rendering::Object * \brief Get the root scene node of the orientation part of this covariance * @return the root scene node of the orientation part of this covariance */ - Ogre::SceneNode * getOrientationSceneNode() + Ogre::SceneNode* getOrientationSceneNode() { return orientation_root_node_; } @@ -168,7 +166,7 @@ class MappedCovarianceVisual : public rviz_rendering::Object * \brief Get the shape used to display position covariance * @return the shape used to display position covariance */ - rviz_rendering::Shape * getPositionShape() + rviz_rendering::Shape* getPositionShape() { return position_shape_; } @@ -177,12 +175,12 @@ class MappedCovarianceVisual : public rviz_rendering::Object * \brief Get the shape used to display orientation covariance in an especific axis * @return the shape used to display orientation covariance in an especific axis */ - rviz_rendering::Shape * getOrientationShape(ShapeIndex index); + rviz_rendering::Shape* getOrientationShape(ShapeIndex index); /** * \brief Sets user data on all ogre objects we own */ - virtual void setUserData(const Ogre::Any & data); + virtual void setUserData(const Ogre::Any& data); /** * \brief Sets visibility of this covariance @@ -204,12 +202,12 @@ class MappedCovarianceVisual : public rviz_rendering::Object /** * \brief Sets position of the frame this covariance is attached */ - virtual void setPosition(const Ogre::Vector3 & position); + virtual void setPosition(const Ogre::Vector3& position); /** * \brief Sets orientation of the frame this covariance is attached */ - virtual void setOrientation(const Ogre::Quaternion & orientation); + virtual void setOrientation(const Ogre::Quaternion& orientation); /** * \brief Sets which frame to attach the covariance of the orientation @@ -217,22 +215,22 @@ class MappedCovarianceVisual : public rviz_rendering::Object virtual void setRotatingFrame(bool use_rotating_frame); private: - void updatePosition(const Eigen::Matrix6d & covariance); - void updateOrientation(const Eigen::Matrix6d & covariance, ShapeIndex index); + void updatePosition(const Eigen::Matrix6d& covariance); + void updateOrientation(const Eigen::Matrix6d& covariance, ShapeIndex index); void updateOrientationVisibility(); - Ogre::SceneNode * root_node_ = nullptr; - Ogre::SceneNode * fixed_orientation_node_ = nullptr; - Ogre::SceneNode * position_scale_node_ = nullptr; - Ogre::SceneNode * position_node_ = nullptr; + Ogre::SceneNode* root_node_ = nullptr; + Ogre::SceneNode* fixed_orientation_node_ = nullptr; + Ogre::SceneNode* position_scale_node_ = nullptr; + Ogre::SceneNode* position_node_ = nullptr; - Ogre::SceneNode * orientation_root_node_ = nullptr; - Ogre::SceneNode * orientation_offset_node_[kNumOriShapes]; + Ogre::SceneNode* orientation_root_node_ = nullptr; + Ogre::SceneNode* orientation_offset_node_[kNumOriShapes]; - rviz_rendering::Shape * position_shape_; //!< Ellipse used for the position - //!< covariance - rviz_rendering::Shape * orientation_shape_[kNumOriShapes]; //!< Cylinders used for the - //!< orientation covariance + rviz_rendering::Shape* position_shape_; //!< Ellipse used for the position + //!< covariance + rviz_rendering::Shape* orientation_shape_[kNumOriShapes]; //!< Cylinders used for the + //!< orientation covariance bool local_rotation_; @@ -248,14 +246,14 @@ class MappedCovarianceVisual : public rviz_rendering::Object private: // Hide Object methods we don't want to expose // NOTE: Apparently we still need to define them... - virtual void setScale(const Ogre::Vector3 &) + virtual void setScale(const Ogre::Vector3&) { } virtual void setColor(float, float, float, float) { } - virtual const Ogre::Vector3 & getPosition(); - virtual const Ogre::Quaternion & getOrientation(); + virtual const Ogre::Vector3& getPosition(); + virtual const Ogre::Quaternion& getOrientation(); // Make MappedCovarianceProperty friend class so it create MappedCovarianceVisual objects friend class MappedCovarianceProperty; diff --git a/fuse_viz/include/fuse_viz/pose_2d_stamped_property.hpp b/fuse_viz/include/fuse_viz/pose_2d_stamped_property.hpp index 33d04ef9f..ed01eac0b 100644 --- a/fuse_viz/include/fuse_viz/pose_2d_stamped_property.hpp +++ b/fuse_viz/include/fuse_viz/pose_2d_stamped_property.hpp @@ -44,7 +44,6 @@ #include - namespace Ogre { @@ -74,18 +73,16 @@ class Pose2DStampedProperty : public rviz_common::properties::BoolProperty using Visual = Pose2DStampedVisual; using VisualPtr = std::shared_ptr; - Pose2DStampedProperty( - const QString & name = "Pose2DStamped", bool default_value = true, - const QString & description = QString(), Property * parent = NULL, - const char * changed_slot = NULL, QObject * receiver = NULL); + Pose2DStampedProperty(const QString& name = "Pose2DStamped", bool default_value = true, + const QString& description = QString(), Property* parent = NULL, + const char* changed_slot = NULL, QObject* receiver = NULL); ~Pose2DStampedProperty() override = default; - VisualPtr createAndInsertOrUpdateVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation); - void eraseVisual(const fuse_core::UUID & uuid); + VisualPtr createAndInsertOrUpdateVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation); + void eraseVisual(const fuse_core::UUID& uuid); void clearVisual(); public Q_SLOTS: @@ -99,21 +96,21 @@ private Q_SLOTS: void updateTextScale(); private: - void updateAxesAlpha(const VisualPtr & constraint); - void updateScale(const VisualPtr & constraint); - void updateShowText(const VisualPtr & constraint); - void updateSphereColorAlpha(const VisualPtr & constraint); - void updateTextScale(const VisualPtr & constraint); - void updateVisibility(const VisualPtr & constraint); + void updateAxesAlpha(const VisualPtr& constraint); + void updateScale(const VisualPtr& constraint); + void updateShowText(const VisualPtr& constraint); + void updateSphereColorAlpha(const VisualPtr& constraint); + void updateTextScale(const VisualPtr& constraint); + void updateVisibility(const VisualPtr& constraint); std::unordered_map variables_; - rviz_common::properties::ColorProperty * color_property_; - rviz_common::properties::BoolProperty * show_text_property_; - rviz_common::properties::FloatProperty * sphere_alpha_property_; - rviz_common::properties::FloatProperty * axes_alpha_property_; - rviz_common::properties::FloatProperty * scale_property_; - rviz_common::properties::FloatProperty * text_scale_property_; + rviz_common::properties::ColorProperty* color_property_; + rviz_common::properties::BoolProperty* show_text_property_; + rviz_common::properties::FloatProperty* sphere_alpha_property_; + rviz_common::properties::FloatProperty* axes_alpha_property_; + rviz_common::properties::FloatProperty* scale_property_; + rviz_common::properties::FloatProperty* text_scale_property_; }; } // namespace fuse_viz diff --git a/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp b/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp index dc240f7ad..67ed1bdbc 100644 --- a/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp +++ b/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.hpp @@ -45,7 +45,6 @@ #include #include - namespace Ogre { @@ -89,10 +88,9 @@ class Pose2DStampedVisual : public rviz_rendering::Object * @param[in] orientation fuse_variables::Orientation2DStamped orientation. * @param[in] visible Initial visibility. */ - Pose2DStampedVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation, const bool visible = true); + Pose2DStampedVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation, const bool visible = true); public: ~Pose2DStampedVisual() override; @@ -102,15 +100,14 @@ class Pose2DStampedVisual : public rviz_rendering::Object * @param[in] position 2D position stamped variable. * @param[in] orientation 2D orientation stamped variable. */ - void setPose2DStamped( - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation); + void setPose2DStamped(const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation); /** * @brief Get the root scene node of this variable visual. * @return the root scene node of this variable visual. */ - Ogre::SceneNode * getSceneNode() + Ogre::SceneNode* getSceneNode() { return root_node_; } @@ -118,15 +115,15 @@ class Pose2DStampedVisual : public rviz_rendering::Object /** * @brief Sets user data on all ogre objects we own */ - void setUserData(const Ogre::Any & data) override; + void setUserData(const Ogre::Any& data) override; void setSphereColor(const float r, const float g, const float b, const float a); void setAxesAlpha(const float alpha); - void setScale(const Ogre::Vector3 & scale) override; + void setScale(const Ogre::Vector3& scale) override; - void setTextScale(const Ogre::Vector3 & scale); + void setTextScale(const Ogre::Vector3& scale); void setTextVisible(const bool visible); @@ -140,33 +137,35 @@ class Pose2DStampedVisual : public rviz_rendering::Object /** * @brief Sets position of the frame this constraint is attached */ - void setPosition(const Ogre::Vector3 & position) override; + void setPosition(const Ogre::Vector3& position) override; /** * @brief Sets orientation of the frame this constraint is attached */ - void setOrientation(const Ogre::Quaternion & orientation) override; + void setOrientation(const Ogre::Quaternion& orientation) override; private: - void setPose2DStamped(const Ogre::Vector3 & position, const Ogre::Quaternion & orientation); + void setPose2DStamped(const Ogre::Vector3& position, const Ogre::Quaternion& orientation); - Ogre::SceneNode * root_node_ = nullptr; - Ogre::SceneNode * sphere_node_ = nullptr; - Ogre::SceneNode * axes_node_ = nullptr; - Ogre::SceneNode * text_node_ = nullptr; + Ogre::SceneNode* root_node_ = nullptr; + Ogre::SceneNode* sphere_node_ = nullptr; + Ogre::SceneNode* axes_node_ = nullptr; + Ogre::SceneNode* text_node_ = nullptr; bool visible_; std::shared_ptr axes_; std::shared_ptr sphere_; - rviz_rendering::MovableText * text_; + rviz_rendering::MovableText* text_; private: // Hide Object methods we don't want to expose // NOTE: Apparently we still need to define them... - void setColor(float, float, float, float) override {} - const Ogre::Vector3 & getPosition() override; - const Ogre::Quaternion & getOrientation() override; + void setColor(float, float, float, float) override + { + } + const Ogre::Vector3& getPosition() override; + const Ogre::Quaternion& getOrientation() override; // Make Pose2DStampedProperty friend class so it create Pose2DStampedVisual objects friend class Pose2DStampedProperty; diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h index 0c952b1fd..d879ee071 100644 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h +++ b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h @@ -35,9 +35,7 @@ #ifndef FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_PROPERTY_H_ #define FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_PROPERTY_H_ -#warning \ - This header is obsolete, please include \ - fuse_viz/relative_pose_2d_stamped_constraint_property.hpp instead +#warning This header is obsolete, please include fuse_viz/relative_pose_2d_stamped_constraint_property.hpp instead #include diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp index 637855afc..7bfb32ebd 100644 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp +++ b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.hpp @@ -47,7 +47,6 @@ #include #include - namespace Ogre { @@ -91,22 +90,20 @@ class RelativePose2DStampedConstraintProperty : public BoolProperty using Visual = RelativePose2DStampedConstraintVisual; using VisualPtr = std::shared_ptr; - RelativePose2DStampedConstraintProperty( - const QString & name = "RelativePose2DStampedConstraint", - bool default_value = true, const QString & description = QString(), - Property * parent = NULL, const char * changed_slot = NULL, - QObject * receiver = NULL); + RelativePose2DStampedConstraintProperty(const QString& name = "RelativePose2DStampedConstraint", + bool default_value = true, const QString& description = QString(), + Property* parent = NULL, const char* changed_slot = NULL, + QObject* receiver = NULL); ~RelativePose2DStampedConstraintProperty() override = default; - VisualPtr createAndInsertVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_constraints::RelativePose2DStampedConstraint & constraint, - const fuse_core::Graph & graph); - void eraseVisual(const fuse_core::UUID & uuid); + VisualPtr createAndInsertVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_constraints::RelativePose2DStampedConstraint& constraint, + const fuse_core::Graph& graph); + void eraseVisual(const fuse_core::UUID& uuid); void clearVisual(); - void setColor(const QColor & color); + void setColor(const QColor& color); public Q_SLOTS: void updateVisibility(); @@ -124,31 +121,31 @@ private Q_SLOTS: void updateTextScale(); private: - void updateColor(const VisualPtr & constraint); - void updateErrorLineAlpha(const VisualPtr & constraint); - void updateErrorLineWidth(const VisualPtr & constraint); - void updateLossMinBrightness(const VisualPtr & constraint); - void updateRelativePoseAxesAlpha(const VisualPtr & constraint); - void updateRelativePoseAxesScale(const VisualPtr & constraint); - void updateRelativePoseLineAlpha(const VisualPtr & constraint); - void updateRelativePoseLineWidth(const VisualPtr & constraint); - void updateShowText(const VisualPtr & constraint); - void updateTextScale(const VisualPtr & constraint); - void updateVisibility(const VisualPtr & constraint); + void updateColor(const VisualPtr& constraint); + void updateErrorLineAlpha(const VisualPtr& constraint); + void updateErrorLineWidth(const VisualPtr& constraint); + void updateLossMinBrightness(const VisualPtr& constraint); + void updateRelativePoseAxesAlpha(const VisualPtr& constraint); + void updateRelativePoseAxesScale(const VisualPtr& constraint); + void updateRelativePoseLineAlpha(const VisualPtr& constraint); + void updateRelativePoseLineWidth(const VisualPtr& constraint); + void updateShowText(const VisualPtr& constraint); + void updateTextScale(const VisualPtr& constraint); + void updateVisibility(const VisualPtr& constraint); std::unordered_map constraints_; - ColorProperty * color_property_; - BoolProperty * show_text_property_; - FloatProperty * text_scale_property_; - FloatProperty * relative_pose_axes_alpha_property_; - FloatProperty * relative_pose_axes_scale_property_; - FloatProperty * relative_pose_line_alpha_property_; - FloatProperty * relative_pose_line_width_property_; - FloatProperty * error_line_alpha_property_; - FloatProperty * error_line_width_property_; - FloatProperty * loss_min_brightness_property_; - MappedCovarianceProperty * covariance_property_; + ColorProperty* color_property_; + BoolProperty* show_text_property_; + FloatProperty* text_scale_property_; + FloatProperty* relative_pose_axes_alpha_property_; + FloatProperty* relative_pose_axes_scale_property_; + FloatProperty* relative_pose_line_alpha_property_; + FloatProperty* relative_pose_line_width_property_; + FloatProperty* error_line_alpha_property_; + FloatProperty* error_line_width_property_; + FloatProperty* loss_min_brightness_property_; + MappedCovarianceProperty* covariance_property_; }; } // namespace fuse_viz diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h index ea506f767..fb3f22d0f 100644 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h +++ b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h @@ -35,8 +35,7 @@ #ifndef FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_VISUAL_H_ #define FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_VISUAL_H_ -#warning \ - This header is obsolete, please include fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp \ +#warning This header is obsolete, please include fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp \ instead #include diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp index c13785839..59a38650c 100644 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp +++ b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp @@ -48,7 +48,6 @@ #include #include - namespace Ogre { @@ -109,10 +108,9 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object * @param[in] constraint fuse_constraints::RelativePose2DStampedConstraint constraint. * @param[in] visible Initial visibility. */ - RelativePose2DStampedConstraintVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_constraints::RelativePose2DStampedConstraint & constraint, - const bool visible = true); + RelativePose2DStampedConstraintVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_constraints::RelativePose2DStampedConstraint& constraint, + const bool visible = true); public: using CovarianceVisualPtr = MappedCovarianceProperty::MappedCovarianceVisualPtr; @@ -125,15 +123,13 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object * @param[in] graph fuse_core::Graph, used to retrieve the first/source and second/target * constraint variables pose. */ - void setConstraint( - const fuse_constraints::RelativePose2DStampedConstraint & constraint, - const fuse_core::Graph & graph); + void setConstraint(const fuse_constraints::RelativePose2DStampedConstraint& constraint, const fuse_core::Graph& graph); /** * @brief Get the root scene node of this constraint visual * @return the root scene node of this constraint visual */ - Ogre::SceneNode * getSceneNode() + Ogre::SceneNode* getSceneNode() { return root_node_; } @@ -141,7 +137,7 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object /** * @brief Sets user data on all ogre objects we own */ - void setUserData(const Ogre::Any & data) override; + void setUserData(const Ogre::Any& data) override; void setRelativePoseLineWidth(const float line_width); @@ -155,9 +151,9 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object void setRelativePoseAxesAlpha(const float alpha); - void setRelativePoseAxesScale(const Ogre::Vector3 & scale); + void setRelativePoseAxesScale(const Ogre::Vector3& scale); - void setTextScale(const Ogre::Vector3 & scale); + void setTextScale(const Ogre::Vector3& scale); void setTextVisible(const bool visible); @@ -171,44 +167,44 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object /** * @brief Sets position of the frame this constraint is attached */ - void setPosition(const Ogre::Vector3 & position) override; + void setPosition(const Ogre::Vector3& position) override; /** * @brief Sets orientation of the frame this constraint is attached */ - void setOrientation(const Ogre::Quaternion & orientation) override; + void setOrientation(const Ogre::Quaternion& orientation) override; - const CovarianceVisualPtr & getCovariance() const + const CovarianceVisualPtr& getCovariance() const { return covariance_; } - void setCovariance(const CovarianceVisualPtr & covariance) + void setCovariance(const CovarianceVisualPtr& covariance) { covariance_ = covariance; } - const std::string & getSource() const + const std::string& getSource() const { return source_; } private: - Ogre::SceneNode * root_node_ = nullptr; - Ogre::SceneNode * relative_pose_line_node_ = nullptr; - Ogre::SceneNode * error_line_node_ = nullptr; - Ogre::SceneNode * relative_pose_axes_node_ = nullptr; - Ogre::SceneNode * text_node_ = nullptr; + Ogre::SceneNode* root_node_ = nullptr; + Ogre::SceneNode* relative_pose_line_node_ = nullptr; + Ogre::SceneNode* error_line_node_ = nullptr; + Ogre::SceneNode* relative_pose_axes_node_ = nullptr; + Ogre::SceneNode* text_node_ = nullptr; std::shared_ptr relative_pose_line_; std::shared_ptr error_line_; std::shared_ptr relative_pose_axes_; - MovableText * text_; + MovableText* text_; CovarianceVisualPtr covariance_; std::string source_; - float loss_scale_{-1.0}; - float min_brightness_{0.0}; + float loss_scale_{ -1.0 }; + float min_brightness_{ 0.0 }; Ogre::ColourValue error_line_color_; bool visible_; @@ -216,14 +212,16 @@ class RelativePose2DStampedConstraintVisual : public rviz_rendering::Object private: // Hide Object methods we don't want to expose // NOTE: Apparently we still need to define them... - void setScale(const Ogre::Vector3 &) override {} - void setColor(float, float, float, float) override {} - const Ogre::Vector3 & getPosition() override; - const Ogre::Quaternion & getOrientation() override; - - Ogre::ColourValue computeLossErrorLineColor( - const Ogre::ColourValue & color, - const float loss_scale); + void setScale(const Ogre::Vector3&) override + { + } + void setColor(float, float, float, float) override + { + } + const Ogre::Vector3& getPosition() override; + const Ogre::Quaternion& getOrientation() override; + + Ogre::ColourValue computeLossErrorLineColor(const Ogre::ColourValue& color, const float loss_scale); // Make RelativePose2DStampedConstraintProperty friend class so it create // RelativePose2DStampedConstraintVisual objects diff --git a/fuse_viz/include/fuse_viz/serialized_graph_display.hpp b/fuse_viz/include/fuse_viz/serialized_graph_display.hpp index 9f72f7a5a..bd8505a9b 100644 --- a/fuse_viz/include/fuse_viz/serialized_graph_display.hpp +++ b/fuse_viz/include/fuse_viz/serialized_graph_display.hpp @@ -63,10 +63,9 @@ class Pose2DStampedProperty; class RelativePose2DStampedConstraintProperty; /** - * @brief An rviz dispaly for fuse_msgs::msg::SerializedGraph messages. + * @brief An rviz display for fuse_msgs::msg::SerializedGraph messages. */ -class SerializedGraphDisplay - : public rviz_common::MessageFilterDisplay +class SerializedGraphDisplay : public rviz_common::MessageFilterDisplay { Q_OBJECT @@ -84,7 +83,7 @@ class SerializedGraphDisplay void onDisable() override; - void load(const rviz_common::Config & config) override; + void load(const rviz_common::Config& config) override; private Q_SLOTS: void updateShowVariables(); @@ -93,18 +92,16 @@ private Q_SLOTS: private: using ChangedByUUIDMap = std::unordered_map; using ConstraintByUUIDMap = - std::unordered_map, - fuse_core::uuid::hash>; + std::unordered_map, fuse_core::uuid::hash>; using ColorBySourceMap = std::unordered_map; - using ConstraintPropertyBySourceMap = std::map; + using ConstraintPropertyBySourceMap = std::map; using ConfigBySourceMap = std::unordered_map; void clear(); void processMessage(fuse_msgs::msg::SerializedGraph::ConstSharedPtr msg) override; - Ogre::SceneNode * root_node_; + Ogre::SceneNode* root_node_; ConstraintByUUIDMap constraint_visuals_; @@ -113,9 +110,9 @@ private Q_SLOTS: ChangedByUUIDMap variables_changed_map_; ChangedByUUIDMap constraints_changed_map_; - BoolProperty * show_variables_property_; - BoolProperty * show_constraints_property_; - Pose2DStampedProperty * variable_property_; + BoolProperty* show_variables_property_; + BoolProperty* show_constraints_property_; + Pose2DStampedProperty* variable_property_; ConstraintPropertyBySourceMap constraint_source_properties_; ConfigBySourceMap constraint_source_configs_; diff --git a/fuse_viz/package.xml b/fuse_viz/package.xml index c2073e9f7..058f80f33 100644 --- a/fuse_viz/package.xml +++ b/fuse_viz/package.xml @@ -25,9 +25,6 @@ rviz_rendering tf2_geometry_msgs - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/fuse_viz/src/mapped_covariance_property.cpp b/fuse_viz/src/mapped_covariance_property.cpp index 4de425c0d..771e79a4a 100644 --- a/fuse_viz/src/mapped_covariance_property.cpp +++ b/fuse_viz/src/mapped_covariance_property.cpp @@ -52,94 +52,85 @@ using rviz_common::properties::ColorProperty; using rviz_common::properties::EnumProperty; using rviz_common::properties::FloatProperty; -MappedCovarianceProperty::MappedCovarianceProperty( - const QString & name, bool default_value, const QString & description, - Property * parent, const char * changed_slot, QObject * receiver) -// NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of -// this constructor -: BoolProperty(name, default_value, description, parent) +MappedCovarianceProperty::MappedCovarianceProperty(const QString& name, bool default_value, const QString& description, + Property* parent, const char* changed_slot, QObject* receiver) + // NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of + // this constructor + : BoolProperty(name, default_value, description, parent) { - position_property_ = new BoolProperty( - "Position", true, "Whether or not to show the position part of covariances", - this, SLOT(updateVisibility())); + position_property_ = new BoolProperty("Position", true, "Whether or not to show the position part of covariances", + this, SLOT(updateVisibility())); position_property_->setDisableChildrenIfFalse(true); position_color_property_ = - new ColorProperty( - "Color", QColor(204, 51, 204), "Color to draw the position covariance ellipse.", - position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + new ColorProperty("Color", QColor(204, 51, 204), "Color to draw the position covariance ellipse.", + position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); - position_alpha_property_ = new FloatProperty( - "Alpha", 0.3f, "0 is fully transparent, 1.0 is fully opaque.", - position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + position_alpha_property_ = new FloatProperty("Alpha", 0.3f, "0 is fully transparent, 1.0 is fully opaque.", + position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); position_alpha_property_->setMin(0); position_alpha_property_->setMax(1); - position_scale_property_ = new FloatProperty( - "Scale", 1.0f, - "Scale factor to be applied to covariance ellipse. Corresponds to the " - "number of standard deviations to display", - position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + position_scale_property_ = new FloatProperty("Scale", 1.0f, + "Scale factor to be applied to covariance ellipse. Corresponds to the " + "number of standard deviations to display", + position_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); position_scale_property_->setMin(0); orientation_property_ = - new BoolProperty( - "Orientation", true, "Whether or not to show the orientation part of covariances", this, - SLOT(updateVisibility())); + new BoolProperty("Orientation", true, "Whether or not to show the orientation part of covariances", this, + SLOT(updateVisibility())); orientation_property_->setDisableChildrenIfFalse(true); orientation_frame_property_ = - new EnumProperty( - "Frame", "Local", "The frame used to display the orientation covariance.", - orientation_property_, - SLOT(updateOrientationFrame()), this); + new EnumProperty("Frame", "Local", "The frame used to display the orientation covariance.", orientation_property_, + SLOT(updateOrientationFrame()), this); orientation_frame_property_->addOption("Local", Local); orientation_frame_property_->addOption("Fixed", Fixed); - orientation_colorstyle_property_ = new EnumProperty( - "Color Style", "Unique", - "Style to color the orientation covariance: XYZ with same unique " - "color or following RGB order", - orientation_property_, SLOT(updateColorStyleChoice()), this); + orientation_colorstyle_property_ = new EnumProperty("Color Style", "Unique", + "Style to color the orientation covariance: XYZ with same unique " + "color or following RGB order", + orientation_property_, SLOT(updateColorStyleChoice()), this); orientation_colorstyle_property_->addOption("Unique", Unique); orientation_colorstyle_property_->addOption("RGB", RGB); orientation_color_property_ = - new ColorProperty( - "Color", QColor(255, 255, 127), "Color to draw the covariance ellipse.", orientation_property_, - SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + new ColorProperty("Color", QColor(255, 255, 127), "Color to draw the covariance ellipse.", orientation_property_, + SLOT(updateColorAndAlphaAndScaleAndOffset()), this); orientation_alpha_property_ = - new FloatProperty( - "Alpha", 0.5f, "0 is fully transparent, 1.0 is fully opaque.", orientation_property_, - SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + new FloatProperty("Alpha", 0.5f, "0 is fully transparent, 1.0 is fully opaque.", orientation_property_, + SLOT(updateColorAndAlphaAndScaleAndOffset()), this); orientation_alpha_property_->setMin(0); orientation_alpha_property_->setMax(1); orientation_offset_property_ = - new FloatProperty( - "Offset", 1.0f, - "For 3D poses is the distance where to position the ellipses representing orientation " - "covariance. For 2D poses is the height of the triangle representing the variance on yaw", - orientation_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + new FloatProperty("Offset", 1.0f, + "For 3D poses is the distance where to position the ellipses representing orientation " + "covariance. For 2D poses is the height of the triangle representing the variance on yaw", + orientation_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); orientation_offset_property_->setMin(0); orientation_scale_property_ = - new FloatProperty( - "Scale", 1.0f, - "Scale factor to be applied to orientation covariance shapes. Corresponds to the number of " - "standard deviations to display", - orientation_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); + new FloatProperty("Scale", 1.0f, + "Scale factor to be applied to orientation covariance shapes. Corresponds to the number of " + "standard deviations to display", + orientation_property_, SLOT(updateColorAndAlphaAndScaleAndOffset()), this); orientation_scale_property_->setMin(0); connect(this, SIGNAL(changed()), this, SLOT(updateVisibility())); // Connect changed() signal here instead of doing it through the initialization of BoolProperty(). // We do this here to make changed_slot be called _after_ updateVisibility() - if (changed_slot && (parent || receiver)) { - if (receiver) { + if (changed_slot && (parent || receiver)) + { + if (receiver) + { connect(this, SIGNAL(changed()), receiver, changed_slot); - } else { + } + else + { connect(this, SIGNAL(changed()), parent, changed_slot); } } @@ -160,13 +151,13 @@ void MappedCovarianceProperty::updateColorStyleChoice() void MappedCovarianceProperty::updateColorAndAlphaAndScaleAndOffset() { - for (const auto & entry : covariances_) { + for (const auto& entry : covariances_) + { updateColorAndAlphaAndScaleAndOffset(entry.second); } } -void MappedCovarianceProperty::updateColorAndAlphaAndScaleAndOffset( - const MappedCovarianceVisualPtr & visual) +void MappedCovarianceProperty::updateColorAndAlphaAndScaleAndOffset(const MappedCovarianceVisualPtr& visual) { float pos_alpha = position_alpha_property_->getFloat(); float pos_scale = position_scale_property_->getFloat(); @@ -177,10 +168,13 @@ void MappedCovarianceProperty::updateColorAndAlphaAndScaleAndOffset( float ori_alpha = orientation_alpha_property_->getFloat(); float ori_offset = orientation_offset_property_->getFloat(); float ori_scale = orientation_scale_property_->getFloat(); - if (orientation_colorstyle_property_->getOptionInt() == Unique) { + if (orientation_colorstyle_property_->getOptionInt() == Unique) + { QColor ori_color = orientation_color_property_->getColor(); visual->setOrientationColor(ori_color.redF(), ori_color.greenF(), ori_color.blueF(), ori_alpha); - } else { + } + else + { visual->setOrientationColorToRGB(ori_alpha); } visual->setOrientationOffset(ori_offset); @@ -189,17 +183,21 @@ void MappedCovarianceProperty::updateColorAndAlphaAndScaleAndOffset( void MappedCovarianceProperty::updateVisibility() { - for (const auto & entry : covariances_) { + for (const auto& entry : covariances_) + { updateVisibility(entry.second); } } -void MappedCovarianceProperty::updateVisibility(const MappedCovarianceVisualPtr & visual) +void MappedCovarianceProperty::updateVisibility(const MappedCovarianceVisualPtr& visual) { bool show_covariance = getBool(); - if (!show_covariance) { + if (!show_covariance) + { visual->setVisible(false); - } else { + } + else + { bool show_position_covariance = position_property_->getBool(); bool show_orientation_covariance = orientation_property_->getBool(); visual->setPositionVisible(show_position_covariance); @@ -209,18 +207,19 @@ void MappedCovarianceProperty::updateVisibility(const MappedCovarianceVisualPtr void MappedCovarianceProperty::updateOrientationFrame() { - for (const auto & entry : covariances_) { + for (const auto& entry : covariances_) + { updateOrientationFrame(entry.second); } } -void MappedCovarianceProperty::updateOrientationFrame(const MappedCovarianceVisualPtr & visual) +void MappedCovarianceProperty::updateOrientationFrame(const MappedCovarianceVisualPtr& visual) { bool use_rotating_frame = (orientation_frame_property_->getOptionInt() == Local); visual->setRotatingFrame(use_rotating_frame); } -void MappedCovarianceProperty::eraseVisual(const std::string & key) +void MappedCovarianceProperty::eraseVisual(const std::string& key) { covariances_.erase(key); } @@ -235,13 +234,12 @@ size_t MappedCovarianceProperty::sizeVisual() return covariances_.size(); } -MappedCovarianceProperty::MappedCovarianceVisualPtr MappedCovarianceProperty::createAndInsertVisual( - const std::string & key, Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node) +MappedCovarianceProperty::MappedCovarianceVisualPtr +MappedCovarianceProperty::createAndInsertVisual(const std::string& key, Ogre::SceneManager* scene_manager, + Ogre::SceneNode* parent_node) { bool use_rotating_frame = (orientation_frame_property_->getOptionInt() == Local); - MappedCovarianceVisualPtr visual(new MappedCovarianceVisual( - scene_manager, parent_node, - use_rotating_frame)); + MappedCovarianceVisualPtr visual(new MappedCovarianceVisual(scene_manager, parent_node, use_rotating_frame)); updateVisibility(visual); updateOrientationFrame(visual); updateColorAndAlphaAndScaleAndOffset(visual); diff --git a/fuse_viz/src/mapped_covariance_visual.cpp b/fuse_viz/src/mapped_covariance_visual.cpp index 95105693c..27e19f9d9 100644 --- a/fuse_viz/src/mapped_covariance_visual.cpp +++ b/fuse_viz/src/mapped_covariance_visual.cpp @@ -56,7 +56,7 @@ double deg2rad(double degrees) } // Local function to force the axis to be right handed for 3D. Taken from ecl_statistics -void makeRightHanded(Eigen::Matrix3d & eigenvectors, Eigen::Vector3d & eigenvalues) +void makeRightHanded(Eigen::Matrix3d& eigenvectors, Eigen::Vector3d& eigenvalues) { // Note that sorting of eigenvalues may end up with left-hand coordinate system. // So here we correctly sort it so that it does end up being righ-handed and normalised. @@ -67,18 +67,21 @@ void makeRightHanded(Eigen::Matrix3d & eigenvectors, Eigen::Vector3d & eigenvalu Eigen::Vector3d c2 = eigenvectors.block<3, 1>(0, 2); c2.normalize(); Eigen::Vector3d cc = c0.cross(c1); - if (cc.dot(c2) < 0) { + if (cc.dot(c2) < 0) + { eigenvectors << c1, c0, c2; double e = eigenvalues[0]; eigenvalues[0] = eigenvalues[1]; eigenvalues[1] = e; - } else { + } + else + { eigenvectors << c0, c1, c2; } } // Local function to force the axis to be right handed for 2D. Based on the one from ecl_statistics -void makeRightHanded(Eigen::Matrix2d & eigenvectors, Eigen::Vector2d & eigenvalues) +void makeRightHanded(Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues) { // Note that sorting of eigenvalues may end up with left-hand coordinate system. // So here we correctly sort it so that it does end up being righ-handed and normalised. @@ -91,19 +94,21 @@ void makeRightHanded(Eigen::Matrix2d & eigenvectors, Eigen::Vector2d & eigenvalu c1.head<2>() = eigenvectors.col(1); c1.normalize(); Eigen::Vector3d cc = c0.cross(c1); - if (cc[2] < 0) { + if (cc[2] < 0) + { eigenvectors << c1.head<2>(), c0.head<2>(); double e = eigenvalues[0]; eigenvalues[0] = eigenvalues[1]; eigenvalues[1] = e; - } else { + } + else + { eigenvectors << c0.head<2>(), c1.head<2>(); } } -void computeShapeScaleAndOrientation3D( - const Eigen::Matrix3d & covariance, Ogre::Vector3 & scale, - Ogre::Quaternion & orientation) +void computeShapeScaleAndOrientation3D(const Eigen::Matrix3d& covariance, Ogre::Vector3& scale, + Ogre::Quaternion& orientation) { Eigen::Vector3d eigenvalues(Eigen::Vector3d::Identity()); Eigen::Matrix3d eigenvectors(Eigen::Matrix3d::Zero()); @@ -112,14 +117,16 @@ void computeShapeScaleAndOrientation3D( // matrix FIXME: Should we use Eigen's pseudoEigenvectors() ? Eigen::SelfAdjointEigenSolver eigensolver(covariance); // Compute eigenvectors and eigenvalues - if (eigensolver.info() == Eigen::Success) { + if (eigensolver.info() == Eigen::Success) + { eigenvalues = eigensolver.eigenvalues(); eigenvectors = eigensolver.eigenvectors(); - } else { + } + else + { static rclcpp::Clock clock; - RCLCPP_WARN_THROTTLE( - rclcpp::get_logger("fuse"), clock, 1000, - "failed to compute eigen vectors/values for position. Is the covariance matrix correct?"); + RCLCPP_WARN_THROTTLE(rclcpp::get_logger("fuse"), clock, 1000, + "failed to compute eigen vectors/values for position. Is the covariance matrix correct?"); eigenvalues = Eigen::Vector3d::Zero(); // Setting the scale to zero will hide it on the screen eigenvectors = Eigen::Matrix3d::Identity(); } @@ -128,11 +135,9 @@ void computeShapeScaleAndOrientation3D( makeRightHanded(eigenvectors, eigenvalues); // Define the rotation - orientation.FromRotationMatrix( - Ogre::Matrix3( - eigenvectors(0, 0), eigenvectors(0, 1), eigenvectors(0, 2), - eigenvectors(1, 0), eigenvectors(1, 1), eigenvectors(1, 2), - eigenvectors(2, 0), eigenvectors(2, 1), eigenvectors(2, 2))); + orientation.FromRotationMatrix(Ogre::Matrix3(eigenvectors(0, 0), eigenvectors(0, 1), eigenvectors(0, 2), + eigenvectors(1, 0), eigenvectors(1, 1), eigenvectors(1, 2), + eigenvectors(2, 0), eigenvectors(2, 1), eigenvectors(2, 2))); // Define the scale. eigenvalues are the variances, so we take the sqrt to draw the standard // deviation @@ -148,9 +153,8 @@ enum Plane XY_PLANE // normal is z-axis }; -void computeShapeScaleAndOrientation2D( - const Eigen::Matrix2d & covariance, Ogre::Vector3 & scale, - Ogre::Quaternion & orientation, Plane plane = XY_PLANE) +void computeShapeScaleAndOrientation2D(const Eigen::Matrix2d& covariance, Ogre::Vector3& scale, + Ogre::Quaternion& orientation, Plane plane = XY_PLANE) { Eigen::Vector2d eigenvalues(Eigen::Vector2d::Identity()); Eigen::Matrix2d eigenvectors(Eigen::Matrix2d::Zero()); @@ -159,14 +163,16 @@ void computeShapeScaleAndOrientation2D( // matrix FIXME: Should we use Eigen's pseudoEigenvectors() ? Eigen::SelfAdjointEigenSolver eigensolver(covariance); // Compute eigenvectors and eigenvalues - if (eigensolver.info() == Eigen::Success) { + if (eigensolver.info() == Eigen::Success) + { eigenvalues = eigensolver.eigenvalues(); eigenvectors = eigensolver.eigenvectors(); - } else { + } + else + { static rclcpp::Clock clock; - RCLCPP_WARN_THROTTLE( - rclcpp::get_logger("fuse"), clock, 1000, - "failed to compute eigen vectors/values for position. Is the covariance matrix correct?"); + RCLCPP_WARN_THROTTLE(rclcpp::get_logger("fuse"), clock, 1000, + "failed to compute eigen vectors/values for position. Is the covariance matrix correct?"); eigenvalues = Eigen::Vector2d::Zero(); // Setting the scale to zero will hide it on the screen eigenvectors = Eigen::Matrix2d::Identity(); } @@ -177,29 +183,28 @@ void computeShapeScaleAndOrientation2D( // Define the rotation and scale of the plane // The Eigenvalues are the variances. The scales are two times the standard // deviation. The scale of the missing dimension is set to zero. - if (plane == YZ_PLANE) { + if (plane == YZ_PLANE) + { orientation.FromRotationMatrix( - Ogre::Matrix3( - 1, 0, 0, 0, eigenvectors(0, 0), eigenvectors(0, 1), 0, eigenvectors(1, 0), - eigenvectors(1, 1))); + Ogre::Matrix3(1, 0, 0, 0, eigenvectors(0, 0), eigenvectors(0, 1), 0, eigenvectors(1, 0), eigenvectors(1, 1))); scale.x = 0; scale.y = 2 * std::sqrt(eigenvalues[0]); scale.z = 2 * std::sqrt(eigenvalues[1]); - } else if (plane == XZ_PLANE) { + } + else if (plane == XZ_PLANE) + { orientation.FromRotationMatrix( - Ogre::Matrix3( - eigenvectors(0, 0), 0, eigenvectors(0, 1), 0, 1, 0, eigenvectors(1, 0), 0, - eigenvectors(1, 1))); + Ogre::Matrix3(eigenvectors(0, 0), 0, eigenvectors(0, 1), 0, 1, 0, eigenvectors(1, 0), 0, eigenvectors(1, 1))); scale.x = 2 * std::sqrt(eigenvalues[0]); scale.y = 0; scale.z = 2 * std::sqrt(eigenvalues[1]); - } else { // plane == XY_PLANE + } + else + { // plane == XY_PLANE orientation.FromRotationMatrix( - Ogre::Matrix3( - eigenvectors(0, 0), eigenvectors(0, 1), 0, eigenvectors(1, 0), - eigenvectors(1, 1), 0, 0, 0, 1)); + Ogre::Matrix3(eigenvectors(0, 0), eigenvectors(0, 1), 0, eigenvectors(1, 0), eigenvectors(1, 1), 0, 0, 0, 1)); scale.x = 2 * std::sqrt(eigenvalues[0]); scale.y = 2 * std::sqrt(eigenvalues[1]); @@ -207,10 +212,11 @@ void computeShapeScaleAndOrientation2D( } } -void radianScaleToMetricScaleBounded(Ogre::Real & radian_scale, float max_degrees) +void radianScaleToMetricScaleBounded(Ogre::Real& radian_scale, float max_degrees) { radian_scale /= 2.0; - if (radian_scale > deg2rad(max_degrees)) { + if (radian_scale > deg2rad(max_degrees)) + { radian_scale = deg2rad(max_degrees); } radian_scale = 2.0 * tan(radian_scale); @@ -220,12 +226,10 @@ void radianScaleToMetricScaleBounded(Ogre::Real & radian_scale, float max_degree const float MappedCovarianceVisual::max_degrees = 89.0; -MappedCovarianceVisual::MappedCovarianceVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - bool is_local_rotation, bool is_visible, float pos_scale, - float ori_scale, float ori_offset) -: Object(scene_manager), local_rotation_(is_local_rotation), pose_2d_(false), orientation_visible_( - is_visible) +MappedCovarianceVisual::MappedCovarianceVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + bool is_local_rotation, bool is_visible, float pos_scale, + float ori_scale, float ori_offset) + : Object(scene_manager), local_rotation_(is_local_rotation), pose_2d_(false), orientation_visible_(is_visible) { // Main node of the visual root_node_ = parent_node->createChildSceneNode(); @@ -236,34 +240,36 @@ MappedCovarianceVisual::MappedCovarianceVisual( position_scale_node_ = fixed_orientation_node_->createChildSceneNode(); // Node to be oriented and scaled from the message's covariance position_node_ = position_scale_node_->createChildSceneNode(); - position_shape_ = new rviz_rendering::Shape( - rviz_rendering::Shape::Sphere, scene_manager_, - position_node_); + position_shape_ = new rviz_rendering::Shape(rviz_rendering::Shape::Sphere, scene_manager_, position_node_); // Node to scale the orientation part of the covariance. May be attached to both the local (root) // node or the fixed frame node. May be re-attached later by setRotatingFrame() - if (local_rotation_) { + if (local_rotation_) + { orientation_root_node_ = root_node_->createChildSceneNode(); - } else { + } + else + { orientation_root_node_ = fixed_orientation_node_->createChildSceneNode(); } - for (int i = 0; i < kNumOriShapes; i++) { + for (int i = 0; i < kNumOriShapes; i++) + { // Node to position and orient the shape along the axis. One for each axis. orientation_offset_node_[i] = orientation_root_node_->createChildSceneNode(); // Does not inherit scale from the parent. This is needed to keep the cylinders with the same // height. The scale is set by setOrientationScale() orientation_offset_node_[i]->setInheritScale(false); - if (i != kYaw2D) { - orientation_shape_[i] = new rviz_rendering::Shape( - rviz_rendering::Shape::Cylinder, - scene_manager_, - orientation_offset_node_[i]); - } else { - orientation_shape_[i] = new rviz_rendering::Shape( - rviz_rendering::Shape::Cone, scene_manager_, - orientation_offset_node_[i]); + if (i != kYaw2D) + { + orientation_shape_[i] = + new rviz_rendering::Shape(rviz_rendering::Shape::Cylinder, scene_manager_, orientation_offset_node_[i]); + } + else + { + orientation_shape_[i] = + new rviz_rendering::Shape(rviz_rendering::Shape::Cone, scene_manager_, orientation_offset_node_[i]); } // Initialize all current scales to 0 @@ -273,21 +279,14 @@ MappedCovarianceVisual::MappedCovarianceVisual( // Position the cylindes at position 1.0 in the respective axis, and perpendicular to the axis. // x-axis (roll) orientation_offset_node_[kRoll]->setPosition(Ogre::Vector3::UNIT_X); - orientation_offset_node_[kRoll]->setOrientation( - Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_X) * - Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_Z)); + orientation_offset_node_[kRoll]->setOrientation(Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_X) * + Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_Z)); // y-axis (pitch) orientation_offset_node_[kPitch]->setPosition(Ogre::Vector3(Ogre::Vector3::UNIT_Y)); - orientation_offset_node_[kPitch]->setOrientation( - Ogre::Quaternion( - Ogre::Degree(90), - Ogre::Vector3::UNIT_Y)); + orientation_offset_node_[kPitch]->setOrientation(Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_Y)); // z-axis (yaw) orientation_offset_node_[kYaw]->setPosition(Ogre::Vector3(Ogre::Vector3::UNIT_Z)); - orientation_offset_node_[kYaw]->setOrientation( - Ogre::Quaternion( - Ogre::Degree(90), - Ogre::Vector3::UNIT_X)); + orientation_offset_node_[kYaw]->setOrientation(Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_X)); // z-axis (yaw 2D) // NOTE: rviz use a cone defined by the file rviz/ogre_media/models/rviz_cone.mesh, and it's // origin is not at the top of the cone. Since we want the top to be at the origin of @@ -299,10 +298,7 @@ MappedCovarianceVisual::MappedCovarianceVisual( // something like a 2D "pie slice" and use it instead of the cone. static const double cone_origin_to_top = 0.49115; orientation_offset_node_[kYaw2D]->setPosition(cone_origin_to_top * Ogre::Vector3::UNIT_X); - orientation_offset_node_[kYaw2D]->setOrientation( - Ogre::Quaternion( - Ogre::Degree(90), - Ogre::Vector3::UNIT_Z)); + orientation_offset_node_[kYaw2D]->setOrientation(Ogre::Quaternion(Ogre::Degree(90), Ogre::Vector3::UNIT_Z)); // set initial visibility and scale // root node is always visible. The visibility will be updated on its childs. @@ -317,7 +313,8 @@ MappedCovarianceVisual::~MappedCovarianceVisual() delete position_shape_; scene_manager_->destroySceneNode(position_node_); - for (int i = 0; i < kNumOriShapes; i++) { + for (int i = 0; i < kNumOriShapes; i++) + { delete orientation_shape_[i]; scene_manager_->destroySceneNode(orientation_offset_node_[i]); } @@ -327,20 +324,25 @@ MappedCovarianceVisual::~MappedCovarianceVisual() scene_manager_->destroySceneNode(root_node_); } -void MappedCovarianceVisual::setCovariance(const geometry_msgs::msg::PoseWithCovariance & pose) +void MappedCovarianceVisual::setCovariance(const geometry_msgs::msg::PoseWithCovariance& pose) { // check for NaN in covariance - for (unsigned i = 0; i < 3; ++i) { - if (std::isnan(pose.covariance[i])) { + for (unsigned i = 0; i < 3; ++i) + { + if (std::isnan(pose.covariance[i])) + { static rclcpp::Clock clock; RCLCPP_WARN_THROTTLE(rclcpp::get_logger("fuse"), clock, 1000, "covariance contains NaN"); return; } } - if (pose.covariance[14] <= 0 && pose.covariance[21] <= 0 && pose.covariance[28] <= 0) { + if (pose.covariance[14] <= 0 && pose.covariance[21] <= 0 && pose.covariance[28] <= 0) + { pose_2d_ = true; - } else { + } + else + { pose_2d_ = false; } @@ -357,48 +359,52 @@ void MappedCovarianceVisual::setCovariance(const geometry_msgs::msg::PoseWithCov Eigen::Map> covariance(pose.covariance.data()); updatePosition(covariance); - if (!pose_2d_) { + if (!pose_2d_) + { updateOrientation(covariance, kRoll); updateOrientation(covariance, kPitch); updateOrientation(covariance, kYaw); - } else { + } + else + { updateOrientation(covariance, kYaw2D); } } -void MappedCovarianceVisual::updatePosition(const Eigen::Matrix6d & covariance) +void MappedCovarianceVisual::updatePosition(const Eigen::Matrix6d& covariance) { // Compute shape and orientation for the position part of covariance Ogre::Vector3 shape_scale; Ogre::Quaternion shape_orientation; - if (pose_2d_) { - computeShapeScaleAndOrientation2D( - covariance.topLeftCorner<2, - 2>(), shape_scale, shape_orientation, XY_PLANE); + if (pose_2d_) + { + computeShapeScaleAndOrientation2D(covariance.topLeftCorner<2, 2>(), shape_scale, shape_orientation, XY_PLANE); // Make the scale in z minimal for better visualization shape_scale.z = 0.001; - } else { - computeShapeScaleAndOrientation3D( - covariance.topLeftCorner<3, - 3>(), shape_scale, shape_orientation); + } + else + { + computeShapeScaleAndOrientation3D(covariance.topLeftCorner<3, 3>(), shape_scale, shape_orientation); } // Rotate and scale the position scene node position_node_->setOrientation(shape_orientation); - if (!shape_scale.isNaN()) { + if (!shape_scale.isNaN()) + { position_node_->setScale(shape_scale); - } else { - RCLCPP_WARN_STREAM( - rclcpp::get_logger( - "fuse"), "position shape_scale contains NaN: " << shape_scale); + } + else + { + RCLCPP_WARN_STREAM(rclcpp::get_logger("fuse"), "position shape_scale contains NaN: " << shape_scale); } } -void MappedCovarianceVisual::updateOrientation(const Eigen::Matrix6d & covariance, ShapeIndex index) +void MappedCovarianceVisual::updateOrientation(const Eigen::Matrix6d& covariance, ShapeIndex index) { Ogre::Vector3 shape_scale; Ogre::Quaternion shape_orientation; // Compute shape and orientation for the orientation shape - if (pose_2d_) { + if (pose_2d_) + { // We should only enter on this scope if the index is kYaw2D assert(index == kYaw2D); // 2D poses only depend on yaw. @@ -415,16 +421,23 @@ void MappedCovarianceVisual::updateOrientation(const Eigen::Matrix6d & covarianc // So we need to convert it to the linear scale of the shape using tan(). // Also, we bound the maximum std radianScaleToMetricScaleBounded(shape_scale.x, max_degrees); - } else { + } + else + { assert(index != kYaw2D); // Get the correct sub-matrix based on the index Eigen::Matrix2d covarianceAxis; - if (index == kRoll) { + if (index == kRoll) + { covarianceAxis = covariance.bottomRightCorner<2, 2>(); - } else if (index == kPitch) { + } + else if (index == kPitch) + { covarianceAxis << covariance(3, 3), covariance(3, 5), covariance(5, 3), covariance(5, 5); - } else if (index == kYaw) { + } + else if (index == kYaw) + { covarianceAxis = covariance.block<2, 2>(3, 3); } @@ -447,12 +460,13 @@ void MappedCovarianceVisual::updateOrientation(const Eigen::Matrix6d & covarianc // Rotate and scale the scene node of the orientation part orientation_shape_[index]->setOrientation(shape_orientation); - if (!shape_scale.isNaN()) { + if (!shape_scale.isNaN()) + { orientation_shape_[index]->setScale(shape_scale); - } else { - RCLCPP_WARN_STREAM( - rclcpp::get_logger( - "fuse"), "orientation shape_scale contains NaN: " << shape_scale); + } + else + { + RCLCPP_WARN_STREAM(rclcpp::get_logger("fuse"), "orientation shape_scale contains NaN: " << shape_scale); } } @@ -464,9 +478,12 @@ void MappedCovarianceVisual::setScales(float pos_scale, float ori_scale) void MappedCovarianceVisual::setPositionScale(float pos_scale) { - if (pose_2d_) { + if (pose_2d_) + { position_scale_node_->setScale(pos_scale, pos_scale, 1.0); - } else { + } + else + { position_scale_node_->setScale(pos_scale, pos_scale, pos_scale); } } @@ -477,14 +494,18 @@ void MappedCovarianceVisual::setOrientationOffset(float ori_offset) orientation_root_node_->setScale(ori_offset, ori_offset, ori_offset); // The scale the offset_nodes as well so the displayed shape represents a 1-sigma // standard deviation when displayed with an scale of 1.0 - // NOTE: We only want to change the scales of the dimentions that represent the + // NOTE: We only want to change the scales of the dimensions that represent the // orientation covariance. The other dimensions are set to 1.0. - for (int i = 0; i < kNumOriShapes; i++) { - if (i == kYaw2D) { + for (int i = 0; i < kNumOriShapes; i++) + { + if (i == kYaw2D) + { // For 2D, the angle is only encoded on x, but we also scale on y to put the top of the cone // at the pose origin orientation_offset_node_[i]->setScale(ori_offset, ori_offset, 1.0); - } else { + } + else + { // For 3D, the angle covariance is encoded on x and z dimensions orientation_offset_node_[i]->setScale(ori_offset, 1.0, ori_offset); } @@ -497,16 +518,20 @@ void MappedCovarianceVisual::setOrientationScale(float ori_scale) // convert it to meters and apply to the shape scale. Note we have different invariant // scales in the 3D and in 2D. current_ori_scale_factor_ = ori_scale; - for (int i = 0; i < kNumOriShapes; i++) { + for (int i = 0; i < kNumOriShapes; i++) + { // Recover the last computed scale Ogre::Vector3 shape_scale = current_ori_scale_[i]; - if (i == kYaw2D) { + if (i == kYaw2D) + { // Changes in scale in 2D only affects the x dimension // Apply the current scale factor shape_scale.x *= current_ori_scale_factor_; // Convert from radians to meters radianScaleToMetricScaleBounded(shape_scale.x, max_degrees); - } else { + } + else + { // Changes in scale in 3D only affects the x and z dimensions // Apply the current scale factor shape_scale.x *= current_ori_scale_factor_; @@ -520,14 +545,15 @@ void MappedCovarianceVisual::setOrientationScale(float ori_scale) } } -void MappedCovarianceVisual::setPositionColor(const Ogre::ColourValue & c) +void MappedCovarianceVisual::setPositionColor(const Ogre::ColourValue& c) { position_shape_->setColor(c); } -void MappedCovarianceVisual::setOrientationColor(const Ogre::ColourValue & c) +void MappedCovarianceVisual::setOrientationColor(const Ogre::ColourValue& c) { - for (int i = 0; i < kNumOriShapes; i++) { + for (int i = 0; i < kNumOriShapes; i++) + { orientation_shape_[i]->setColor(c); } } @@ -550,20 +576,21 @@ void MappedCovarianceVisual::setOrientationColor(float r, float g, float b, floa setOrientationColor(Ogre::ColourValue(r, g, b, a)); } -const Ogre::Vector3 & MappedCovarianceVisual::getPositionCovarianceScale() +const Ogre::Vector3& MappedCovarianceVisual::getPositionCovarianceScale() { return position_node_->getScale(); } -const Ogre::Quaternion & MappedCovarianceVisual::getPositionCovarianceOrientation() +const Ogre::Quaternion& MappedCovarianceVisual::getPositionCovarianceOrientation() { return position_node_->getOrientation(); } -void MappedCovarianceVisual::setUserData(const Ogre::Any & data) +void MappedCovarianceVisual::setUserData(const Ogre::Any& data) { position_shape_->setUserData(data); - for (int i = 0; i < kNumOriShapes; i++) { + for (int i = 0; i < kNumOriShapes; i++) + { orientation_shape_[i]->setUserData(data); } } @@ -593,42 +620,46 @@ void MappedCovarianceVisual::updateOrientationVisibility() orientation_offset_node_[kYaw2D]->setVisible(orientation_visible_ && pose_2d_); } -const Ogre::Vector3 & MappedCovarianceVisual::getPosition() +const Ogre::Vector3& MappedCovarianceVisual::getPosition() { return position_node_->getPosition(); } -const Ogre::Quaternion & MappedCovarianceVisual::getOrientation() +const Ogre::Quaternion& MappedCovarianceVisual::getOrientation() { return position_node_->getOrientation(); } -void MappedCovarianceVisual::setPosition(const Ogre::Vector3 & position) +void MappedCovarianceVisual::setPosition(const Ogre::Vector3& position) { root_node_->setPosition(position); } -void MappedCovarianceVisual::setOrientation(const Ogre::Quaternion & orientation) +void MappedCovarianceVisual::setOrientation(const Ogre::Quaternion& orientation) { root_node_->setOrientation(orientation); } void MappedCovarianceVisual::setRotatingFrame(bool is_local_rotation) { - if (local_rotation_ == is_local_rotation) { + if (local_rotation_ == is_local_rotation) + { return; } local_rotation_ = is_local_rotation; - if (local_rotation_) { + if (local_rotation_) + { root_node_->addChild(fixed_orientation_node_->removeChild(orientation_root_node_->getName())); - } else { + } + else + { fixed_orientation_node_->addChild(root_node_->removeChild(orientation_root_node_->getName())); } } -rviz_rendering::Shape * MappedCovarianceVisual::getOrientationShape(ShapeIndex index) +rviz_rendering::Shape* MappedCovarianceVisual::getOrientationShape(ShapeIndex index) { return orientation_shape_[index]; } diff --git a/fuse_viz/src/pose_2d_stamped_property.cpp b/fuse_viz/src/pose_2d_stamped_property.cpp index 226751336..378c764ad 100644 --- a/fuse_viz/src/pose_2d_stamped_property.cpp +++ b/fuse_viz/src/pose_2d_stamped_property.cpp @@ -51,55 +51,45 @@ using rviz_common::properties::ColorProperty; using rviz_common::properties::FloatProperty; using rviz_common::properties::Property; -Pose2DStampedProperty::Pose2DStampedProperty( - const QString & name, bool default_value, const QString & description, - Property * parent, const char * changed_slot, QObject * receiver) -// NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of -// this constructor -: BoolProperty(name, default_value, description, parent) +Pose2DStampedProperty::Pose2DStampedProperty(const QString& name, bool default_value, const QString& description, + Property* parent, const char* changed_slot, QObject* receiver) + // NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of + // this constructor + : BoolProperty(name, default_value, description, parent) { - color_property_ = new ColorProperty( - "Color", QColor(255, 0, 0), "Color to draw the variable sphere.", this, - SLOT(updateSphereColorAlpha())); + color_property_ = new ColorProperty("Color", QColor(255, 0, 0), "Color to draw the variable sphere.", this, + SLOT(updateSphereColorAlpha())); sphere_alpha_property_ = - new FloatProperty( - "Sphere Alpha", 1.0, "Alpha of variable sphere.", this, - SLOT(updateSphereColorAlpha())); + new FloatProperty("Sphere Alpha", 1.0, "Alpha of variable sphere.", this, SLOT(updateSphereColorAlpha())); sphere_alpha_property_->setMin(0.0); sphere_alpha_property_->setMax(1.0); - axes_alpha_property_ = - new FloatProperty( - "Axes Alpha", 0.0, "Alpha of variable axes.", this, - SLOT(updateAxesAlpha())); + axes_alpha_property_ = new FloatProperty("Axes Alpha", 0.0, "Alpha of variable axes.", this, SLOT(updateAxesAlpha())); axes_alpha_property_->setMin(0.0); axes_alpha_property_->setMax(1.0); - scale_property_ = new FloatProperty( - "Scale", 1.0, "Scale of variable sphere and axes.", this, SLOT( - updateScale())); + scale_property_ = new FloatProperty("Scale", 1.0, "Scale of variable sphere and axes.", this, SLOT(updateScale())); scale_property_->setMin(0.0); show_text_property_ = - new BoolProperty( - "Show Text", false, "Show variable type and UUID.", this, SLOT( - updateShowText())); - - text_scale_property_ = - new FloatProperty( - "Text Scale", 1.0, "Scale of variable text.", this, - SLOT(updateTextScale())); + new BoolProperty("Show Text", false, "Show variable type and UUID.", this, SLOT(updateShowText())); + + text_scale_property_ = new FloatProperty("Text Scale", 1.0, "Scale of variable text.", this, SLOT(updateTextScale())); text_scale_property_->setMin(0.0); connect(this, SIGNAL(changed()), this, SLOT(updateVisibility())); // Connect changed() signal here instead of doing it through the initialization of BoolProperty(). // We do this here to make changed_slot be called _after_ updateVisibility() - if (changed_slot && (parent || receiver)) { - if (receiver) { + if (changed_slot && (parent || receiver)) + { + if (receiver) + { connect(this, SIGNAL(changed()), receiver, changed_slot); - } else { + } + else + { connect(this, SIGNAL(changed()), parent, changed_slot); } } @@ -107,16 +97,19 @@ Pose2DStampedProperty::Pose2DStampedProperty( setDisableChildrenIfFalse(true); } -Pose2DStampedProperty::VisualPtr Pose2DStampedProperty::createAndInsertOrUpdateVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation) +Pose2DStampedProperty::VisualPtr +Pose2DStampedProperty::createAndInsertOrUpdateVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation) { - auto & visual = variables_[position.uuid()]; + auto& visual = variables_[position.uuid()]; - if (visual) { + if (visual) + { visual->setPose2DStamped(position, orientation); - } else { + } + else + { visual.reset(new Visual(scene_manager, parent_node, position, orientation)); visual->setPose2DStamped(position, orientation); @@ -132,7 +125,7 @@ Pose2DStampedProperty::VisualPtr Pose2DStampedProperty::createAndInsertOrUpdateV return visual; } -void Pose2DStampedProperty::eraseVisual(const fuse_core::UUID & uuid) +void Pose2DStampedProperty::eraseVisual(const fuse_core::UUID& uuid) { variables_.erase(uuid); } @@ -144,76 +137,80 @@ void Pose2DStampedProperty::clearVisual() void Pose2DStampedProperty::updateVisibility() { - for (auto & entry : variables_) { + for (auto& entry : variables_) + { updateVisibility(entry.second); } } void Pose2DStampedProperty::updateAxesAlpha() { - for (auto & entry : variables_) { + for (auto& entry : variables_) + { updateAxesAlpha(entry.second); } } void Pose2DStampedProperty::updateScale() { - for (auto & entry : variables_) { + for (auto& entry : variables_) + { updateScale(entry.second); } } void Pose2DStampedProperty::updateShowText() { - for (auto & entry : variables_) { + for (auto& entry : variables_) + { updateShowText(entry.second); } } void Pose2DStampedProperty::updateSphereColorAlpha() { - for (auto & entry : variables_) { + for (auto& entry : variables_) + { updateSphereColorAlpha(entry.second); } } void Pose2DStampedProperty::updateTextScale() { - for (auto & entry : variables_) { + for (auto& entry : variables_) + { updateTextScale(entry.second); } } -void Pose2DStampedProperty::updateAxesAlpha(const VisualPtr & variable) +void Pose2DStampedProperty::updateAxesAlpha(const VisualPtr& variable) { variable->setAxesAlpha(axes_alpha_property_->getFloat()); } -void Pose2DStampedProperty::updateScale(const VisualPtr & variable) +void Pose2DStampedProperty::updateScale(const VisualPtr& variable) { - variable->setScale(Ogre::Vector3{scale_property_->getFloat()}); // NOLINT(whitespace/braces) + variable->setScale(Ogre::Vector3{ scale_property_->getFloat() }); // NOLINT(whitespace/braces) } -void Pose2DStampedProperty::updateShowText(const VisualPtr & variable) +void Pose2DStampedProperty::updateShowText(const VisualPtr& variable) { variable->setTextVisible(show_text_property_->getBool()); } -void Pose2DStampedProperty::updateSphereColorAlpha(const VisualPtr & variable) +void Pose2DStampedProperty::updateSphereColorAlpha(const VisualPtr& variable) { const auto color = color_property_->getColor(); - variable->setSphereColor( - color.redF(), color.greenF(), - color.blueF(), sphere_alpha_property_->getFloat()); + variable->setSphereColor(color.redF(), color.greenF(), color.blueF(), sphere_alpha_property_->getFloat()); } -void Pose2DStampedProperty::updateTextScale(const VisualPtr & variable) +void Pose2DStampedProperty::updateTextScale(const VisualPtr& variable) { - variable->setTextScale(Ogre::Vector3{text_scale_property_->getFloat()}); // NOLINT + variable->setTextScale(Ogre::Vector3{ text_scale_property_->getFloat() }); // NOLINT } -void Pose2DStampedProperty::updateVisibility(const VisualPtr & variable) +void Pose2DStampedProperty::updateVisibility(const VisualPtr& variable) { const auto visible = getBool(); diff --git a/fuse_viz/src/pose_2d_stamped_visual.cpp b/fuse_viz/src/pose_2d_stamped_visual.cpp index 19f4a1773..f9e2c4f3b 100644 --- a/fuse_viz/src/pose_2d_stamped_visual.cpp +++ b/fuse_viz/src/pose_2d_stamped_visual.cpp @@ -50,17 +50,14 @@ namespace fuse_viz { using rviz_rendering::MovableText; -Pose2DStampedVisual::Pose2DStampedVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation, const bool visible) -: Object(scene_manager), root_node_(parent_node->createChildSceneNode()), visible_(visible) +Pose2DStampedVisual::Pose2DStampedVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation, const bool visible) + : Object(scene_manager), root_node_(parent_node->createChildSceneNode()), visible_(visible) { // Create sphere: sphere_node_ = root_node_->createChildSceneNode(); - sphere_ = std::make_shared( - rviz_rendering::Shape::Sphere, scene_manager_, - sphere_node_); + sphere_ = std::make_shared(rviz_rendering::Shape::Sphere, scene_manager_, sphere_node_); setSphereColor(1.0, 0.0, 0.0, 1.0); // Create axes: @@ -69,7 +66,7 @@ Pose2DStampedVisual::Pose2DStampedVisual( // Create text: const auto caption = position.type() + "::" + fuse_core::uuid::to_string(position.uuid()) + '\n' + - orientation.type() + "::" + fuse_core::uuid::to_string(orientation.uuid()); + orientation.type() + "::" + fuse_core::uuid::to_string(orientation.uuid()); text_ = new MovableText(caption); text_->setCaption(caption); text_->setTextAlignment(MovableText::H_CENTER, MovableText::V_ABOVE); @@ -96,14 +93,13 @@ Pose2DStampedVisual::~Pose2DStampedVisual() scene_manager_->destroySceneNode(root_node_); } -void Pose2DStampedVisual::setPose2DStamped( - const fuse_variables::Position2DStamped & position, - const fuse_variables::Orientation2DStamped & orientation) +void Pose2DStampedVisual::setPose2DStamped(const fuse_variables::Position2DStamped& position, + const fuse_variables::Orientation2DStamped& orientation) { setPose2DStamped(toOgre(position), toOgre(orientation)); } -void Pose2DStampedVisual::setUserData(const Ogre::Any & data) +void Pose2DStampedVisual::setUserData(const Ogre::Any& data) { axes_->setUserData(data); sphere_->setUserData(data); @@ -116,31 +112,25 @@ void Pose2DStampedVisual::setSphereColor(const float r, const float g, const flo void Pose2DStampedVisual::setAxesAlpha(const float alpha) { - static const auto & default_x_color_ = axes_->getDefaultXColor(); - static const auto & default_y_color_ = axes_->getDefaultYColor(); - static const auto & default_z_color_ = axes_->getDefaultZColor(); - - axes_->setXColor( - Ogre::ColourValue( - default_x_color_.r, default_x_color_.g, default_x_color_.b, - alpha)); // NOLINT - axes_->setYColor( - Ogre::ColourValue( - default_y_color_.r, default_y_color_.g, default_y_color_.b, - alpha)); // NOLINT - axes_->setZColor( - Ogre::ColourValue( - default_z_color_.r, default_z_color_.g, default_z_color_.b, - alpha)); // NOLINT + static const auto& default_x_color_ = axes_->getDefaultXColor(); + static const auto& default_y_color_ = axes_->getDefaultYColor(); + static const auto& default_z_color_ = axes_->getDefaultZColor(); + + axes_->setXColor(Ogre::ColourValue(default_x_color_.r, default_x_color_.g, default_x_color_.b, + alpha)); // NOLINT + axes_->setYColor(Ogre::ColourValue(default_y_color_.r, default_y_color_.g, default_y_color_.b, + alpha)); // NOLINT + axes_->setZColor(Ogre::ColourValue(default_z_color_.r, default_z_color_.g, default_z_color_.b, + alpha)); // NOLINT } -void Pose2DStampedVisual::setScale(const Ogre::Vector3 & scale) +void Pose2DStampedVisual::setScale(const Ogre::Vector3& scale) { sphere_->setScale(scale); axes_->setScale(scale); } -void Pose2DStampedVisual::setTextScale(const Ogre::Vector3 & scale) +void Pose2DStampedVisual::setTextScale(const Ogre::Vector3& scale) { text_node_->setScale(scale); } @@ -156,9 +146,7 @@ void Pose2DStampedVisual::setVisible(const bool visible) axes_node_->setVisible(visible); } -void Pose2DStampedVisual::setPose2DStamped( - const Ogre::Vector3 & position, - const Ogre::Quaternion & orientation) +void Pose2DStampedVisual::setPose2DStamped(const Ogre::Vector3& position, const Ogre::Quaternion& orientation) { axes_->setPosition(position); axes_->setOrientation(orientation); @@ -166,22 +154,22 @@ void Pose2DStampedVisual::setPose2DStamped( text_node_->setPosition(position); } -const Ogre::Vector3 & Pose2DStampedVisual::getPosition() +const Ogre::Vector3& Pose2DStampedVisual::getPosition() { return root_node_->getPosition(); } -const Ogre::Quaternion & Pose2DStampedVisual::getOrientation() +const Ogre::Quaternion& Pose2DStampedVisual::getOrientation() { return root_node_->getOrientation(); } -void Pose2DStampedVisual::setPosition(const Ogre::Vector3 & position) +void Pose2DStampedVisual::setPosition(const Ogre::Vector3& position) { root_node_->setPosition(position); } -void Pose2DStampedVisual::setOrientation(const Ogre::Quaternion & orientation) +void Pose2DStampedVisual::setOrientation(const Ogre::Quaternion& orientation) { root_node_->setOrientation(orientation); } diff --git a/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp b/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp index 7b3381d98..1816b5af2 100644 --- a/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp +++ b/fuse_viz/src/relative_pose_2d_stamped_constraint_property.cpp @@ -54,81 +54,71 @@ using rviz_common::properties::FloatProperty; using rviz_common::properties::Property; RelativePose2DStampedConstraintProperty::RelativePose2DStampedConstraintProperty( - const QString & name, bool default_value, const QString & description, Property * parent, - const char * changed_slot, QObject * receiver) -// NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of -// this constructor -: BoolProperty(name, default_value, description, parent) + const QString& name, bool default_value, const QString& description, Property* parent, const char* changed_slot, + QObject* receiver) + // NOTE: changed_slot and receiver aren't passed to BoolProperty here, but initialized at the end of + // this constructor + : BoolProperty(name, default_value, description, parent) { color_property_ = - new ColorProperty( - "Color", QColor(255, 255, 255), "Color to draw the constraint relative pose and error lines.", - this, SLOT(updateColor())); + new ColorProperty("Color", QColor(255, 255, 255), "Color to draw the constraint relative pose and error lines.", + this, SLOT(updateColor())); - relative_pose_axes_alpha_property_ = new FloatProperty( - "Axes Alpha", 0.0, "Alpha of constraint relative pose axes.", - this, SLOT(updateRelativePoseAxesAlpha())); + relative_pose_axes_alpha_property_ = new FloatProperty("Axes Alpha", 0.0, "Alpha of constraint relative pose axes.", + this, SLOT(updateRelativePoseAxesAlpha())); relative_pose_axes_alpha_property_->setMin(0.0); relative_pose_axes_alpha_property_->setMax(1.0); - relative_pose_axes_scale_property_ = new FloatProperty( - "Axes Scale", 1.0, "Scale of constraint relative pose axes.", - this, SLOT(updateRelativePoseAxesScale())); + relative_pose_axes_scale_property_ = new FloatProperty("Axes Scale", 1.0, "Scale of constraint relative pose axes.", + this, SLOT(updateRelativePoseAxesScale())); relative_pose_axes_scale_property_->setMin(0.0); - relative_pose_line_alpha_property_ = new FloatProperty( - "Line Alpha", 1.0, "Alpha of constraint relative pose line.", - this, SLOT(updateRelativePoseLineAlpha())); + relative_pose_line_alpha_property_ = new FloatProperty("Line Alpha", 1.0, "Alpha of constraint relative pose line.", + this, SLOT(updateRelativePoseLineAlpha())); relative_pose_line_alpha_property_->setMin(0.0); relative_pose_line_alpha_property_->setMax(1.0); relative_pose_line_width_property_ = new FloatProperty( - "Line Width", 0.1, "Line width of constraint relative pose line.", this, - SLOT(updateRelativePoseLineWidth())); + "Line Width", 0.1, "Line width of constraint relative pose line.", this, SLOT(updateRelativePoseLineWidth())); relative_pose_line_width_property_->setMin(0.0); error_line_alpha_property_ = - new FloatProperty( - "Error Line Alpha", 0.5, "Alpha of constraint error line.", this, - SLOT(updateErrorLineAlpha())); + new FloatProperty("Error Line Alpha", 0.5, "Alpha of constraint error line.", this, SLOT(updateErrorLineAlpha())); error_line_alpha_property_->setMin(0.0); error_line_alpha_property_->setMax(1.0); - error_line_width_property_ = new FloatProperty( - "Error Line Width", 0.1, "Line width of constraint error line.", this, - SLOT(updateErrorLineWidth())); + error_line_width_property_ = new FloatProperty("Error Line Width", 0.1, "Line width of constraint error line.", this, + SLOT(updateErrorLineWidth())); error_line_width_property_->setMin(0.0); - loss_min_brightness_property_ = new FloatProperty( - "Loss Min Brightness", 0.25, - "Min brightness to show the loss impact on the constraint error " - "line.", - this, SLOT(updateLossMinBrightness())); + loss_min_brightness_property_ = new FloatProperty("Loss Min Brightness", 0.25, + "Min brightness to show the loss impact on the constraint error " + "line.", + this, SLOT(updateLossMinBrightness())); loss_min_brightness_property_->setMin(0.0); loss_min_brightness_property_->setMax(1.0); show_text_property_ = - new BoolProperty( - "Show Text", false, "Show constraint source, type and UUID.", this, - SLOT(updateShowText())); - - text_scale_property_ = - new FloatProperty( - "Text Scale", 1.0, "Scale of variable text.", this, - SLOT(updateTextScale())); + new BoolProperty("Show Text", false, "Show constraint source, type and UUID.", this, SLOT(updateShowText())); + + text_scale_property_ = new FloatProperty("Text Scale", 1.0, "Scale of variable text.", this, SLOT(updateTextScale())); text_scale_property_->setMin(0.0); covariance_property_ = new MappedCovarianceProperty( - "Covariance", true, "Whether or not the constraint covariance should be shown.", this); + "Covariance", true, "Whether or not the constraint covariance should be shown.", this); connect(this, SIGNAL(changed()), this, SLOT(updateVisibility())); // Connect changed() signal here instead of doing it through the initialization of BoolProperty(). // We do this here to make changed_slot be called _after_ updateVisibility() - if (changed_slot && (parent || receiver)) { - if (receiver) { + if (changed_slot && (parent || receiver)) + { + if (receiver) + { connect(this, SIGNAL(changed()), receiver, changed_slot); - } else { + } + else + { connect(this, SIGNAL(changed()), parent, changed_slot); } } @@ -136,19 +126,15 @@ RelativePose2DStampedConstraintProperty::RelativePose2DStampedConstraintProperty setDisableChildrenIfFalse(true); } -RelativePose2DStampedConstraintProperty::VisualPtr RelativePose2DStampedConstraintProperty:: -createAndInsertVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_constraints::RelativePose2DStampedConstraint & constraint, - const fuse_core::Graph & graph) +RelativePose2DStampedConstraintProperty::VisualPtr RelativePose2DStampedConstraintProperty::createAndInsertVisual( + Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_constraints::RelativePose2DStampedConstraint& constraint, const fuse_core::Graph& graph) { - VisualPtr visual{new Visual(scene_manager, parent_node, constraint)}; + VisualPtr visual{ new Visual(scene_manager, parent_node, constraint) }; constraints_[constraint.uuid()] = visual; - visual->setCovariance( - covariance_property_->createAndInsertVisual( - fuse_core::uuid::to_string(constraint.uuid()), - scene_manager, parent_node)); + visual->setCovariance(covariance_property_->createAndInsertVisual(fuse_core::uuid::to_string(constraint.uuid()), + scene_manager, parent_node)); visual->setConstraint(constraint, graph); updateColor(visual); @@ -166,7 +152,7 @@ createAndInsertVisual( return visual; } -void RelativePose2DStampedConstraintProperty::eraseVisual(const fuse_core::UUID & uuid) +void RelativePose2DStampedConstraintProperty::eraseVisual(const fuse_core::UUID& uuid) { covariance_property_->eraseVisual(fuse_core::uuid::to_string(uuid)); constraints_.erase(uuid); @@ -178,161 +164,161 @@ void RelativePose2DStampedConstraintProperty::clearVisual() constraints_.clear(); } -void RelativePose2DStampedConstraintProperty::setColor(const QColor & color) +void RelativePose2DStampedConstraintProperty::setColor(const QColor& color) { color_property_->setColor(color); } void RelativePose2DStampedConstraintProperty::updateVisibility() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateVisibility(entry.second); } } void RelativePose2DStampedConstraintProperty::updateColor() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateColor(entry.second); } } void RelativePose2DStampedConstraintProperty::updateErrorLineAlpha() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateErrorLineAlpha(entry.second); } } void RelativePose2DStampedConstraintProperty::updateErrorLineWidth() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateErrorLineWidth(entry.second); } } void RelativePose2DStampedConstraintProperty::updateLossMinBrightness() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateLossMinBrightness(entry.second); } } void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesAlpha() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateRelativePoseAxesAlpha(entry.second); } } void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesScale() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateRelativePoseAxesScale(entry.second); } } void RelativePose2DStampedConstraintProperty::updateRelativePoseLineAlpha() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateRelativePoseLineAlpha(entry.second); } } void RelativePose2DStampedConstraintProperty::updateRelativePoseLineWidth() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateRelativePoseLineWidth(entry.second); } } void RelativePose2DStampedConstraintProperty::updateShowText() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateShowText(entry.second); } } void RelativePose2DStampedConstraintProperty::updateTextScale() { - for (auto & entry : constraints_) { + for (auto& entry : constraints_) + { updateTextScale(entry.second); } } -void RelativePose2DStampedConstraintProperty::updateColor(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateColor(const VisualPtr& constraint) { const auto color = color_property_->getColor(); - constraint->setRelativePoseLineColor( - color.redF(), color.greenF(), color.blueF(), - relative_pose_line_alpha_property_->getFloat()); - constraint->setErrorLineColor( - color.redF(), color.greenF(), - color.blueF(), error_line_alpha_property_->getFloat()); + constraint->setRelativePoseLineColor(color.redF(), color.greenF(), color.blueF(), + relative_pose_line_alpha_property_->getFloat()); + constraint->setErrorLineColor(color.redF(), color.greenF(), color.blueF(), error_line_alpha_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateErrorLineAlpha(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateErrorLineAlpha(const VisualPtr& constraint) { const auto color = color_property_->getColor(); - constraint->setErrorLineColor( - color.redF(), color.greenF(), - color.blueF(), error_line_alpha_property_->getFloat()); + constraint->setErrorLineColor(color.redF(), color.greenF(), color.blueF(), error_line_alpha_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateErrorLineWidth(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateErrorLineWidth(const VisualPtr& constraint) { constraint->setErrorLineWidth(error_line_width_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateLossMinBrightness(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateLossMinBrightness(const VisualPtr& constraint) { constraint->setLossMinBrightness(loss_min_brightness_property_->getFloat()); updateErrorLineAlpha(constraint); } -void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesAlpha( - const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesAlpha(const VisualPtr& constraint) { constraint->setRelativePoseAxesAlpha(relative_pose_axes_alpha_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesScale( - const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateRelativePoseAxesScale(const VisualPtr& constraint) { - constraint->setRelativePoseAxesScale( - Ogre::Vector3{relative_pose_axes_scale_property_->getFloat()}); + constraint->setRelativePoseAxesScale(Ogre::Vector3{ relative_pose_axes_scale_property_->getFloat() }); } -void RelativePose2DStampedConstraintProperty::updateRelativePoseLineAlpha( - const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateRelativePoseLineAlpha(const VisualPtr& constraint) { const auto color = color_property_->getColor(); - constraint->setRelativePoseLineColor( - color.redF(), color.greenF(), color.blueF(), - relative_pose_line_alpha_property_->getFloat()); + constraint->setRelativePoseLineColor(color.redF(), color.greenF(), color.blueF(), + relative_pose_line_alpha_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateRelativePoseLineWidth( - const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateRelativePoseLineWidth(const VisualPtr& constraint) { constraint->setRelativePoseLineWidth(relative_pose_line_width_property_->getFloat()); } -void RelativePose2DStampedConstraintProperty::updateShowText(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateShowText(const VisualPtr& constraint) { constraint->setTextVisible(show_text_property_->getBool()); } -void RelativePose2DStampedConstraintProperty::updateTextScale(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateTextScale(const VisualPtr& constraint) { - constraint->setTextScale(Ogre::Vector3{text_scale_property_->getFloat()}); // NOLINT + constraint->setTextScale(Ogre::Vector3{ text_scale_property_->getFloat() }); // NOLINT } -void RelativePose2DStampedConstraintProperty::updateVisibility(const VisualPtr & constraint) +void RelativePose2DStampedConstraintProperty::updateVisibility(const VisualPtr& constraint) { const auto visible = getBool(); diff --git a/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp b/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp index 49ee0b91b..9beb1b6cf 100644 --- a/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp +++ b/fuse_viz/src/relative_pose_2d_stamped_constraint_visual.cpp @@ -66,16 +66,15 @@ namespace fuse_viz * @param[in] constraint A constraint * @return The constraint name of the form: source@type::uuid */ -std::string constraint_name(const fuse_constraints::RelativePose2DStampedConstraint & constraint) +std::string constraint_name(const fuse_constraints::RelativePose2DStampedConstraint& constraint) { - return constraint.source() + '@' + constraint.type() + "::" + fuse_core::uuid::to_string( - constraint.uuid()); + return constraint.source() + '@' + constraint.type() + "::" + fuse_core::uuid::to_string(constraint.uuid()); } RelativePose2DStampedConstraintVisual::RelativePose2DStampedConstraintVisual( - Ogre::SceneManager * scene_manager, Ogre::SceneNode * parent_node, - const fuse_constraints::RelativePose2DStampedConstraint & constraint, const bool visible) -: Object(scene_manager) + Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, + const fuse_constraints::RelativePose2DStampedConstraint& constraint, const bool visible) + : Object(scene_manager) , root_node_(parent_node->createChildSceneNode()) , source_(constraint.source()) , visible_(visible) @@ -94,9 +93,7 @@ RelativePose2DStampedConstraintVisual::RelativePose2DStampedConstraintVisual( // Create constraint relative pose axes: relative_pose_axes_node_ = root_node_->createChildSceneNode(); - relative_pose_axes_ = std::make_shared( - scene_manager_, - relative_pose_axes_node_, 10.0, 1.0); + relative_pose_axes_ = std::make_shared(scene_manager_, relative_pose_axes_node_, 10.0, 1.0); // Draw text: const auto caption = constraint_name(constraint); @@ -125,17 +122,16 @@ RelativePose2DStampedConstraintVisual::~RelativePose2DStampedConstraintVisual() } void RelativePose2DStampedConstraintVisual::setConstraint( - const fuse_constraints::RelativePose2DStampedConstraint & constraint, - const fuse_core::Graph & graph) + const fuse_constraints::RelativePose2DStampedConstraint& constraint, const fuse_core::Graph& graph) { // Update constraint relative pose line: - const auto & variables = constraint.variables(); + const auto& variables = constraint.variables(); const auto pose1 = getPose(graph, variables.at(0), variables.at(1)); - const auto & delta = constraint.delta(); - const tf2::Transform pose_delta{tf2::Quaternion{tf2::Vector3{0, 0, 1}, delta[2]}, - tf2::Vector3{delta[0], delta[1], 0}}; + const auto& delta = constraint.delta(); + const tf2::Transform pose_delta{ tf2::Quaternion{ tf2::Vector3{ 0, 0, 1 }, delta[2] }, + tf2::Vector3{ delta[0], delta[1], 0 } }; const auto absolute_pose = pose1 * pose_delta; const auto absolute_position_ogre = toOgre(absolute_pose.getOrigin()); @@ -162,14 +158,15 @@ void RelativePose2DStampedConstraintVisual::setConstraint( // Set error line color brightness based on the loss function impact on the constraint cost: auto loss_function = constraint.lossFunction(); - if (loss_function) { + if (loss_function) + { // Evaluate cost function without loss: - const double position1[] = {pose1.getOrigin().getX(), pose1.getOrigin().getY()}; - const double yaw1[] = {tf2::getYaw(pose1.getRotation())}; - const double position2[] = {pose2.getOrigin().getX(), pose2.getOrigin().getY()}; - const double yaw2[] = {tf2::getYaw(pose2.getRotation())}; + const double position1[] = { pose1.getOrigin().getX(), pose1.getOrigin().getY() }; + const double yaw1[] = { tf2::getYaw(pose1.getRotation()) }; + const double position2[] = { pose2.getOrigin().getX(), pose2.getOrigin().getY() }; + const double yaw2[] = { tf2::getYaw(pose2.getRotation()) }; - const double * parameters[] = {position1, yaw1, position2, yaw2}; + const double* parameters[] = { position1, yaw1, position2, yaw2 }; auto cost_function = constraint.costFunction(); @@ -200,16 +197,14 @@ void RelativePose2DStampedConstraintVisual::setConstraint( loss_function->Evaluate(squared_norm, rho); delete loss_function; - if (rho[0] > squared_norm) { + if (rho[0] > squared_norm) + { static rclcpp::Clock clock; - RCLCPP_WARN_STREAM_THROTTLE( - rclcpp::get_logger( - "fuse"), clock, 10.0 * 1000, - "Detected invalid loss value of " << rho[0] << " greater than squared residual of " - << squared_norm << " for constraint " << - constraint_name(constraint) - << " with loss type " << constraint.loss()->type() - << ". Loss value clamped to the squared residual."); + RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("fuse"), clock, 10.0 * 1000, + "Detected invalid loss value of " + << rho[0] << " greater than squared residual of " << squared_norm + << " for constraint " << constraint_name(constraint) << " with loss type " + << constraint.loss()->type() << ". Loss value clamped to the squared residual."); rho[0] = squared_norm; } @@ -227,9 +222,8 @@ void RelativePose2DStampedConstraintVisual::setConstraint( // Compute error line color with the loss function impact: const auto loss_error_line_color = computeLossErrorLineColor(error_line_color_, loss_scale_); - error_line_->setColor( - loss_error_line_color.r, loss_error_line_color.g, loss_error_line_color.b, - error_line_color_.a); + error_line_->setColor(loss_error_line_color.r, loss_error_line_color.g, loss_error_line_color.b, + error_line_color_.a); } // Update constraint relative pose axes: @@ -240,7 +234,7 @@ void RelativePose2DStampedConstraintVisual::setConstraint( text_node_->setPosition(absolute_position_ogre); } -void RelativePose2DStampedConstraintVisual::setUserData(const Ogre::Any & data) +void RelativePose2DStampedConstraintVisual::setUserData(const Ogre::Any& data) { relative_pose_line_->setUserData(data); error_line_->setUserData(data); @@ -263,16 +257,13 @@ void RelativePose2DStampedConstraintVisual::setLossMinBrightness(const float min min_brightness_ = min_brightness; } -void RelativePose2DStampedConstraintVisual::setRelativePoseLineColor( - const float r, const float g, const float b, - const float a) +void RelativePose2DStampedConstraintVisual::setRelativePoseLineColor(const float r, const float g, const float b, + const float a) { relative_pose_line_->setColor(r, g, b, a); } -void RelativePose2DStampedConstraintVisual::setErrorLineColor( - const float r, const float g, const float b, - const float a) +void RelativePose2DStampedConstraintVisual::setErrorLineColor(const float r, const float g, const float b, const float a) { // Cache error line color w/o the loss function impact, so we can change its darkness based on the // loss function impact on the constraint cost: Note that we cannot recover/retrieve the color @@ -284,31 +275,30 @@ void RelativePose2DStampedConstraintVisual::setErrorLineColor( // Compute error line color with the impact of the loss function, in case the constraint has one: const auto loss_error_line_color = computeLossErrorLineColor(error_line_color_, loss_scale_); - error_line_->setColor( - loss_error_line_color.r, loss_error_line_color.r, loss_error_line_color.b, - loss_error_line_color.a); + error_line_->setColor(loss_error_line_color.r, loss_error_line_color.r, loss_error_line_color.b, + loss_error_line_color.a); } void RelativePose2DStampedConstraintVisual::setRelativePoseAxesAlpha(const float alpha) { - static const auto & default_x_color_ = relative_pose_axes_->getDefaultXColor(); - static const auto & default_y_color_ = relative_pose_axes_->getDefaultYColor(); - static const auto & default_z_color_ = relative_pose_axes_->getDefaultZColor(); + static const auto& default_x_color_ = relative_pose_axes_->getDefaultXColor(); + static const auto& default_y_color_ = relative_pose_axes_->getDefaultYColor(); + static const auto& default_z_color_ = relative_pose_axes_->getDefaultZColor(); relative_pose_axes_->setXColor( - Ogre::ColourValue(default_x_color_.r, default_x_color_.g, default_x_color_.b, alpha)); // NOLINT + Ogre::ColourValue(default_x_color_.r, default_x_color_.g, default_x_color_.b, alpha)); // NOLINT relative_pose_axes_->setYColor( - Ogre::ColourValue(default_y_color_.r, default_y_color_.g, default_y_color_.b, alpha)); // NOLINT + Ogre::ColourValue(default_y_color_.r, default_y_color_.g, default_y_color_.b, alpha)); // NOLINT relative_pose_axes_->setZColor( - Ogre::ColourValue(default_z_color_.r, default_z_color_.g, default_z_color_.b, alpha)); // NOLINT + Ogre::ColourValue(default_z_color_.r, default_z_color_.g, default_z_color_.b, alpha)); // NOLINT } -void RelativePose2DStampedConstraintVisual::setRelativePoseAxesScale(const Ogre::Vector3 & scale) +void RelativePose2DStampedConstraintVisual::setRelativePoseAxesScale(const Ogre::Vector3& scale) { relative_pose_axes_->setScale(scale); } -void RelativePose2DStampedConstraintVisual::setTextScale(const Ogre::Vector3 & scale) +void RelativePose2DStampedConstraintVisual::setTextScale(const Ogre::Vector3& scale) { text_node_->setScale(scale); } @@ -325,32 +315,32 @@ void RelativePose2DStampedConstraintVisual::setVisible(const bool visible) relative_pose_axes_node_->setVisible(visible); } -const Ogre::Vector3 & RelativePose2DStampedConstraintVisual::getPosition() +const Ogre::Vector3& RelativePose2DStampedConstraintVisual::getPosition() { return root_node_->getPosition(); } -const Ogre::Quaternion & RelativePose2DStampedConstraintVisual::getOrientation() +const Ogre::Quaternion& RelativePose2DStampedConstraintVisual::getOrientation() { return root_node_->getOrientation(); } -void RelativePose2DStampedConstraintVisual::setPosition(const Ogre::Vector3 & position) +void RelativePose2DStampedConstraintVisual::setPosition(const Ogre::Vector3& position) { root_node_->setPosition(position); } -void RelativePose2DStampedConstraintVisual::setOrientation(const Ogre::Quaternion & orientation) +void RelativePose2DStampedConstraintVisual::setOrientation(const Ogre::Quaternion& orientation) { root_node_->setOrientation(orientation); } -Ogre::ColourValue RelativePose2DStampedConstraintVisual::computeLossErrorLineColor( - const Ogre::ColourValue & color, - const float loss_scale) +Ogre::ColourValue RelativePose2DStampedConstraintVisual::computeLossErrorLineColor(const Ogre::ColourValue& color, + const float loss_scale) { // Skip if the loss scale is negative, which means the constraint has no loss: - if (loss_scale < 0.0) { + if (loss_scale < 0.0) + { return color; } @@ -363,7 +353,7 @@ Ogre::ColourValue RelativePose2DStampedConstraintVisual::computeLossErrorLineCol // would get an incorrect loss brightness. // // However, we cannot do this because it changes the color of the error line, which should be - // consistent for all constraints visuals. Instead, we clamp the minium brightness: + // consistent for all constraints visuals. Instead, we clamp the minimum brightness: const auto min_brightness = std::min(min_brightness_, brightness); // Scale brightness by the loss scale within the [min_brightness, 1] range: @@ -373,9 +363,7 @@ Ogre::ColourValue RelativePose2DStampedConstraintVisual::computeLossErrorLineCol Ogre::ColourValue loss_error_line_color; loss_error_line_color.setHSB(hue, saturation, loss_brightness); - return Ogre::ColourValue( - loss_error_line_color.r, loss_error_line_color.g, - loss_error_line_color.b, color.a); + return Ogre::ColourValue(loss_error_line_color.r, loss_error_line_color.g, loss_error_line_color.b, color.a); } } // namespace fuse_viz diff --git a/fuse_viz/src/serialized_graph_display.cpp b/fuse_viz/src/serialized_graph_display.cpp index 134438b54..4f6cac411 100644 --- a/fuse_viz/src/serialized_graph_display.cpp +++ b/fuse_viz/src/serialized_graph_display.cpp @@ -63,24 +63,21 @@ using rviz_common::properties::BoolProperty; SerializedGraphDisplay::SerializedGraphDisplay() { show_variables_property_ = - new BoolProperty( - "Variables", true, "The list of all variables.", this, - SLOT(updateShowVariables())); - - variable_property_ = new Pose2DStampedProperty( - "pose_2d", true, - "Pose2DStamped (fuse_variables::Position2DStamped + " - "fuse_variables::Orientation2DStamped) variable.", - show_variables_property_, SLOT(queueRender()), this); - - show_constraints_property_ = new BoolProperty( - "Constraints", true, "The list of all constraints by source.", this, - SLOT(updateShowConstraints())); + new BoolProperty("Variables", true, "The list of all variables.", this, SLOT(updateShowVariables())); + + variable_property_ = new Pose2DStampedProperty("pose_2d", true, + "Pose2DStamped (fuse_variables::Position2DStamped + " + "fuse_variables::Orientation2DStamped) variable.", + show_variables_property_, SLOT(queueRender()), this); + + show_constraints_property_ = new BoolProperty("Constraints", true, "The list of all constraints by source.", this, + SLOT(updateShowConstraints())); } SerializedGraphDisplay::~SerializedGraphDisplay() { - if (initialized()) { + if (initialized()) + { clear(); root_node_->removeAndDestroyAllChildren(); @@ -108,7 +105,8 @@ void SerializedGraphDisplay::onEnable() variable_property_->updateVisibility(); - for (auto & entry : constraint_source_properties_) { + for (auto& entry : constraint_source_properties_) + { entry.second->updateVisibility(); } } @@ -120,7 +118,7 @@ void SerializedGraphDisplay::onDisable() root_node_->setVisible(false); } -void SerializedGraphDisplay::load(const rviz_common::Config & config) +void SerializedGraphDisplay::load(const rviz_common::Config& config) { MFDClass::load(config); @@ -129,8 +127,7 @@ void SerializedGraphDisplay::load(const rviz_common::Config & config) // is present in the graph: const auto constraints_config = config.mapGetChild("Constraints"); - for (rviz_common::Config::MapIterator iter = constraints_config.mapIterator(); iter.isValid(); - iter.advance()) + for (rviz_common::Config::MapIterator iter = constraints_config.mapIterator(); iter.isValid(); iter.advance()) { constraint_source_configs_[iter.currentKey().toStdString()] = iter.currentChild(); } @@ -145,7 +142,8 @@ void SerializedGraphDisplay::updateShowConstraints() { const auto visible = show_constraints_property_->getBool(); - for (auto & entry : constraint_source_properties_) { + for (auto& entry : constraint_source_properties_) + { entry.second->setBool(visible); } } @@ -156,7 +154,8 @@ void SerializedGraphDisplay::clear() delete variable_property_; - for (auto & entry : constraint_source_properties_) { + for (auto& entry : constraint_source_properties_) + { delete entry.second; } constraint_source_properties_.clear(); @@ -169,59 +168,62 @@ void SerializedGraphDisplay::processMessage(fuse_msgs::msg::SerializedGraph::Con { Ogre::Vector3 position; Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg->header, position, orientation)) { - RCLCPP_DEBUG_STREAM( - rclcpp::get_logger("fuse"), - "Error transforming from frame '" << msg->header.frame_id << "' to frame '" - << qPrintable(fixed_frame_) << "'"); + if (!context_->getFrameManager()->getTransform(msg->header, position, orientation)) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("fuse"), "Error transforming from frame '" + << msg->header.frame_id << "' to frame '" + << qPrintable(fixed_frame_) << "'"); } root_node_->setPosition(position); root_node_->setOrientation(orientation); - for (auto & entry : variables_changed_map_) { + for (auto& entry : variables_changed_map_) + { entry.second = false; } - for (auto & entry : constraints_changed_map_) { + for (auto& entry : constraints_changed_map_) + { entry.second = false; } const auto graph = graph_deserializer_.deserialize(msg); - for (const auto & variable : graph->getVariables()) { - const auto orientation = dynamic_cast(&variable); - if (!orientation) { + for (const auto& variable : graph->getVariables()) + { + const auto orientation = dynamic_cast(&variable); + if (!orientation) + { continue; } - const auto position_uuid = fuse_variables::Position2DStamped( - orientation->stamp(), orientation->deviceId()).uuid(); - if (!graph->variableExists(position_uuid)) { + const auto position_uuid = fuse_variables::Position2DStamped(orientation->stamp(), orientation->deviceId()).uuid(); + if (!graph->variableExists(position_uuid)) + { continue; } - const auto position = - dynamic_cast(&graph->getVariable(position_uuid)); + const auto position = dynamic_cast(&graph->getVariable(position_uuid)); - variable_property_->createAndInsertOrUpdateVisual( - scene_manager_, root_node_, *position, - *orientation); + variable_property_->createAndInsertOrUpdateVisual(scene_manager_, root_node_, *position, *orientation); variables_changed_map_[position_uuid] = true; } - for (const auto & constraint : graph->getConstraints()) { - const auto relative_pose = - dynamic_cast(&constraint); - if (!relative_pose) { + for (const auto& constraint : graph->getConstraints()) + { + const auto relative_pose = dynamic_cast(&constraint); + if (!relative_pose) + { continue; } const auto constraint_uuid = constraint.uuid(); - const auto & constraint_source = constraint.source(); + const auto& constraint_source = constraint.source(); - if (source_color_map_.find(constraint_source) == source_color_map_.end()) { + if (source_color_map_.find(constraint_source) == source_color_map_.end()) + { // Generate hue color automatically based on the number of sources including the new one (n) // The hue is computed in such a way that the (dynamic) colormap is always well spread along // the spectrum. This is achieved by traversing a virtual complete binary tree in breadth- @@ -232,78 +234,90 @@ void SerializedGraphDisplay::processMessage(fuse_msgs::msg::SerializedGraph::Con const auto m = n + 1 - std::pow(2, level); const auto hue = (2 * (m - 1) + 1) / std::pow(2, level + 1); - auto & source_color = source_color_map_[constraint_source]; + auto& source_color = source_color_map_[constraint_source]; source_color.setHSB(hue, 1.0, 1.0); // Insert constraint sorted alphabetically: const auto description = constraint_source + ' ' + constraint.type() + " constraint."; - const auto constraint_source_property = new RelativePose2DStampedConstraintProperty( - QString::fromStdString(constraint_source), true, QString::fromStdString( - description), nullptr, - SLOT(queueRender()), this); + const auto constraint_source_property = + new RelativePose2DStampedConstraintProperty(QString::fromStdString(constraint_source), true, + QString::fromStdString(description), nullptr, SLOT(queueRender()), + this); const auto result = constraint_source_properties_.insert( - {constraint_source, constraint_source_property}); // NOLINT(whitespace/braces) + { constraint_source, constraint_source_property }); // NOLINT(whitespace/braces) - if (!result.second) { + if (!result.second) + { delete constraint_source_property; throw std::runtime_error("Failed to insert " + description); } - show_constraints_property_->addChild( - constraint_source_property, - std::distance(constraint_source_properties_.begin(), result.first)); - - if (constraint_source_configs_.find(constraint_source) == constraint_source_configs_.end()) { - constraint_source_properties_[constraint_source]->setColor( - rviz_common::properties::ogreToQt( - source_color)); - } else { - constraint_source_properties_[constraint_source]->load( - constraint_source_configs_[ - constraint_source]); + show_constraints_property_->addChild(constraint_source_property, + std::distance(constraint_source_properties_.begin(), result.first)); + + if (constraint_source_configs_.find(constraint_source) == constraint_source_configs_.end()) + { + constraint_source_properties_[constraint_source]->setColor(rviz_common::properties::ogreToQt(source_color)); + } + else + { + constraint_source_properties_[constraint_source]->load(constraint_source_configs_[constraint_source]); } } - if (constraint_visuals_.find(constraint_uuid) == constraint_visuals_.end()) { - constraint_visuals_[constraint_uuid] = - constraint_source_properties_[constraint_source]->createAndInsertVisual( - scene_manager_, root_node_, *relative_pose, *graph); - } else { + if (constraint_visuals_.find(constraint_uuid) == constraint_visuals_.end()) + { + constraint_visuals_[constraint_uuid] = constraint_source_properties_[constraint_source]->createAndInsertVisual( + scene_manager_, root_node_, *relative_pose, *graph); + } + else + { constraint_visuals_[constraint_uuid]->setConstraint(*relative_pose, *graph); } constraints_changed_map_[constraint_uuid] = true; } - for (const auto & entry : variables_changed_map_) { - if (!entry.second) { + for (const auto& entry : variables_changed_map_) + { + if (!entry.second) + { variable_property_->eraseVisual(entry.first); } } - for (auto it = variables_changed_map_.begin(); it != variables_changed_map_.end(); ) { - if (it->second) { + for (auto it = variables_changed_map_.begin(); it != variables_changed_map_.end();) + { + if (it->second) + { ++it; - } else { + } + else + { it = variables_changed_map_.erase(it); } } - for (const auto & entry : constraints_changed_map_) { - if (!entry.second) { - constraint_source_properties_[constraint_visuals_[entry.first]->getSource()]->eraseVisual( - entry.first); + for (const auto& entry : constraints_changed_map_) + { + if (!entry.second) + { + constraint_source_properties_[constraint_visuals_[entry.first]->getSource()]->eraseVisual(entry.first); constraint_visuals_.erase(entry.first); } } - for (auto it = constraints_changed_map_.begin(); it != constraints_changed_map_.end(); ) { - if (it->second) { + for (auto it = constraints_changed_map_.begin(); it != constraints_changed_map_.end();) + { + if (it->second) + { ++it; - } else { + } + else + { it = constraints_changed_map_.erase(it); } } diff --git a/markdown-link-check-config.json b/markdown-link-check-config.json new file mode 100644 index 000000000..1bd58aa4a --- /dev/null +++ b/markdown-link-check-config.json @@ -0,0 +1 @@ +{"ignorePatterns": [{"pattern": "^https://docs.google.com"}]}