global_position_measurement_interface.hpp

/****************************************************************************
 * Copyright (c) 2023 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <optional>
#include <rclcpp/rclcpp.hpp>
#include <Eigen/Eigen>
 
#include <px4_msgs/msg/vehicle_global_position.hpp>
#include <px4_ros2/navigation/experimental/navigation_interface_base.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);
  ~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