From be32430e07ca9ffec0a0edf53af1b2c8a69c3cbf Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 9 Dec 2024 17:56:31 +0000 Subject: [PATCH 1/8] remove currently-unused mujoco dependency --- fuse.repos | 4 ---- fuse_models/CMakeLists.txt | 10 +++++----- fuse_models/package.xml | 2 +- 3 files changed, 6 insertions(+), 10 deletions(-) diff --git a/fuse.repos b/fuse.repos index f00574a17..d569f4ec3 100644 --- a/fuse.repos +++ b/fuse.repos @@ -7,7 +7,3 @@ repositories: type: git url: https://github.com/giafranchini/covariance_geometry_ros.git version: iron - mujoco: - type: git - url: https://github.com/google-deepmind/mujoco.git - version: 3.2.4 diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index 355ffc8be..838e9b3ba 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -31,7 +31,7 @@ find_package(tf2 REQUIRED) find_package(tf2_2d REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(mujoco REQUIRED) +# find_package(mujoco REQUIRED) find_package(Ceres REQUIRED) find_package(Eigen3 REQUIRED) @@ -47,7 +47,7 @@ add_library( src/graph_ignition.cpp src/imu_2d.cpp src/imu_3d.cpp - src/mujoco_model.cpp + # src/mujoco_model.cpp src/odometry_2d.cpp src/odometry_2d_publisher.cpp src/odometry_3d.cpp @@ -77,7 +77,7 @@ target_link_libraries( fuse_publishers::fuse_publishers fuse_variables::fuse_variables ${geometry_msgs_TARGETS} - mujoco::mujoco + # mujoco::mujoco ${nav_msgs_TARGETS} pluginlib::pluginlib rclcpp_components::component @@ -136,7 +136,7 @@ ament_export_dependencies( tf2_ros Ceres Eigen3 - Boost - mujoco) + Boost) +# mujoco) ament_package() diff --git a/fuse_models/package.xml b/fuse_models/package.xml index 7fe1e09f1..b2c173c6a 100644 --- a/fuse_models/package.xml +++ b/fuse_models/package.xml @@ -16,7 +16,7 @@ boost libceres-dev eigen - mujoco + covariance_geometry_ros fuse_constraints From 8842d49b47323ff6792af7ed95900b73437996b0 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 9 Dec 2024 18:27:33 +0000 Subject: [PATCH 2/8] switch to submodules --- .gitmodules | 6 ++++++ dependencies/covariance_geometry_ros | 1 + dependencies/tf2_2d | 1 + fuse.repos | 9 --------- 4 files changed, 8 insertions(+), 9 deletions(-) create mode 100644 .gitmodules create mode 160000 dependencies/covariance_geometry_ros create mode 160000 dependencies/tf2_2d delete mode 100644 fuse.repos diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..a7c3ea4d1 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,6 @@ +[submodule "dependencies/tf2_2d"] + path = dependencies/tf2_2d + url = https://github.com/locusrobotics/tf2_2d.git +[submodule "dependencies/covariance_geometry_ros"] + path = dependencies/covariance_geometry_ros + url = https://github.com/giafranchini/covariance_geometry_ros.git diff --git a/dependencies/covariance_geometry_ros b/dependencies/covariance_geometry_ros new file mode 160000 index 000000000..182993bae --- /dev/null +++ b/dependencies/covariance_geometry_ros @@ -0,0 +1 @@ +Subproject commit 182993bae25f64ed45c98b32cea13d851ae38fc3 diff --git a/dependencies/tf2_2d b/dependencies/tf2_2d new file mode 160000 index 000000000..41b3d5b05 --- /dev/null +++ b/dependencies/tf2_2d @@ -0,0 +1 @@ +Subproject commit 41b3d5b058a6ef89d6b3ed264d0c658be0e6691c diff --git a/fuse.repos b/fuse.repos deleted file mode 100644 index d569f4ec3..000000000 --- a/fuse.repos +++ /dev/null @@ -1,9 +0,0 @@ -repositories: - locusrobotics/tf2_2d: - type: git - url: https://github.com/locusrobotics/tf2_2d.git - version: rolling - covariance_geometry_ros: - type: git - url: https://github.com/giafranchini/covariance_geometry_ros.git - version: iron From 2104a1e058a740b31705f3bb6e68ead3be3acae0 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 9 Dec 2024 18:37:35 +0000 Subject: [PATCH 3/8] remove vcs docker step --- Dockerfile | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Dockerfile b/Dockerfile index 472ad5a29..907df3036 100644 --- a/Dockerfile +++ b/Dockerfile @@ -23,8 +23,7 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ --mount=type=cache,target=/var/lib/apt,sharing=locked \ apt-get update && apt-get upgrade -y && \ . /opt/ros/rolling/setup.sh && \ - vcs import src --input src/fuse/fuse.repos && \ - rosdep install --from-paths src -y --ignore-src --skip-keys=mujoco && \ + rosdep install --from-paths src -y --ignore-src && \ colcon build --mixin compile-commands coverage-gcc coverage-pytest # Set up final environment and entrypoint. From 4cefcaa84dc9e437f420f44056316e295337633d Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 9 Dec 2024 18:43:03 +0000 Subject: [PATCH 4/8] checkout submodules too --- .github/workflows/ci.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index d58661793..07c96bdc5 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -23,6 +23,8 @@ jobs: steps: - name: Checkout source uses: actions/checkout@v4 + with: + submodules: 'true' - name: Setup Docker Buildx uses: docker/setup-buildx-action@v2 From b1273fa3f8cb88303750063256488375cc39d33d Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 9 Dec 2024 23:24:04 +0000 Subject: [PATCH 5/8] fix clang tidy when PR doesn't change cpp files --- .github/workflows/ci.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 07c96bdc5..96c51d0fc 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -92,7 +92,8 @@ jobs: files: | **.cpp **.hpp - - run: run-clang-tidy -j $(nproc --all) -p build/ -export-fixes clang-tidy-fixes.yaml -config-file src/fuse/.clang-tidy ${{ steps.changed-cpp-files.outputs.all_changed_files }} + - if: ${{ steps.changed-cpp-files.outputs.all_changed_files != '' }} + run: run-clang-tidy -j $(nproc --all) -p build/ -export-fixes clang-tidy-fixes.yaml -config-file src/fuse/.clang-tidy ${{ steps.changed-cpp-files.outputs.all_changed_files }} working-directory: /colcon_ws - uses: asarium/clang-tidy-action@v1.0 with: From 22cec8ddae5925c60c80db8e6526d52a26d38842 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 12 Dec 2024 23:06:42 +0000 Subject: [PATCH 6/8] got humble building --- .github/workflows/ci.yaml | 4 +- .github/workflows/pre-commit.yaml | 2 +- Dockerfile | 6 +- dependencies/covariance_geometry_ros | 2 +- entrypoint.sh | 2 +- .../include/fuse_core/callback_wrapper.hpp | 6 +- .../node_interfaces/node_interfaces.hpp | 148 +++++-- .../node_interfaces_helpers.hpp | 387 ++++++++++++++++++ fuse_core/src/callback_wrapper.cpp | 8 +- .../fuse_models/common/sensor_proc.hpp | 2 +- fuse_models/src/acceleration_2d.cpp | 5 +- fuse_models/src/graph_ignition.cpp | 10 +- fuse_models/src/imu_2d.cpp | 5 +- fuse_models/src/imu_3d.cpp | 5 +- fuse_models/src/odometry_2d.cpp | 5 +- fuse_models/src/odometry_2d_publisher.cpp | 5 +- fuse_models/src/odometry_3d.cpp | 5 +- fuse_models/src/odometry_3d_publisher.cpp | 5 +- .../src/omnidirectional_3d_ignition.cpp | 12 +- fuse_models/src/pose_2d.cpp | 5 +- fuse_models/src/transform_sensor.cpp | 5 +- fuse_models/src/twist_2d.cpp | 5 +- fuse_models/src/unicycle_2d_ignition.cpp | 12 +- fuse_optimizers/src/fixed_lag_smoother.cpp | 2 +- fuse_publishers/src/pose_2d_publisher.cpp | 6 +- fuse_tutorials/src/apriltag_simulator.cpp | 3 +- fuse_tutorials/src/range_sensor_simulator.cpp | 11 +- .../src/three_dimensional_simulator.cpp | 3 +- 28 files changed, 569 insertions(+), 107 deletions(-) create mode 100644 fuse_core/include/fuse_core/node_interfaces/node_interfaces_helpers.hpp diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 96c51d0fc..e48237896 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -8,7 +8,7 @@ on: # Run when a commit is pushed to main push: branches: - - main + - humble permissions: # Allow reading the source code @@ -61,7 +61,7 @@ jobs: steps: - name: Unit test workspace run: | - . /opt/ros/rolling/setup.sh + . /opt/ros/humble/setup.sh . /colcon_ws/install/local_setup.sh colcon test --event-handlers console_direct+ --packages-select-regex fuse* working-directory: /colcon_ws diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index 416508d7a..e55c1faa7 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -7,7 +7,7 @@ on: pull_request: push: branches: - - main + - humble jobs: pre-commit: diff --git a/Dockerfile b/Dockerfile index 907df3036..f79eed547 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,6 +1,6 @@ # Docker setup that's used for CI. -FROM osrf/ros:rolling-desktop-full +FROM osrf/ros:humble-desktop-full # Install external packages. # hadolint ignore=DL3008 @@ -11,7 +11,7 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ clang-tidy \ python3-vcstool \ # use cyclonedds instead of fastdds - ros-rolling-rmw-cyclonedds-cpp + ros-humble-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. @@ -22,7 +22,7 @@ WORKDIR /colcon_ws 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 && \ + . /opt/ros/humble/setup.sh && \ rosdep install --from-paths src -y --ignore-src && \ colcon build --mixin compile-commands coverage-gcc coverage-pytest diff --git a/dependencies/covariance_geometry_ros b/dependencies/covariance_geometry_ros index 182993bae..4a13ea862 160000 --- a/dependencies/covariance_geometry_ros +++ b/dependencies/covariance_geometry_ros @@ -1 +1 @@ -Subproject commit 182993bae25f64ed45c98b32cea13d851ae38fc3 +Subproject commit 4a13ea862709bbebed60f6d1fad3ce91aa23250f diff --git a/entrypoint.sh b/entrypoint.sh index 992ad8540..5d7f28470 100755 --- a/entrypoint.sh +++ b/entrypoint.sh @@ -1,6 +1,6 @@ #!/bin/bash -source /opt/ros/rolling/setup.bash +source /opt/ros/humble/setup.bash if [ -f /colcon_ws/install/local_setup.bash ] then diff --git a/fuse_core/include/fuse_core/callback_wrapper.hpp b/fuse_core/include/fuse_core/callback_wrapper.hpp index 7f920f6ba..e5ffb65d6 100644 --- a/fuse_core/include/fuse_core/callback_wrapper.hpp +++ b/fuse_core/include/fuse_core/callback_wrapper.hpp @@ -169,7 +169,7 @@ class CallbackAdapter : public rclcpp::Waitable /** * @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* wait_set) override; /** * @brief add_to_wait_set is called by rclcpp during NodeWaitables::add_waitable() and @@ -177,11 +177,11 @@ 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& data) override; void addCallback(const std::shared_ptr& callback); 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 17fe3626b..15fa57da9 100644 --- a/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp +++ b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp @@ -17,17 +17,7 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "fuse_core/node_interfaces/node_interfaces_helpers.hpp" #define ALL_FUSE_CORE_NODE_INTERFACES \ fuse_core::node_interfaces::Base, fuse_core::node_interfaces::Clock, fuse_core::node_interfaces::Graph, \ @@ -39,19 +29,129 @@ namespace fuse_core { namespace node_interfaces { -using Base = rclcpp::node_interfaces::NodeBaseInterface; -using Clock = rclcpp::node_interfaces::NodeClockInterface; -using Graph = rclcpp::node_interfaces::NodeGraphInterface; -using Logging = rclcpp::node_interfaces::NodeLoggingInterface; -using Parameters = rclcpp::node_interfaces::NodeParametersInterface; -using Services = rclcpp::node_interfaces::NodeServicesInterface; -using TimeSource = rclcpp::node_interfaces::NodeTimeSourceInterface; -using Timers = rclcpp::node_interfaces::NodeTimersInterface; -using Topics = rclcpp::node_interfaces::NodeTopicsInterface; -using Waitables = rclcpp::node_interfaces::NodeWaitablesInterface; - -template -using NodeInterfaces = ::rclcpp::node_interfaces::NodeInterfaces; + +/// TODO(CH3): Remove this once https://github.com/ros2/rclcpp/pull/2041 is merged and released + +/// A helper class for aggregating node interfaces +template +class NodeInterfaces : public std::enable_shared_from_this>, public InterfaceTs... +{ + static_assert(0 != sizeof...(InterfaceTs), "Template parameters must be populated!"); + +public: + RCLCPP_SMART_PTR_DEFINITIONS(NodeInterfaces) + + /// Create a new NodeInterfaces object with no bound node interfaces. + NodeInterfaces() : InterfaceTs()... + { + } + + /// Create a new NodeInterfaces object bound with the passed in node-like object's interfaces. + /** + * Specify which interfaces you want to bind using the template parameters by specifying + * interface support classes to use. Any unmentioned interfaces will be unavailable to bind. + * + * You may use any of the available support classes in + * node_interfaces/node_interfaces_helpers.hpp: + * - Base: Supports NodeBaseInterface + * - Clock: Supports NodeClockInterface + * - Graph: Supports NodeGraphInterface + * - Logging: Supports NodeLoggingInterface + * - Parameters: Supports NodeParametersInterface + * - Services: Supports NodeServicesInterface + * - TimeSource: Supports NodeTimeSourceInterface + * - Timers: Supports NodeTimersInterface + * - Topics: Supports NodeTopicsInterface + * - Waitables: Supports NodeWaitablesInterface + * + * Or you can define your own interface support classes! + * + * Each of the support classes should define: + * - Default constructor + * - Templated constructor taking NodeT + * - get_node__interface() + * - set_node__interface() + * + * Usage example: + * - ```NodeInterfaces(node)``` will bind just the + * NodeBaseInterface. + * - ```NodeInterfaces< rclcpp::node_interfaces::Base, rclcpp::node_interfaces::Clock>(node)``` + * will bind both the NodeBaseInterface and NodeClockInterface. + * + * \param[in] node Node-like object to bind the interfaces of. + */ + template + NodeInterfaces(NodeT& node) // NOLINT(runtime/explicit) + : InterfaceTs(node)... + { + } // Implicit constructor for node-like passing to functions + + /// SharedPtr Constructor + template + NodeInterfaces(std::shared_ptr node) // NOLINT(runtime/explicit) + : InterfaceTs(node ? *node : throw std::runtime_error("Passed in NodeT is nullptr!"))... + { + } +}; + +/// Create a new NodeInterfaces object bound with no node interfaces. +/** + * Specify which interfaces you want to bind using the template parameters by specifying + * interface support classes to use. Any unmentioned interfaces will be unavailable to bind. + * + * This method will return a NodeInterfaces with no bound interfaces. You must set them using + * ```NodeInterfaces->set__interface(InterfaceT::SharedPtr interface)``` + * + * See the rclcpp::node_interfaces::NodeInterfaces class for usage examples and support classes. + * + * \sa rclcpp::node_interfaces::NodeInterfaces + * \param[in] node Node-like object to bind the interfaces of. + * \returns a NodeInterfaces::SharedPtr supporting the stated interfaces, but bound with none of + * them + */ +template +typename NodeInterfaces::SharedPtr get_node_interfaces() +{ + static_assert(0 != sizeof...(InterfaceTs), "Template parameters must be populated!"); + return std::make_shared>(); +} + +/// Create a new NodeInterfaces object bound with the passed in node-like object's interfaces. +/** + * Specify which interfaces you want to bind using the template parameters by specifying + * interface support classes to use. Any unmentioned interfaces will be unavailable to bind. + * + * See the rclcpp::node_interfaces::NodeInterfaces class for usage examples and support classes. + * + * \sa rclcpp::node_interfaces::NodeInterfaces + * \param[in] node Node-like object to bind the interfaces of. + * \returns a NodeInterfaces::SharedPtr bound with the node-like objects's interfaces + */ +template +typename NodeInterfaces::SharedPtr get_node_interfaces(NodeT& node) +{ + static_assert(0 != sizeof...(InterfaceTs), "Template parameters must be populated!"); + return std::make_shared>(node); +} + +/// Create a new NodeInterfaces object bound with the passed in node-like shared_ptr's interfaces. +/** + * Specify which interfaces you want to bind using the template parameters by specifying + * interface support classes to use. Any unmentioned interfaces will be unavailable to bind. + * + * See the rclcpp::node_interfaces::NodeInterfaces class for usage examples and support classes. + * + * \sa rclcpp::node_interfaces::NodeInterfaces + * \param[in] node Node-like object to bind the interfaces of. + * \returns a NodeInterfaces::SharedPtr bound with the node-like objects's interfaces + */ +template +typename NodeInterfaces::SharedPtr get_node_interfaces(std::shared_ptr node) +{ + static_assert(0 != sizeof...(InterfaceTs), "Template parameters must be populated!"); + return std::make_shared>(node); +} + } // namespace node_interfaces } // namespace fuse_core diff --git a/fuse_core/include/fuse_core/node_interfaces/node_interfaces_helpers.hpp b/fuse_core/include/fuse_core/node_interfaces/node_interfaces_helpers.hpp new file mode 100644 index 000000000..1d1c48a95 --- /dev/null +++ b/fuse_core/include/fuse_core/node_interfaces/node_interfaces_helpers.hpp @@ -0,0 +1,387 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef FUSE_CORE__NODE_INTERFACES__NODE_INTERFACES_HELPERS_HPP_ +#define FUSE_CORE__NODE_INTERFACES__NODE_INTERFACES_HELPERS_HPP_ + +#include + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace fuse_core +{ +namespace node_interfaces +{ + +/// TODO(CH3): Remove this once https://github.com/ros2/rclcpp/pull/2041 is merged and released + +// Helper classes to be inherited by NodeInterfaces to support node interface aggregation +// via multiple inheritance. + +// These also provide a more terse way to configure the supported interfaces! + +/// NodeInterfaces support for NodeBaseInterface +class Base +{ +public: + /// Default constructor with no bound NodeBaseInterface + RCLCPP_PUBLIC + Base() + { + impl_ = nullptr; + } + + /// Bind the passed in node-like object's NodeBaseInterface + template + explicit Base(NodeT& node) + { + impl_ = node.get_node_base_interface(); + } + + /// Return the bound NodeBaseInterface + inline rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() + { + return impl_; + } + + /// Set the bound NodeBaseInterface + inline void set_node_base_interface(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr impl_; +}; + +/// NodeInterfaces support for NodeClockInterface +class Clock +{ +public: + /// Default constructor with no bound NodeClockInterface + RCLCPP_PUBLIC + Clock() + { + impl_ = nullptr; + } + + /// Bind the passed in node-like object's NodeClockInterface + template + explicit Clock(NodeT& node) + { + impl_ = node.get_node_clock_interface(); + } + + /// Return the bound NodeClockInterface + inline rclcpp::node_interfaces::NodeClockInterface::SharedPtr get_node_clock_interface() + { + return impl_; + } + + /// Set the bound NodeClockInterface + inline void set_node_clock_interface(rclcpp::node_interfaces::NodeClockInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeClockInterface::SharedPtr impl_; +}; + +/// NodeInterfaces support for NodeGraphInterface +class Graph +{ +public: + /// Default constructor with no bound NodeGraphInterface + RCLCPP_PUBLIC + Graph() + { + impl_ = nullptr; + } + + /// Bind the passed in node-like object's NodeGraphInterface + template + explicit Graph(NodeT& node) + { + impl_ = node.get_node_graph_interface(); + } + + /// Return the bound NodeGraphInterface + inline rclcpp::node_interfaces::NodeGraphInterface::SharedPtr get_node_graph_interface() + { + return impl_; + } + + /// Set the bound NodeGraphInterface + inline void set_node_graph_interface(rclcpp::node_interfaces::NodeGraphInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr impl_; +}; + +/// NodeInterfaces support for NodeLoggingInterface +class Logging +{ +public: + /// Default constructor with no bound NodeLoggingInterface + RCLCPP_PUBLIC + Logging() + { + impl_ = nullptr; + } + + /// Bind the passed in node-like object's NodeLoggingInterface + template + explicit Logging(NodeT& node) + { + impl_ = node.get_node_logging_interface(); + } + + /// Return the bound NodeLoggingInterface + inline rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr get_node_logging_interface() + { + return impl_; + } + + /// Set the bound NodeLoggingInterface + inline void set_node_logging_interface(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr impl_; +}; + +/// NodeInterfaces support for NodeParametersInterface +class Parameters +{ +public: + /// Default constructor with no bound NodeParametersInterface + RCLCPP_PUBLIC + Parameters() + { + impl_ = nullptr; + } + + /// Bind the passed in node-like object's NodeParametersInterface + template + explicit Parameters(NodeT& node) + { + impl_ = node.get_node_parameters_interface(); + } + + /// Return the bound NodeParametersInterface + inline rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface() + { + return impl_; + } + + /// Set the bound NodeParametersInterface + inline void set_node_parameters_interface(rclcpp::node_interfaces::NodeParametersInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr impl_; +}; + +/// NodeInterfaces support for NodeServicesInterface +class Services +{ +public: + /// Default constructor with no bound NodeServicesInterface + RCLCPP_PUBLIC + Services() + { + impl_ = nullptr; + } + + /// Bind the passed in node-like object's NodeServicesInterface + template + explicit Services(NodeT& node) + { + impl_ = node.get_node_services_interface(); + } + + /// Return the bound NodeServicesInterface + inline rclcpp::node_interfaces::NodeServicesInterface::SharedPtr get_node_services_interface() + { + return impl_; + } + + /// Set the bound NodeServicesInterface + inline void set_node_services_interface(rclcpp::node_interfaces::NodeServicesInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr impl_; +}; + +/// NodeInterfaces support for NodeTimeSourceInterface +class TimeSource +{ +public: + /// Default constructor with no bound NodeTimeSourceInterface + RCLCPP_PUBLIC + TimeSource() + { + impl_ = nullptr; + } + + /// Bind the passed in node-like object's NodeTimeSourceInterface + template + explicit TimeSource(NodeT& node) + { + impl_ = node.get_node_time_source_interface(); + } + + /// Return the bound NodeTimeSourceInterface + inline rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr get_node_time_source_interface() + { + return impl_; + } + + /// Set the bound NodeTimeSourceInterface + inline void set_node_time_source_interface(rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr impl_; +}; + +/// NodeInterfaces support for NodeTimersInterface +class Timers +{ +public: + /// Default constructor with no bound NodeTimersInterface + RCLCPP_PUBLIC + Timers() + { + impl_ = nullptr; + } + + /// Bind the passed in node-like object's NodeTimersInterface + template + explicit Timers(NodeT& node) + { + impl_ = node.get_node_timers_interface(); + } + + /// Return the bound NodeTimersInterface + inline rclcpp::node_interfaces::NodeTimersInterface::SharedPtr get_node_timers_interface() + { + return impl_; + } + + /// Set the bound NodeTimersInterface + inline void set_node_timers_interface(rclcpp::node_interfaces::NodeTimersInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeTimersInterface::SharedPtr impl_; +}; + +/// NodeInterfaces support for NodeTopicsInterface +class Topics +{ +public: + /// Default constructor with no bound NodeTopicsInterface + RCLCPP_PUBLIC + Topics() + { + impl_ = nullptr; + } + + /// Bind the passed in node-like object's NodeTopicsInterface + template + explicit Topics(NodeT& node) + { + impl_ = node.get_node_topics_interface(); + } + + /// Return the bound NodeTopicsInterface + inline rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr get_node_topics_interface() + { + return impl_; + } + + /// Set the bound NodeTopicsInterface + inline void set_node_topics_interface(rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr impl_; +}; + +/// NodeInterfaces support for NodeWaitablesInterface +class Waitables +{ +public: + /// Default constructor with no bound NodeWaitablesInterface + RCLCPP_PUBLIC + Waitables() + { + impl_ = nullptr; + } + + /// Bind the passed in node-like object's NodeWaitablesInterface + template + explicit Waitables(NodeT& node) + { + impl_ = node.get_node_waitables_interface(); + } + + /// Return the bound NodeWaitablesInterface + inline rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr get_node_waitables_interface() + { + return impl_; + } + + /// Set the bound NodeWaitablesInterface + inline void set_node_waitables_interface(rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr interface) + { + impl_ = interface; + } + +private: + rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr impl_; +}; + +} // namespace node_interfaces +} // namespace fuse_core + +#endif // FUSE_CORE__NODE_INTERFACES__NODE_INTERFACES_HELPERS_HPP_ diff --git a/fuse_core/src/callback_wrapper.cpp b/fuse_core/src/callback_wrapper.cpp index fbbcdecbc..a55310990 100644 --- a/fuse_core/src/callback_wrapper.cpp +++ b/fuse_core/src/callback_wrapper.cpp @@ -60,7 +60,7 @@ size_t CallbackAdapter::get_number_of_ready_guard_conditions() /** * @brief tell the CallbackGroup that this waitable is ready to execute anything */ -bool CallbackAdapter::is_ready(rcl_wait_set_t const& wait_set) +bool CallbackAdapter::is_ready(rcl_wait_set_t* wait_set) { (void)wait_set; return !callback_queue_.empty(); @@ -72,9 +72,9 @@ 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_, nullptr)) + if (RCL_RET_OK != rcl_wait_set_add_guard_condition(wait_set, &gc_, nullptr)) { RCLCPP_WARN(rclcpp::get_logger("fuse"), "Could not add callback waitable to wait set."); } @@ -111,7 +111,7 @@ 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) +void CallbackAdapter::execute(std::shared_ptr& data) { if (!data) { diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 373b39b20..16f8a98c1 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -87,7 +87,7 @@ #include -static auto const sensorProcClock = rclcpp::Clock(); +static auto sensorProcClock = rclcpp::Clock(); namespace tf2 { diff --git a/fuse_models/src/acceleration_2d.cpp b/fuse_models/src/acceleration_2d.cpp index e720ec010..85a914eaf 100644 --- a/fuse_models/src/acceleration_2d.cpp +++ b/fuse_models/src/acceleration_2d.cpp @@ -84,10 +84,7 @@ void Acceleration2D::onInit() } 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_); } void Acceleration2D::onStart() diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index db74fb258..3c757a91b 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -67,9 +67,11 @@ void GraphIgnition::onInit() // Connect to the reset service 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_); + 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().get_rmw_qos_profile(), cb_group_); } // Advertise @@ -84,7 +86,7 @@ void GraphIgnition::onInit() 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_); + rclcpp::ServicesQoS().get_rmw_qos_profile(), cb_group_); } void GraphIgnition::start() diff --git a/fuse_models/src/imu_2d.cpp b/fuse_models/src/imu_2d.cpp index 2521f5dfb..f4aeb51f8 100644 --- a/fuse_models/src/imu_2d.cpp +++ b/fuse_models/src/imu_2d.cpp @@ -91,10 +91,7 @@ void Imu2D::onInit() } 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_); } void Imu2D::onStart() diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp index 1a12217cf..492bae9b8 100644 --- a/fuse_models/src/imu_3d.cpp +++ b/fuse_models/src/imu_3d.cpp @@ -91,10 +91,7 @@ void Imu3D::onInit() } 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_); } void Imu3D::onStart() diff --git a/fuse_models/src/odometry_2d.cpp b/fuse_models/src/odometry_2d.cpp index e9f5d43b5..73582ef49 100644 --- a/fuse_models/src/odometry_2d.cpp +++ b/fuse_models/src/odometry_2d.cpp @@ -90,10 +90,7 @@ void Odometry2D::onInit() } 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_); } void Odometry2D::onStart() diff --git a/fuse_models/src/odometry_2d_publisher.cpp b/fuse_models/src/odometry_2d_publisher.cpp index e8a67f04e..c99d6708e 100644 --- a/fuse_models/src/odometry_2d_publisher.cpp +++ b/fuse_models/src/odometry_2d_publisher.cpp @@ -96,10 +96,7 @@ void Odometry2DPublisher::onInit() // 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_); } // Advertise the topics diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp index cd05a3c22..53c115c53 100644 --- a/fuse_models/src/odometry_3d.cpp +++ b/fuse_models/src/odometry_3d.cpp @@ -90,10 +90,7 @@ void Odometry3D::onInit() } 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_); } void Odometry3D::onStart() diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 202161b46..980defaf6 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -102,10 +102,7 @@ void Odometry3DPublisher::onInit() // 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_); } // Advertise the topics diff --git a/fuse_models/src/omnidirectional_3d_ignition.cpp b/fuse_models/src/omnidirectional_3d_ignition.cpp index a70c08316..5e37654dd 100644 --- a/fuse_models/src/omnidirectional_3d_ignition.cpp +++ b/fuse_models/src/omnidirectional_3d_ignition.cpp @@ -96,9 +96,11 @@ void Omnidirectional3DIgnition::onInit() // Connect to the reset service 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_); + 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().get_rmw_qos_profile(), cb_group_); } // Advertise @@ -113,13 +115,13 @@ void Omnidirectional3DIgnition::onInit() fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_service), std::bind(&Omnidirectional3DIgnition::setPoseServiceCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), - rclcpp::ServicesQoS(), cb_group_); + rclcpp::ServicesQoS().get_rmw_qos_profile(), 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(&Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), - rclcpp::ServicesQoS(), cb_group_); + rclcpp::ServicesQoS().get_rmw_qos_profile(), cb_group_); } void Omnidirectional3DIgnition::start() diff --git a/fuse_models/src/pose_2d.cpp b/fuse_models/src/pose_2d.cpp index 53ae1ce51..82b2010ab 100644 --- a/fuse_models/src/pose_2d.cpp +++ b/fuse_models/src/pose_2d.cpp @@ -87,10 +87,7 @@ void Pose2D::onInit() } 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_); } void Pose2D::onStart() diff --git a/fuse_models/src/transform_sensor.cpp b/fuse_models/src/transform_sensor.cpp index 176b90ee8..6e41bcdc5 100644 --- a/fuse_models/src/transform_sensor.cpp +++ b/fuse_models/src/transform_sensor.cpp @@ -90,10 +90,7 @@ void TransformSensor::onInit() } 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_); } void TransformSensor::onStart() diff --git a/fuse_models/src/twist_2d.cpp b/fuse_models/src/twist_2d.cpp index 83bf129de..4574c5517 100644 --- a/fuse_models/src/twist_2d.cpp +++ b/fuse_models/src/twist_2d.cpp @@ -84,10 +84,7 @@ void Twist2D::onInit() } 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_); } void Twist2D::onStart() diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index 1bec4e0fa..4b40dade5 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -94,9 +94,11 @@ void Unicycle2DIgnition::onInit() // Connect to the reset service 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_); + 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().get_rmw_qos_profile(), cb_group_); } // Advertise @@ -111,13 +113,13 @@ void Unicycle2DIgnition::onInit() 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_); + rclcpp::ServicesQoS().get_rmw_qos_profile(), 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_); + rclcpp::ServicesQoS().get_rmw_qos_profile(), cb_group_); } void Unicycle2DIgnition::start() diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index f8816a299..e853fdc47 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -103,7 +103,7 @@ FixedLagSmoother::FixedLagSmoother(fuse_core::node_interfaces::NodeInterfacesget_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()); + rclcpp::ServicesQoS().get_rmw_qos_profile(), interfaces_.get_node_base_interface()->get_default_callback_group()); } FixedLagSmoother::~FixedLagSmoother() diff --git a/fuse_publishers/src/pose_2d_publisher.cpp b/fuse_publishers/src/pose_2d_publisher.cpp index 5291c1aa8..67c014773 100644 --- a/fuse_publishers/src/pose_2d_publisher.cpp +++ b/fuse_publishers/src/pose_2d_publisher.cpp @@ -32,6 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include #include @@ -178,10 +179,7 @@ void Pose2DPublisher::onInit() // , 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()); + tf_listener_ = std::make_unique(*tf_buffer_, &interfaces_); } } diff --git a/fuse_tutorials/src/apriltag_simulator.cpp b/fuse_tutorials/src/apriltag_simulator.cpp index 43e952834..6a70c62c2 100644 --- a/fuse_tutorials/src/apriltag_simulator.cpp +++ b/fuse_tutorials/src/apriltag_simulator.cpp @@ -277,7 +277,8 @@ void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces( interfaces.get_node_base_interface(), interfaces.get_node_graph_interface(), - interfaces.get_node_services_interface(), "/state_estimation/set_pose_service", rclcpp::ServicesQoS()); + interfaces.get_node_services_interface(), "/state_estimation/set_pose_service", + rclcpp::ServicesQoS().get_rmw_qos_profile(), interfaces.get_node_base_interface()->get_default_callback_group()); while (!client->wait_for_service(std::chrono::seconds(30)) && interfaces.get_node_base_interface()->get_context()->is_valid()) diff --git a/fuse_tutorials/src/range_sensor_simulator.cpp b/fuse_tutorials/src/range_sensor_simulator.cpp index 89cb88d52..34f0f429a 100644 --- a/fuse_tutorials/src/range_sensor_simulator.cpp +++ b/fuse_tutorials/src/range_sensor_simulator.cpp @@ -124,8 +124,7 @@ 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, rclcpp::Clock& clock) { auto msg = std::make_shared(); msg->header.stamp = clock.now(); @@ -221,10 +220,10 @@ void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfacespose.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().get_rmw_qos_profile(), interfaces.get_node_base_interface()->get_default_callback_group()); while (!client->wait_for_service(std::chrono::seconds(30)) && interfaces.get_node_base_interface()->get_context()->is_valid()) diff --git a/fuse_tutorials/src/three_dimensional_simulator.cpp b/fuse_tutorials/src/three_dimensional_simulator.cpp index 52dd53490..a4b3bd1a5 100644 --- a/fuse_tutorials/src/three_dimensional_simulator.cpp +++ b/fuse_tutorials/src/three_dimensional_simulator.cpp @@ -253,7 +253,8 @@ void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces( interfaces.get_node_base_interface(), interfaces.get_node_graph_interface(), - interfaces.get_node_services_interface(), "/state_estimation/set_pose_service", rclcpp::ServicesQoS()); + interfaces.get_node_services_interface(), "/state_estimation/set_pose_service", + rclcpp::ServicesQoS().get_rmw_qos_profile(), interfaces.get_node_base_interface()->get_default_callback_group()); while (!client->wait_for_service(std::chrono::seconds(30)) && interfaces.get_node_base_interface()->get_context()->is_valid()) From fe832493bcd930517d69c659fd8d70ad58003ce2 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 12 Dec 2024 23:40:04 +0000 Subject: [PATCH 7/8] don't run clang tidy on humble main branch either --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index e48237896..3102f8898 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -74,7 +74,7 @@ jobs: working-directory: /colcon_ws clang_tidy: - if: github.ref != 'refs/heads/main' + if: github.ref != 'refs/heads/humble' needs: # Ensure the test job runs after the build job finishes instead of attempting to run in parallel - build-ws From ebc65661bb1d39a99221ccffb0fe73b1317af660 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 13 Dec 2024 16:11:30 -0700 Subject: [PATCH 8/8] fix uninitialized acceleration and overwritten jacobian --- .../include/fuse_models/omnidirectional_3d_predict.hpp | 4 +++- fuse_models/src/odometry_3d_publisher.cpp | 4 ++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp index ab64cd352..856882e6c 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp @@ -493,7 +493,9 @@ inline void predict(const fuse_core::Vector3d& position1, const Eigen::Quaternio vel_linear2.y(), vel_linear2.z(), vel_angular2.x(), vel_angular2.y(), vel_angular2.z(), acc_linear2.x(), acc_linear2.y(), acc_linear2.z(), jacobians.data(), jacobian_quat2rpy); - jacobian << J[0], J[1], J[2], J[3], J[4]; + // TODO(henrygerardmoore): figure out how to fix this + // see https://github.com/locusrobotics/fuse/pull/354#discussion_r1884288806 + jacobian << J[0], J[1], J[2], J[3], J[4].block<15, 2>(0, 0); // Convert back to quaternion orientation2 = Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) * diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 980defaf6..e49359421 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -465,6 +465,10 @@ void Odometry3DPublisher::predict(tf2::Transform& pose, nav_msgs::msg::Odometry& acceleration_linear << acceleration_output.accel.accel.linear.x, acceleration_output.accel.accel.linear.y, acceleration_output.accel.accel.linear.z; } + else + { + acceleration_linear = fuse_core::Vector3d::Zero(); + } ::fuse_models::predict(position, orientation, velocity_linear, velocity_angular, acceleration_linear, dt, position, orientation, velocity_linear, velocity_angular, acceleration_linear, jacobian);