transformer.h

 
#ifndef TRANSFORMER_H
#define TRANSFORMER_H
 
/* #define PCL_SPECIALIZATION true */
/* #define OPENCV_SPECIALIZATION true */
 
/* #define EIGEN_DONT_VECTORIZE */
/* #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT */
 
/* includes //{ */
 
#include <rclcpp/rclcpp.hpp>
 
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
#include <mrs_msgs/msg/reference_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/vector3_stamped.hpp>
 
#include <std_msgs/msg/header.hpp>
 
#include <mrs_lib/geometry/misc.h>
 
#ifdef PCL_SPECIALIZATION
#include <pcl_ros/point_cloud.hpp>
#include <pcl_ros/transforms.hpp>
#include <pcl_conversions/pcl_conversions.h>
#endif
 
#include <mutex>
#include <experimental/type_traits>
 
//}
 
#ifdef PCL_SPECIALIZATION
 
namespace tf2
{
 
  template <typename pt_t>
  void doTransform(const pcl::PointCloud<pt_t>& cloud_in, pcl::PointCloud<pt_t>& cloud_out, const geometry_msgs::msg::TransformStamped& transform)
  {
    pcl_ros::transformPointCloud(cloud_in, cloud_out, transform.transform);
    pcl_conversions::toPCL(transform.header.stamp, cloud_out.header.stamp);
    cloud_out.header.frame_id = transform.header.frame_id;
  }
 
} // namespace tf2
 
#endif
 
namespace mrs_lib
{
 
  static const std::string UTM_ORIGIN = "utm_origin";
  static const std::string LATLON_ORIGIN = "latlon_origin";
 
  /* class Transformer //{ */
 
  class Transformer
  {
 
  public:
    /* Constructor, assignment operator and overloads //{ */
 
    Transformer();
 
    Transformer(const rclcpp::Node::SharedPtr& node);
 
    /* Transformer(const rclcpp::Node::SharedPtr& node, const rclcpp::Duration& cache_time = rclcpp::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME)); */
 
    Transformer(const rclcpp::Node::SharedPtr& node, const rclcpp::Clock::SharedPtr& clock,
                const rclcpp::Duration& cache_time = rclcpp::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), const rclcpp::QoS& qos = rclcpp::ServicesQoS());
 
    Transformer& operator=(Transformer&& other);
 
    //}
 
    // | ------------------ Configuration methods ----------------- |
 
    /* setDefaultFrame() //{ */
 
    void setDefaultFrame(const std::string& frame_id)
    {
      std::scoped_lock lck(mutex_);
      default_frame_id_ = frame_id;
    }
 
    //}
 
    /* setDefaultPrefix() //{ */
 
    void setDefaultPrefix(const std::string& prefix)
    {
      std::scoped_lock lck(mutex_);
      if (prefix.empty())
        prefix_ = "";
      else
        prefix_ = prefix + "/";
    }
 
    //}
 
    /* setLatLon() //{ */
 
    void setLatLon(const double lat, const double lon);
 
    //}
 
    /* setLookupTimeout() //{ */
 
    void setLookupTimeout(const rclcpp::Duration timeout = rclcpp::Duration(0, 0))
    {
      std::scoped_lock lck(mutex_);
      lookup_timeout_ = timeout;
    }
 
    //}
 
    /* retryLookupNewest() //{ */
 
    void retryLookupNewest(const bool retry = true)
    {
      std::scoped_lock lck(mutex_);
      retry_lookup_newest_ = retry;
    }
 
    //}
 
    /* beQuiet() //{ */
 
    void beQuiet(const bool quiet = true)
    {
      std::scoped_lock lck(mutex_);
      quiet_ = quiet;
    }
 
    //}
 
    /* resolveFrame() //{ */
    std::string resolveFrame(const std::string& frame_id)
    {
      std::scoped_lock lck(mutex_);
      return resolveFrameImpl(frame_id);
    }
    //}
 
