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/subscription.hpp>
#include <px4_ros2/utils/message_version.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 */