attitude.hpp

/****************************************************************************
 * Copyright (c) 2024 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <px4_msgs/msg/vehicle_attitude.hpp>
#include <px4_ros2/common/context.hpp>
#include <px4_ros2/utils/geometry.hpp>
#include <px4_ros2/utils/subscription.hpp>
 
namespace px4_ros2
{
class OdometryAttitude : public Subscription<px4_msgs::msg::VehicleAttitude>
{
public:
  explicit OdometryAttitude(Context & context);
 
  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]};
  }
 
  float roll() const
  {
    return quaternionToRoll(attitude());
  }
 
  float pitch() const
  {
    return quaternionToPitch(attitude());
  }
 
  float yaw() const
  {
    return quaternionToYaw(attitude());
  }
};
 
} /* namespace px4_ros2 */