PX4 ROS 2 Interface Library
Library to interface with PX4 from a companion computer using ROS 2
global_position.hpp
1 /****************************************************************************
2  * Copyright (c) 2023 PX4 Development Team.
3  * SPDX-License-Identifier: BSD-3-Clause
4  ****************************************************************************/
5 
6 #pragma once
7 
8 #include <Eigen/Eigen>
9 #include <px4_msgs/msg/vehicle_global_position.hpp>
10 #include <px4_ros2/common/context.hpp>
11 #include <px4_ros2/utils/subscription.hpp>
12 
13 namespace px4_ros2
14 {
22 class OdometryGlobalPosition : public Subscription<px4_msgs::msg::VehicleGlobalPosition>
23 {
24 public:
25  explicit OdometryGlobalPosition(Context & context);
26 
30  bool positionValid() const
31  {
32  return lastValid() && last().lat_lon_valid && last().alt_valid;
33  }
34 
40  Eigen::Vector3d position() const
41  {
42  const px4_msgs::msg::VehicleGlobalPosition & pos = last();
43  return {pos.lat, pos.lon, pos.alt};
44  }
45 };
46 
48 } /* namespace px4_ros2 */
Definition: context.hpp:20
Provides access to the vehicle's global position estimate.
Definition: global_position.hpp:23
Eigen::Vector3d position() const
Get the vehicle's global position.
Definition: global_position.hpp:40
bool positionValid() const
Definition: global_position.hpp:30
Provides a subscription to arbitrary ROS topics.
Definition: subscription.hpp:23
bool lastValid(const std::chrono::duration< int64_t, DurationT > max_delay=500ms) const
Check whether the last message is still valid. To be valid, the message must have been received withi...
Definition: subscription.hpp:84
const px4_msgs::msg::VehicleGlobalPosition & last() const
Get the last-received message.
Definition: subscription.hpp:58