global_position_measurement_interface.hpp

/****************************************************************************
 * Copyright (c) 2023 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <optional>
#include <px4_msgs/msg/vehicle_global_position.hpp>
#include <px4_ros2/navigation/experimental/navigation_interface_base.hpp>
#include <rclcpp/rclcpp.hpp>
 
namespace px4_ros2 {
struct GlobalPositionMeasurement {
  rclcpp::Time timestamp_sample{};
 
  std::optional<Eigen::Vector2d> lat_lon{std::nullopt};
  std::optional<float> horizontal_variance{std::nullopt};
 
  std::optional<float> altitude_msl{std::nullopt};
  std::optional<float> vertical_variance{std::nullopt};
};
 
class GlobalPositionMeasurementInterface : public PositionMeasurementInterfaceBase {
 public:
  explicit GlobalPositionMeasurementInterface(rclcpp::Node& node,
                                              std::string topic_namespace_prefix = "");
  ~GlobalPositionMeasurementInterface() override = default;
 
  void update(const GlobalPositionMeasurement& global_position_measurement) const;
 
  inline void reset() { ++_lat_lon_reset_counter; }
 
 private:
  bool isMeasurementNonEmpty(const GlobalPositionMeasurement& measurement) const;
 
  bool isVarianceValid(const GlobalPositionMeasurement& measurement) const;
 
  bool isFrameValid(const GlobalPositionMeasurement& measurement) const;
 
  bool isValueNotNAN(const GlobalPositionMeasurement& measurement) const;
 
  rclcpp::Publisher<px4_msgs::msg::VehicleGlobalPosition>::SharedPtr _aux_global_position_pub;
 
  uint8_t _lat_lon_reset_counter{
      0}; 
  // uint8_t _altitude_frame;
};
 
}  // namespace px4_ros2