frame.hpp

/****************************************************************************
 * Copyright (c) 2023 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <px4_msgs/msg/vehicle_odometry.hpp>
 
namespace px4_ros2 {
 
enum class PoseFrame { Unknown, LocalNED, LocalFRD };
 
enum class VelocityFrame { Unknown, LocalNED, LocalFRD, BodyFRD };
 
constexpr inline uint8_t poseFrameToMessageFrame(px4_ros2::PoseFrame frame) noexcept
{
  switch (frame) {
    case PoseFrame::Unknown:
      return px4_msgs::msg::VehicleOdometry::POSE_FRAME_UNKNOWN;
 
    case PoseFrame::LocalNED:
      return px4_msgs::msg::VehicleOdometry::POSE_FRAME_NED;
 
    case PoseFrame::LocalFRD:
      return px4_msgs::msg::VehicleOdometry::POSE_FRAME_FRD;
  }
 
  return px4_msgs::msg::VehicleOdometry::POSE_FRAME_UNKNOWN;
}
 
constexpr inline uint8_t velocityFrameToMessageFrame(px4_ros2::VelocityFrame frame) noexcept
{
  switch (frame) {
    case VelocityFrame::Unknown:
      return px4_msgs::msg::VehicleOdometry::VELOCITY_FRAME_UNKNOWN;
 
    case VelocityFrame::LocalNED:
      return px4_msgs::msg::VehicleOdometry::VELOCITY_FRAME_NED;
 
    case VelocityFrame::LocalFRD:
      return px4_msgs::msg::VehicleOdometry::VELOCITY_FRAME_FRD;
 
    case VelocityFrame::BodyFRD:
      return px4_msgs::msg::VehicleOdometry::VELOCITY_FRAME_BODY_FRD;
  }
 
  return px4_msgs::msg::VehicleOdometry::POSE_FRAME_UNKNOWN;
}
 
}  // namespace px4_ros2