home_position.hpp

/****************************************************************************
 * Copyright (c) 2023-2024 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <px4_msgs/msg/home_position.hpp>
#include <px4_ros2/common/context.hpp>
#include <px4_ros2/utils/message_version.hpp>
#include <px4_ros2/utils/subscription.hpp>
 
namespace px4_ros2 {
class HomePosition : public Subscription<px4_msgs::msg::HomePosition> {
 public:
  explicit HomePosition(Context& context)
      : Subscription<px4_msgs::msg::HomePosition>(
            context, "fmu/out/home_position" +
                         +px4_ros2::getMessageNameVersion<px4_msgs::msg::HomePosition>())
  {
  }
 
  Eigen::Vector3f localPosition() const
  {
    const px4_msgs::msg::HomePosition& home = last();
    return Eigen::Vector3f{home.x, home.y, home.z};
  }
 
  Eigen::Vector3d globalPosition() const
  {
    const px4_msgs::msg::HomePosition& home = last();
    return Eigen::Vector3d{home.lat, home.lon, home.alt};
  }
 
  float yaw() const { return last().yaw; }
 
  bool localPositionValid() const { return lastValid() && last().valid_lpos; }
 
  bool globaHorizontalPositionValid() const { return lastValid() && last().valid_hpos; }
 
  bool altitudeValid() const { return lastValid() && last().valid_alt; }
 
  bool manualHome() const { return last().manual_home; }
};
 
} /* namespace px4_ros2 */