    /* transformSingle() //{ */
 
    template <class T>
    [[nodiscard]] std::optional<T> transformSingle(const T& what, const std::string& to_frame);
 
    template <class T>
    [[nodiscard]] std::optional<std::shared_ptr<T>> transformSingle(const std::shared_ptr<T>& what, const std::string& to_frame)
    {
      return transformSingle(std::shared_ptr<const T>(what), to_frame);
    }
 
    template <class T>
    [[nodiscard]] std::optional<std::shared_ptr<T>> transformSingle(const std::shared_ptr<const T>& what, const std::string& to_frame)
    {
      auto ret = transformSingle(*what, to_frame);
      if (ret == std::nullopt)
        return std::nullopt;
      else
        return std::make_shared<T>(std::move(ret.value()));
    }
 
    template <class T>
    [[nodiscard]] std::optional<T> transformSingle(const std::string& from_frame, const T& what, const std::string& to_frame,
                                                   const rclcpp::Time& time_stamp = rclcpp::Time(0));
 
    template <class T>
    [[nodiscard]] std::optional<std::shared_ptr<T>> transformSingle(const std::string& from_frame, const std::shared_ptr<T>& what, const std::string& to_frame,
                                                                    const rclcpp::Time& time_stamp = rclcpp::Time(0))
    {
      return transformSingle(from_frame, std::shared_ptr<const T>(what), to_frame, time_stamp);
    }
 
    template <class T>
    [[nodiscard]] std::optional<std::shared_ptr<T>> transformSingle(const std::string& from_frame, const std::shared_ptr<const T>& what,
                                                                    const std::string& to_frame, const rclcpp::Time& time_stamp = rclcpp::Time(0))
    {
      auto ret = transformSingle(from_frame, *what, to_frame, time_stamp);
      if (ret == std::nullopt)
        return std::nullopt;
      else
        return std::make_shared<T>(std::move(ret.value()));
    }
 
    //}
 
    /* transform() //{ */
 
    template <class T>
    [[nodiscard]] std::optional<T> transform(const T& what, const geometry_msgs::msg::TransformStamped& tf);
 
    template <class T>
    [[nodiscard]] std::optional<std::shared_ptr<T>> transform(const std::shared_ptr<const T>& what, const geometry_msgs::msg::TransformStamped& tf)
    {
      auto ret = transform(*what, tf);
      if (ret == std::nullopt)
        return std::nullopt;
      else
        return std::make_shared<T>(std::move(ret.value()));
    }
 
    template <class T>
    [[nodiscard]] std::optional<std::shared_ptr<T>> transform(const std::shared_ptr<T>& what, const geometry_msgs::msg::TransformStamped& tf)
    {
      return transform(std::shared_ptr<const T>(what), tf);
    }
 
    //}
 
    /* transformAsVector() method //{ */
    [[nodiscard]] std::optional<Eigen::Vector3d> transformAsVector(const Eigen::Vector3d& what, const geometry_msgs::msg::TransformStamped& tf);
 
    [[nodiscard]] std::optional<Eigen::Vector3d> transformAsVector(const std::string& from_frame, const Eigen::Vector3d& what, const std::string& to_frame,
                                                                   const rclcpp::Time& time_stamp = rclcpp::Time(0));
    //}
 
    /* transformAsPoint() method //{ */
    [[nodiscard]] std::optional<Eigen::Vector3d> transformAsPoint(const Eigen::Vector3d& what, const geometry_msgs::msg::TransformStamped& tf);
 
    [[nodiscard]] std::optional<Eigen::Vector3d> transformAsPoint(const std::string& from_frame, const Eigen::Vector3d& what, const std::string& to_frame,
                                                                  const rclcpp::Time& time_stamp = rclcpp::Time(0));
    //}
 
    /* getTransform() //{ */
 
