diff --git a/px4_ros2_cpp/CMakeLists.txt b/px4_ros2_cpp/CMakeLists.txt index 6e4b0165..5e19794c 100644 --- a/px4_ros2_cpp/CMakeLists.txt +++ b/px4_ros2_cpp/CMakeLists.txt @@ -41,6 +41,7 @@ set(HEADER_FILES include/px4_ros2/navigation/experimental/global_position_measurement_interface.hpp include/px4_ros2/navigation/experimental/local_position_measurement_interface.hpp include/px4_ros2/navigation/experimental/navigation_interface_base.hpp + include/px4_ros2/odometry/attitude.hpp include/px4_ros2/odometry/global_position.hpp include/px4_ros2/odometry/local_position.hpp include/px4_ros2/utils/frame_conversion.hpp @@ -66,6 +67,7 @@ add_library(px4_ros2_cpp src/control/setpoint_types/experimental/trajectory.cpp src/navigation/experimental/global_position_measurement_interface.cpp src/navigation/experimental/local_position_measurement_interface.cpp + src/odometry/attitude.cpp src/odometry/global_position.cpp src/odometry/local_position.cpp src/utils/geodesic.cpp diff --git a/px4_ros2_cpp/include/px4_ros2/odometry/attitude.hpp b/px4_ros2_cpp/include/px4_ros2/odometry/attitude.hpp new file mode 100644 index 00000000..8fc20890 --- /dev/null +++ b/px4_ros2_cpp/include/px4_ros2/odometry/attitude.hpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +namespace px4_ros2 +{ +/** \ingroup odometry + * @{ + */ + +/** + * Provides access to the vehicle's attitude estimate + */ +class OdometryAttitude : public Subscription +{ +public: + explicit OdometryAttitude(Context & context); + + /** + * @brief Get the vehicle's attitude. + * + * @return the attitude quaternion + */ + Eigen::Quaternionf attitude() const + { + const px4_msgs::msg::VehicleAttitude & att = last(); + return Eigen::Quaternionf{att.q[0], att.q[1], att.q[2], att.q[3]}; + } + + /** + * @brief Get the vehicle's roll in extrinsic RPY order. + * + * @return the attitude roll in radians within [-pi, pi] + */ + float roll() const + { + const px4_msgs::msg::VehicleAttitude & att = last(); + const Eigen::Quaternionf q{att.q[0], att.q[1], att.q[2], att.q[3]}; + return quaternionToRoll(q); + } + + /** + * @brief Get the vehicle's pitch in extrinsic RPY order. + * + * @return the attitude pitch in radians within [-pi, pi] + */ + float pitch() const + { + const px4_msgs::msg::VehicleAttitude & att = last(); + const Eigen::Quaternionf q{att.q[0], att.q[1], att.q[2], att.q[3]}; + return quaternionToPitch(q); + } + + /** + * @brief Get the vehicle's yaw in extrinsic RPY order. + * + * @return the attitude yaw in radians within [-pi, pi] + */ + float yaw() const + { + const px4_msgs::msg::VehicleAttitude & att = last(); + const Eigen::Quaternionf q{att.q[0], att.q[1], att.q[2], att.q[3]}; + return quaternionToYaw(q); + } +}; + +/** @}*/ +} /* namespace px4_ros2 */ 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 97db8987..9372c0cd 100644 --- a/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp +++ b/px4_ros2_cpp/include/px4_ros2/odometry/local_position.hpp @@ -60,6 +60,17 @@ class OdometryLocalPosition : public Subscription + +namespace px4_ros2 +{ + +OdometryAttitude::OdometryAttitude(Context & context) +: Subscription(context, "/fmu/out/vehicle_attitude") +{ + RequirementFlags requirements{}; + requirements.attitude = true; + context.setRequirement(requirements); +} + +} // namespace px4_ros2