From 2463ae8c4d3f5aa2c61eccd661c5c5f951efa181 Mon Sep 17 00:00:00 2001 From: Daniel Mesham Date: Thu, 14 Dec 2023 17:34:04 +0100 Subject: [PATCH] odometry: use subscription class for global and local position APIs --- .../px4_ros2/odometry/global_position.hpp | 7 +-- .../px4_ros2/odometry/local_position.hpp | 6 +- .../px4_ros2/odometry/subscription.hpp | 57 +++++++++++++------ px4_ros2_cpp/src/odometry/global_position.cpp | 12 ++-- px4_ros2_cpp/src/odometry/local_position.cpp | 12 ++-- 5 files changed, 55 insertions(+), 39 deletions(-) diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp index 0225827..c320d93 100644 --- a/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp +++ b/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp @@ -5,11 +5,10 @@ #pragma once -#include -#include #include +#include #include - +#include using namespace std::chrono_literals; // NOLINT @@ -40,7 +39,7 @@ class OdometryGlobalPosition private: rclcpp::Node & _node; - rclcpp::Subscription::SharedPtr _vehicle_global_position_sub; + Subscription _vehicle_global_position_sub; px4_msgs::msg::VehicleGlobalPosition _vehicle_global_position; rclcpp::Time _last_vehicle_global_position; }; diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp index e230ffa..894b1a9 100644 --- a/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp +++ b/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp @@ -5,13 +5,11 @@ #pragma once -#include -#include #include +#include #include #include - namespace px4_ros2 { /** \ingroup odometry @@ -61,7 +59,7 @@ class OdometryLocalPosition } private: - rclcpp::Subscription::SharedPtr _vehicle_local_position_sub; + Subscription _vehicle_local_position_sub; px4_msgs::msg::VehicleLocalPosition _vehicle_local_position; }; diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp index ace2dcc..4c253a1 100644 --- a/px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp +++ b/px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp @@ -7,28 +7,51 @@ #include -namespace px4_ros2 { +namespace px4_ros2 +{ +/** \ingroup odometry + * @{ + */ -template class Subscription { +/** + * Provides a subscription to arbitrary ROS topics. + */ +template +class Subscription +{ public: - Subscription(Context &context, const std::string &topic) - : _node(context.node()), _topic(context.topicNamespacePrefix() + topic) { + Subscription(Context & context, const std::string & topic) + : _node(context.node()), _topic(context.topicNamespacePrefix() + topic) + { _subscription = _node.create_subscription( - _topic, rclcpp::QoS(1).best_effort(), - [this](const typename RosMessageType::UniquePtr msg) { - _last = *msg; - _last_message_time = _node.get_clock()->now(); - for (const auto& callback : _callbacks) { - callback(_last); - } - }); + _topic, rclcpp::QoS(1).best_effort(), + [this](const typename RosMessageType::UniquePtr msg) { + _last = *msg; + _last_message_time = _node.get_clock()->now(); + for (const auto & callback : _callbacks) { + callback(_last); + } + }); } - void subscribe(std::function callback) { + /** + * @brief Register a callback to be executed for each message. + * + * @param callback the callback function + */ + void subscribe(std::function callback) + { _callbacks.push_back(callback); } - RosMessageType last() const { + /** + * @brief Get the last-received message. + * + * @returns the last-received ROS message + * @throws std::runtime_error when no messages have been received + */ + RosMessageType last() const + { if (_last_message_time.seconds() == 0) { throw std::runtime_error("No messages received."); } @@ -36,11 +59,10 @@ template class Subscription { } private: - rclcpp::Node &_node; + rclcpp::Node & _node; std::string _topic; - typename rclcpp::Subscription::SharedPtr _subscription{ - nullptr}; + typename rclcpp::Subscription::SharedPtr _subscription{nullptr}; RosMessageType _last; rclcpp::Time _last_message_time; @@ -48,4 +70,5 @@ template class Subscription { std::vector> _callbacks{}; }; +/** @}*/ } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/odometry/global_position.cpp b/px4_ros2_cpp/src/odometry/global_position.cpp index 8e01b9a..e045387 100644 --- a/px4_ros2_cpp/src/odometry/global_position.cpp +++ b/px4_ros2_cpp/src/odometry/global_position.cpp @@ -5,20 +5,17 @@ #include - namespace px4_ros2 { OdometryGlobalPosition::OdometryGlobalPosition(Context & context) : _node(context.node()), + _vehicle_global_position_sub(context, "/fmu/out/vehicle_global_position"), _last_vehicle_global_position(_node.get_clock()->now() - 1s) { - _vehicle_global_position_sub = - context.node().create_subscription( - context.topicNamespacePrefix() + "/fmu/out/vehicle_global_position", rclcpp::QoS( - 1).best_effort(), - [this, context](px4_msgs::msg::VehicleGlobalPosition::UniquePtr msg) { - _vehicle_global_position = *msg; + _vehicle_global_position_sub.subscribe( + [this](const px4_msgs::msg::VehicleGlobalPosition msg) { + _vehicle_global_position = msg; _last_vehicle_global_position = _node.get_clock()->now(); }); @@ -26,4 +23,5 @@ OdometryGlobalPosition::OdometryGlobalPosition(Context & context) requirements.global_position = true; context.setRequirement(requirements); } + } // namespace px4_ros2 diff --git a/px4_ros2_cpp/src/odometry/local_position.cpp b/px4_ros2_cpp/src/odometry/local_position.cpp index 01aa7fe..a8be336 100644 --- a/px4_ros2_cpp/src/odometry/local_position.cpp +++ b/px4_ros2_cpp/src/odometry/local_position.cpp @@ -5,18 +5,15 @@ #include - namespace px4_ros2 { OdometryLocalPosition::OdometryLocalPosition(Context & context) +: _vehicle_local_position_sub(context, "/fmu/out/vehicle_local_position") { - _vehicle_local_position_sub = - context.node().create_subscription( - context.topicNamespacePrefix() + "/fmu/out/vehicle_local_position", rclcpp::QoS( - 1).best_effort(), - [this](px4_msgs::msg::VehicleLocalPosition::UniquePtr msg) { - _vehicle_local_position = *msg; + _vehicle_local_position_sub.subscribe( + [this](const px4_msgs::msg::VehicleLocalPosition msg) { + _vehicle_local_position = msg; }); RequirementFlags requirements{}; @@ -24,4 +21,5 @@ OdometryLocalPosition::OdometryLocalPosition(Context & context) requirements.local_alt = true; context.setRequirement(requirements); } + } // namespace px4_ros2