PX4 ROS 2 Interface Library
Library to interface with PX4 from a companion computer using ROS 2
local_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_local_position.hpp>
10 #include <px4_ros2/common/context.hpp>
11 #include <px4_ros2/utils/subscription.hpp>
12 
13 namespace px4_ros2
14 {
22 class OdometryLocalPosition : public Subscription<px4_msgs::msg::VehicleLocalPosition>
23 {
24 public:
31  explicit OdometryLocalPosition(Context & context, bool local_position_is_optional = false);
32 
33  bool positionXYValid() const
34  {
35  return lastValid() && last().xy_valid;
36  }
37 
38  bool positionZValid() const
39  {
40  return lastValid() && last().z_valid;
41  }
42 
43  Eigen::Vector3f positionNed() const
44  {
45  const px4_msgs::msg::VehicleLocalPosition & pos = last();
46  return {pos.x, pos.y, pos.z};
47  }
48 
49  bool velocityXYValid() const
50  {
51  return lastValid() && last().v_xy_valid;
52  }
53 
54  bool velocityZValid() const
55  {
56  return lastValid() && last().v_z_valid;
57  }
58  Eigen::Vector3f velocityNed() const
59  {
60  const px4_msgs::msg::VehicleLocalPosition & pos = last();
61  return {pos.vx, pos.vy, pos.vz};
62  }
63 
64  Eigen::Vector3f accelerationNed() const
65  {
66  const px4_msgs::msg::VehicleLocalPosition & pos = last();
67  return {pos.ax, pos.ay, pos.az};
68  }
69 
75  float heading() const
76  {
77  const px4_msgs::msg::VehicleLocalPosition & pos = last();
78  return pos.heading;
79  }
80 
81  float distanceGround() const
82  {
83  const px4_msgs::msg::VehicleLocalPosition & pos = last();
84  return pos.dist_bottom;
85  }
86 };
87 
89 } /* namespace px4_ros2 */
Definition: context.hpp:20
Provides access to the vehicle's local position estimate.
Definition: local_position.hpp:23
float heading() const
Get the vehicle's heading relative to NED earth-fixed frame.
Definition: local_position.hpp:75
OdometryLocalPosition(Context &context, bool local_position_is_optional=false)
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::VehicleLocalPosition & last() const
Get the last-received message.
Definition: subscription.hpp:58