From 0548a02010f4850dc997c3499c4cd465ea7714a8 Mon Sep 17 00:00:00 2001 From: Daniel Mesham Date: Fri, 15 Dec 2023 11:22:41 +0100 Subject: [PATCH] odometry: naming updates and tidying up --- .../px4_ros2/odometry/global_position.hpp | 19 ++------- .../px4_ros2/odometry/local_position.hpp | 25 ++++++------ .../px4_ros2/odometry/subscription.hpp | 39 +++++++++++++++---- px4_ros2_cpp/src/odometry/global_position.cpp | 10 +---- px4_ros2_cpp/src/odometry/local_position.cpp | 7 +--- 5 files changed, 49 insertions(+), 51 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 c320d93..164dea4 100644 --- a/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp +++ b/px4_ros2_cpp/include/px4_ros2/odometry/global_position.hpp @@ -10,8 +10,6 @@ #include #include -using namespace std::chrono_literals; // NOLINT - namespace px4_ros2 { /** \ingroup odometry @@ -21,27 +19,16 @@ namespace px4_ros2 /** * Provides access to the vehicle's global position estimate */ -class OdometryGlobalPosition +class OdometryGlobalPosition : public Subscription { public: explicit OdometryGlobalPosition(Context & context); - bool valid() - { - return _node.get_clock()->now() - _last_vehicle_global_position < 500ms; - } - Eigen::Vector3d position() const { - return {_vehicle_global_position.lat, _vehicle_global_position.lon, - _vehicle_global_position.alt}; + const px4_msgs::msg::VehicleGlobalPosition pos = last(); + return {pos.lat, pos.lon, pos.alt}; } - -private: - rclcpp::Node & _node; - 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 894b1a9..a0afb01 100644 --- a/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp +++ b/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp @@ -19,48 +19,47 @@ namespace px4_ros2 /** * Provides access to the vehicle's local position estimate */ -class OdometryLocalPosition +class OdometryLocalPosition : public Subscription { public: explicit OdometryLocalPosition(Context & context); - bool positionXyValid() const + bool positionXYValid() const { - return _vehicle_local_position.xy_valid; + return last().xy_valid; } bool positionZValid() const { - return _vehicle_local_position.z_valid; + return last().z_valid; } Eigen::Vector3f position() const { - return {_vehicle_local_position.x, _vehicle_local_position.y, _vehicle_local_position.z}; + const px4_msgs::msg::VehicleLocalPosition pos = last(); + return {pos.x, pos.y, pos.z}; } bool velocityXYValid() const { - return _vehicle_local_position.v_xy_valid; + return last().v_xy_valid; } bool velocityZValid() const { - return _vehicle_local_position.v_z_valid; + return last().v_z_valid; } Eigen::Vector3f velocity() const { - return {_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz}; + const px4_msgs::msg::VehicleLocalPosition pos = last(); + return {pos.vx, pos.vy, pos.vz}; } Eigen::Vector3f acceleration() const { - return {_vehicle_local_position.ax, _vehicle_local_position.ay, _vehicle_local_position.az}; + const px4_msgs::msg::VehicleLocalPosition pos = last(); + return {pos.ax, pos.ay, pos.az}; } - -private: - 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 4c253a1..7faf97b 100644 --- a/px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp +++ b/px4_ros2_cpp/include/px4_ros2/odometry/subscription.hpp @@ -7,6 +7,8 @@ #include +using namespace std::chrono_literals; // NOLINT + namespace px4_ros2 { /** \ingroup odometry @@ -21,10 +23,11 @@ class Subscription { public: Subscription(Context & context, const std::string & topic) - : _node(context.node()), _topic(context.topicNamespacePrefix() + topic) + : _node(context.node()) { + const std::string namespaced_topic = context.topicNamespacePrefix() + topic; _subscription = _node.create_subscription( - _topic, rclcpp::QoS(1).best_effort(), + namespaced_topic, rclcpp::QoS(1).best_effort(), [this](const typename RosMessageType::UniquePtr msg) { _last = *msg; _last_message_time = _node.get_clock()->now(); @@ -35,11 +38,11 @@ class Subscription } /** - * @brief Register a callback to be executed for each message. + * @brief Add a callback to execute when receiving a new message. * * @param callback the callback function */ - void subscribe(std::function callback) + void onUpdate(std::function callback) { _callbacks.push_back(callback); } @@ -50,7 +53,7 @@ class Subscription * @returns the last-received ROS message * @throws std::runtime_error when no messages have been received */ - RosMessageType last() const + const RosMessageType & last() const { if (_last_message_time.seconds() == 0) { throw std::runtime_error("No messages received."); @@ -58,16 +61,38 @@ class Subscription return _last; } + /** + * @brief Get the receive-time of the last message. + * + * @returns the time at which the last ROS message was received + */ + const rclcpp::Time & lastTime() const + { + return _last_message_time; + } + + /** + * @brief Check whether the last message is still valid. + * To be valid, the message must have been received within a given time of the current time. + * + * @param max_delay the maximum delay between the current time and when the message was received + * @return true if the last message was received within the maximum delay + */ + template + bool lastValid(const std::chrono::duration max_delay = 500ms) const + { + return _node.get_clock()->now() - _last_message_time < max_delay; + } + private: rclcpp::Node & _node; - std::string _topic; typename rclcpp::Subscription::SharedPtr _subscription{nullptr}; RosMessageType _last; rclcpp::Time _last_message_time; - std::vector> _callbacks{}; + std::vector> _callbacks{}; }; /** @}*/ diff --git a/px4_ros2_cpp/src/odometry/global_position.cpp b/px4_ros2_cpp/src/odometry/global_position.cpp index e045387..aadba58 100644 --- a/px4_ros2_cpp/src/odometry/global_position.cpp +++ b/px4_ros2_cpp/src/odometry/global_position.cpp @@ -9,16 +9,8 @@ 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) +: Subscription(context, "/fmu/out/vehicle_global_position") { - _vehicle_global_position_sub.subscribe( - [this](const px4_msgs::msg::VehicleGlobalPosition msg) { - _vehicle_global_position = msg; - _last_vehicle_global_position = _node.get_clock()->now(); - }); - RequirementFlags requirements{}; requirements.global_position = true; context.setRequirement(requirements); diff --git a/px4_ros2_cpp/src/odometry/local_position.cpp b/px4_ros2_cpp/src/odometry/local_position.cpp index a8be336..5eb70b9 100644 --- a/px4_ros2_cpp/src/odometry/local_position.cpp +++ b/px4_ros2_cpp/src/odometry/local_position.cpp @@ -9,13 +9,8 @@ namespace px4_ros2 { OdometryLocalPosition::OdometryLocalPosition(Context & context) -: _vehicle_local_position_sub(context, "/fmu/out/vehicle_local_position") +: Subscription(context, "/fmu/out/vehicle_local_position") { - _vehicle_local_position_sub.subscribe( - [this](const px4_msgs::msg::VehicleLocalPosition msg) { - _vehicle_local_position = msg; - }); - RequirementFlags requirements{}; requirements.local_position = true; requirements.local_alt = true;