odometry.hpp

/****************************************************************************
 * Copyright (c) 2023 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <px4_msgs/msg/vehicle_odometry.hpp>
#include <px4_ros2/common/context.hpp>
#include <px4_ros2/utils/geometry.hpp>
#include <px4_ros2/utils/subscription.hpp>
 
namespace px4_ros2 {
class Odometry : public Subscription<px4_msgs::msg::VehicleOdometry> {
 public:
  explicit Odometry(Context& context);
 
  Eigen::Vector3f positionNed() const
  {
    const px4_msgs::msg::VehicleOdometry& odom = last();
    return {odom.position[0], odom.position[1], odom.position[2]};
  }
 
  Eigen::Vector3f velocityNed() const
  {
    const px4_msgs::msg::VehicleOdometry& odom = last();
    return {odom.velocity[0], odom.velocity[1], odom.velocity[2]};
  }
 
  Eigen::Vector3f angularVelocityFrd() const
  {
    const px4_msgs::msg::VehicleOdometry& odom = last();
    return {odom.angular_velocity[0], odom.angular_velocity[1], odom.angular_velocity[2]};
  }
 
  Eigen::Quaternionf attitude() const
  {
    const px4_msgs::msg::VehicleOdometry& odom = last();
    return Eigen::Quaternionf{odom.q[0], odom.q[1], odom.q[2], odom.q[3]};
  }
 
  float roll() const { return quaternionToRoll(attitude()); }
 
  float pitch() const { return quaternionToPitch(attitude()); }
 
  float yaw() const { return quaternionToYaw(attitude()); }
};
 
} /* namespace px4_ros2 */