local_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_odometry.hpp>
#include <px4_ros2/common/frame.hpp>
#include <px4_ros2/navigation/experimental/navigation_interface_base.hpp>
#include <rclcpp/rclcpp.hpp>
 
namespace px4_ros2 {
 
using AuxLocalPosition = px4_msgs::msg::VehicleOdometry;
 
struct LocalPositionMeasurement {
  rclcpp::Time timestamp_sample{};
 
  std::optional<Eigen::Vector2f> position_xy{std::nullopt};
  std::optional<Eigen::Vector2f> position_xy_variance{std::nullopt};
  std::optional<float> position_z{std::nullopt};
  std::optional<float> position_z_variance{std::nullopt};
 
  std::optional<Eigen::Vector2f> velocity_xy{std::nullopt};
  std::optional<Eigen::Vector2f> velocity_xy_variance{std::nullopt};
  std::optional<float> velocity_z{std::nullopt};
  std::optional<float> velocity_z_variance{std::nullopt};
 
  std::optional<Eigen::Quaternionf> attitude_quaternion{std::nullopt};
  std::optional<Eigen::Vector3f> attitude_variance{std::nullopt};
};
 
class LocalPositionMeasurementInterface : public PositionMeasurementInterfaceBase {
 public:
  explicit LocalPositionMeasurementInterface(rclcpp::Node& node, PoseFrame pose_frame,
                                             VelocityFrame velocity_frame,
                                             std::string topic_namespace_prefix = "");
  ~LocalPositionMeasurementInterface() override = default;
 
  void update(const LocalPositionMeasurement& local_position_measurement) const;
 
 private:
  bool isMeasurementNonEmpty(const LocalPositionMeasurement& measurement) const;
 
  bool isVarianceValid(const LocalPositionMeasurement& measurement) const;
 
  bool isFrameValid(const LocalPositionMeasurement& measurement) const;
 
  bool isValueNotNAN(const LocalPositionMeasurement& measurement) const;
 
  rclcpp::Publisher<AuxLocalPosition>::SharedPtr _aux_local_position_pub;
 
  const uint8_t _pose_frame;
  const uint8_t _velocity_frame;
};
 
}  // namespace px4_ros2