geodesic.hpp

/****************************************************************************
 * Copyright (c) 2024 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <cmath>
#include <px4_msgs/msg/vehicle_local_position.hpp>
#include <px4_ros2/common/context.hpp>
#include <px4_ros2/components/shared_subscription.hpp>
#include <rclcpp/rclcpp.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(const px4_msgs::msg::VehicleLocalPosition::UniquePtr& msg);
 
  rclcpp::Node& _node;
  std::unique_ptr<MapProjectionImpl> _map_projection_math;
  SharedSubscriptionCallbackInstance _vehicle_local_position_cb;
};
 
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