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