geodesic.hpp

/****************************************************************************
 * Copyright (c) 2024 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <cmath>
#include <Eigen/Eigen>
#include <rclcpp/rclcpp.hpp>
#include <px4_msgs/msg/vehicle_local_position.hpp>
#include <px4_ros2/common/context.hpp>
 
namespace px4_ros2
{
class MapProjectionImpl;
 
class MapProjection
{
public:
  explicit MapProjection(Context & context);
 
  ~MapProjection();
 
  bool isInitialized() const;
 
  Eigen::Vector2f globalToLocal(const Eigen::Vector2d & global_position) const;
 
  Eigen::Vector3f globalToLocal(const Eigen::Vector3d & global_position) const;
 
  Eigen::Vector2d localToGlobal(const Eigen::Vector2f & local_position) const;
 
  Eigen::Vector3d localToGlobal(const Eigen::Vector3f & local_position) const;
 
private:
  void assertInitalized() const;
 
  void vehicleLocalPositionCallback(px4_msgs::msg::VehicleLocalPosition::UniquePtr msg);
 
  rclcpp::Node & _node;
  std::unique_ptr<MapProjectionImpl> _map_projection_math;
  rclcpp::Subscription<px4_msgs::msg::VehicleLocalPosition>::SharedPtr _vehicle_local_position_sub;
};
 
float horizontalDistanceToGlobalPosition(
  const Eigen::Vector2d & global_position_now,
  const Eigen::Vector2d & global_position_next);
 
static inline float horizontalDistanceToGlobalPosition(
  const Eigen::Vector3d & global_position_now,
  const Eigen::Vector3d & global_position_next)
{
  return horizontalDistanceToGlobalPosition(
    static_cast<Eigen::Vector2d>(global_position_now.head(2)),
    static_cast<Eigen::Vector2d>(global_position_next.head(2)));
}
 
float distanceToGlobalPosition(
  const Eigen::Vector3d & global_position_now,
  const Eigen::Vector3d & global_position_next);
 
float headingToGlobalPosition(
  const Eigen::Vector2d & global_position_now,
  const Eigen::Vector2d & global_position_next);
 
static inline float headingToGlobalPosition(
  const Eigen::Vector3d & global_position_now,
  const Eigen::Vector3d & global_position_next)
{
  return headingToGlobalPosition(
    static_cast<Eigen::Vector2d>(global_position_now.head(2)),
    static_cast<Eigen::Vector2d>(global_position_next.head(2)));
}
 
Eigen::Vector2f vectorToGlobalPosition(
  const Eigen::Vector2d & global_position_now,
  const Eigen::Vector2d & global_position_next);
 
static inline Eigen::Vector3f vectorToGlobalPosition(
  const Eigen::Vector3d & global_position_now,
  const Eigen::Vector3d & global_position_next)
{
  const Eigen::Vector2f vector_res =
    vectorToGlobalPosition(
    static_cast<Eigen::Vector2d>(global_position_now.head(2)),
    static_cast<Eigen::Vector2d>(global_position_next.head(2)));
  const double d_alt = -(global_position_next.z() - global_position_now.z());
  return Eigen::Vector3f {vector_res.x(), vector_res.y(), static_cast<float>(d_alt)};
}
 
Eigen::Vector2d globalPositionFromLineAndDist(
  const Eigen::Vector2d & global_position_line_start,
  const Eigen::Vector2d & global_position_line_end,
  float dist_from_start);
 
Eigen::Vector2d globalPositionFromHeadingAndDist(
  const Eigen::Vector2d & global_position_now,
  float heading, float dist);
 
static inline Eigen::Vector3d globalPositionFromHeadingAndDist(
  const Eigen::Vector3d & global_position_now,
  float heading, float dist)
{
  const Eigen::Vector2d global_position_res = globalPositionFromHeadingAndDist(
    static_cast<Eigen::Vector2d>(global_position_now.head(2)), heading, dist);
  return Eigen::Vector3d {
    global_position_res.x(),
    global_position_res.y(),
    global_position_now.z()};
}
 
Eigen::Vector2d addVectorToGlobalPosition(
  const Eigen::Vector2d & global_position,
  const Eigen::Vector2f & vector_ne);
 
static inline Eigen::Vector3d addVectorToGlobalPosition(
  const Eigen::Vector3d & global_position,
  const Eigen::Vector3f & vector_ned)
{
  const Eigen::Vector2d global_position_res = addVectorToGlobalPosition(
    static_cast<Eigen::Vector2d>(global_position.head(2)),
    static_cast<Eigen::Vector2f>(vector_ned.head(2)));
  const double d_alt = -vector_ned.z();
  return Eigen::Vector3d {global_position_res.x(), global_position_res.y(),
    global_position.z() + d_alt};
}
 
}  // namespace px4_ros2