local_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_odometry.hpp>
#include <px4_ros2/common/frame.hpp>
#include <px4_ros2/navigation/experimental/navigation_interface_base.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);
  ~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