    [[nodiscard]] std::optional<geometry_msgs::msg::TransformStamped> getTransform(const std::string& from_frame, const std::string& to_frame,
                                                                                   const rclcpp::Time& time_stamp = rclcpp::Time(0));
 
    [[nodiscard]] std::optional<geometry_msgs::msg::TransformStamped> getTransform(const std::string& from_frame, const rclcpp::Time& from_stamp,
                                                                                   const std::string& to_frame, const rclcpp::Time& to_stamp,
                                                                                   const std::string& fixed_frame);
 
    //}
 
  private:
    /* private members, methods etc //{ */
 
    std::mutex mutex_;
 
    // keeps track whether a non-basic constructor was called and the transform listener is initialized
    bool initialized_ = false;
    rclcpp::Node::SharedPtr node_;
 
    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
    std::unique_ptr<tf2_ros::TransformListener> tf_listener_ptr_;
 
    // user-configurable options
    std::string default_frame_id_ = "";
    std::string prefix_ = ""; // if not empty, includes the forward slash
    bool quiet_ = false;
    rclcpp::Duration lookup_timeout_ = rclcpp::Duration(0, 0);
    bool retry_lookup_newest_ = false;
 
    bool got_utm_zone_ = false;
    std::array<char, 10> utm_zone_ = {};
 
    // returns the first namespace prefix of the frame (if any) includin the forward slash
    std::string getFramePrefix(const std::string& frame_id);
 
    template <class T>
    std::optional<T> transformImpl(const geometry_msgs::msg::TransformStamped& tf, const T& what);
    std::optional<mrs_msgs::msg::ReferenceStamped> transformImpl(const geometry_msgs::msg::TransformStamped& tf, const mrs_msgs::msg::ReferenceStamped& what);
    std::optional<Eigen::Vector3d> transformImpl(const geometry_msgs::msg::TransformStamped& tf, const Eigen::Vector3d& what);
 
    [[nodiscard]] std::optional<geometry_msgs::msg::TransformStamped> getTransformImpl(const std::string& from_frame, const std::string& to_frame,
                                                                                       const rclcpp::Time& time_stamp, const std::string& latlon_frame);
 
    [[nodiscard]] std::optional<geometry_msgs::msg::TransformStamped> getTransformImpl(const std::string& from_frame, const rclcpp::Time& from_stamp,
                                                                                       const std::string& to_frame, const rclcpp::Time& to_stamp,
                                                                                       const std::string& fixed_frame, const std::string& latlon_frame);
 
    std::string resolveFrameImpl(const std::string& frame_id);
 
    template <class T>
    std::optional<T> doTransform(const T& what, const geometry_msgs::msg::TransformStamped& tf);
 
    //}
 
    // | ------------------- some helper methods ------------------ |
  public:
    /* frame_from(), frame_to() and inverse() methods //{ */
 
    static constexpr const std::string& frame_from(const geometry_msgs::msg::TransformStamped& msg)
    {
      return msg.child_frame_id;
    }
 
    static constexpr std::string& frame_from(geometry_msgs::msg::TransformStamped& msg)
    {
      return msg.child_frame_id;
    }
 
    static constexpr const std::string& frame_to(const geometry_msgs::msg::TransformStamped& msg)
    {
      return msg.header.frame_id;
    }
 
    static constexpr std::string& frame_to(geometry_msgs::msg::TransformStamped& msg)
    {
      return msg.header.frame_id;
    }
 
    static geometry_msgs::msg::TransformStamped inverse(const geometry_msgs::msg::TransformStamped& msg)
    {
      tf2::Transform tf2;
      tf2::fromMsg(msg.transform, tf2);
      tf2 = tf2.inverse();
      return create_transform(frame_to(msg), frame_from(msg), msg.header.stamp, tf2::toMsg(tf2));
    }
    //}
 
