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/aux_global_position.hpp>
#include <px4_ros2/navigation/experimental/navigation_interface_base.hpp>
#include <rclcpp/rclcpp.hpp>
 
namespace px4_ros2 {
enum class GlobalPositionSource : uint8_t {
  Unknown = px4_msgs::msg::AuxGlobalPosition::SOURCE_UNKNOWN,
  GNSS = px4_msgs::msg::AuxGlobalPosition::SOURCE_GNSS,
  Vision = px4_msgs::msg::AuxGlobalPosition::SOURCE_VISION,
  Pseudolites = px4_msgs::msg::AuxGlobalPosition::SOURCE_PSEUDOLITES,
  Terrain = px4_msgs::msg::AuxGlobalPosition::SOURCE_TERRAIN,
  Magnetic = px4_msgs::msg::AuxGlobalPosition::SOURCE_MAGNETIC,
  Estimator = px4_msgs::msg::AuxGlobalPosition::SOURCE_ESTIMATOR,
};
 
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, uint8_t id = 1,
      GlobalPositionSource source = GlobalPositionSource::Vision,
      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::AuxGlobalPosition>::SharedPtr _aux_global_position_pub;
 
  const uint8_t _id;
  const GlobalPositionSource _source;
  uint8_t _lat_lon_reset_counter{
      0}; 
};
 
}  // namespace px4_ros2