  private:
    /* create_transform() method //{ */
    static geometry_msgs::msg::TransformStamped create_transform(const std::string& from_frame, const std::string& to_frame, const rclcpp::Time& time_stamp)
    {
      geometry_msgs::msg::TransformStamped ret;
      frame_from(ret) = from_frame;
      frame_to(ret) = to_frame;
      ret.header.stamp = time_stamp;
      return ret;
    }
 
    static geometry_msgs::msg::TransformStamped create_transform(const std::string& from_frame, const std::string& to_frame, const rclcpp::Time& time_stamp,
                                                                 const geometry_msgs::msg::Transform& tf)
    {
      geometry_msgs::msg::TransformStamped ret;
      frame_from(ret) = from_frame;
      frame_to(ret) = to_frame;
      ret.header.stamp = time_stamp;
      ret.transform = tf;
      return ret;
    }
    //}
 
    /* copyChangeFrame() and related methods //{ */
 
    // helper type and member for detecting whether a message has a header using SFINAE
    template <typename T>
    using has_header_member_chk = decltype(std::declval<T&>().header);
 
    template <typename T>
    static constexpr bool has_header_member_v = std::experimental::is_detected<has_header_member_chk, T>::value;
 
    template <typename msg_t>
    std_msgs::msg::Header getHeader(const msg_t& msg);
 
#ifdef PCL_SPECIALIZATION
    template <typename pt_t>
    std_msgs::msg::Header getHeader(const pcl::PointCloud<pt_t>& cloud);
 
    template <typename pt_t>
    void setHeader(pcl::PointCloud<pt_t>& cloud, const std_msgs::msg::Header& header);
#endif
 
    template <typename msg_t>
    void setHeader(msg_t& msg, const std_msgs::msg::Header& header);
 
    template <typename T>
    T copyChangeFrame(const T& what, const std::string& frame_id);
 
    //}
 
    /* methods for converting between lattitude/longitude and UTM coordinates //{ */
    geometry_msgs::msg::Point LLtoUTM(const geometry_msgs::msg::Point& what, const std::string& prefix);
    geometry_msgs::msg::PointStamped LLtoUTM(const geometry_msgs::msg::PointStamped& what, const std::string& prefix);
    geometry_msgs::msg::Pose LLtoUTM(const geometry_msgs::msg::Pose& what, const std::string& prefix);
    geometry_msgs::msg::PoseStamped LLtoUTM(const geometry_msgs::msg::PoseStamped& what, const std::string& prefix);
 
    std::optional<geometry_msgs::msg::Point> UTMtoLL(const geometry_msgs::msg::Point& what, const std::string& prefix);
    std::optional<geometry_msgs::msg::PointStamped> UTMtoLL(const geometry_msgs::msg::PointStamped& what, const std::string& prefix);
    std::optional<geometry_msgs::msg::Pose> UTMtoLL(const geometry_msgs::msg::Pose& what, const std::string& prefix);
    std::optional<geometry_msgs::msg::PoseStamped> UTMtoLL(const geometry_msgs::msg::PoseStamped& what, const std::string& prefix);
 
    // helper types and member for detecting whether the UTMtoLL and LLtoUTM methods are defined for a certain message
    template <class Class, typename Message>
    using UTMLL_method_chk = decltype(std::declval<Class>().UTMtoLL(std::declval<const Message&>(), ""));
 
    template <class Class, typename Message>
    using LLUTM_method_chk = decltype(std::declval<Class>().LLtoUTM(std::declval<const Message&>(), ""));
 
    template <class Class, typename Message>
    static constexpr bool UTMLL_exists_v =
        std::experimental::is_detected<UTMLL_method_chk, Class, Message>::value && std::experimental::is_detected<LLUTM_method_chk, Class, Message>::value;
    //}
  };
 
  //}
 
} // namespace mrs_lib
 
#ifndef TRANSFORMER_HPP
#include <mrs_lib/impl/transformer.hpp>
#endif
 
#endif // TRANSFORMER_H