transformer.hpp

#ifndef TRANSFORMER_HPP
#define TRANSFORMER_HPP
 
#ifndef TRANSFORMER_H
#include <mrs_lib/transformer.h>
#endif
 
namespace mrs_lib
{
 
  // | --------------------- helper methods --------------------- |
 
  /* getHeader() overloads for different message types (pointers, pointclouds etc) //{ */
 
  template <typename msg_t>
  std_msgs::msg::Header Transformer::getHeader(const msg_t& msg)
  {
    return msg.header;
  }
 
#ifdef PCL_SPECIALIZATION
  template <typename pt_t>
  std_msgs::msg::Header Transformer::getHeader(const pcl::PointCloud<pt_t>& cloud)
  {
    std_msgs::msg::Header ret;
    pcl_conversions::fromPCL(cloud.header, ret);
    return ret;
  }
#endif
 
  //}
 
  /* setHeader() overloads for different message types (pointers, pointclouds etc) //{ */
 
  template <typename msg_t>
  void Transformer::setHeader(msg_t& msg, const std_msgs::msg::Header& header)
  {
    msg.header = header;
  }
 
#ifdef PCL_SPECIALIZATION
  template <typename pt_t>
  void Transformer::setHeader(pcl::PointCloud<pt_t>& cloud, const std_msgs::msg::Header& header)
  {
    pcl_conversions::toPCL(header, cloud.header);
  }
#endif
 
  //}
 
  /* copyChangeFrame() helper function //{ */
 
  template <typename T>
  T Transformer::copyChangeFrame(const T& what, const std::string& frame_id)
  {
    T ret = what;
    if constexpr (has_header_member_v<T>)
    {
      std_msgs::msg::Header new_header = getHeader(what);
      new_header.frame_id = frame_id;
      setHeader(ret, new_header);
    }
    return ret;
  }
 
  //}
 
  /* transformImpl() //{ */
 
  template <class T>
  std::optional<T> Transformer::transformImpl(const geometry_msgs::msg::TransformStamped& tf, const T& what)
  {
 
    const std::string from_frame = frame_from(tf);
    const std::string to_frame = frame_to(tf);
 
    if (from_frame == to_frame)
      return copyChangeFrame(what, from_frame);
 
    const std::string latlon_frame_name = resolveFrameImpl(LATLON_ORIGIN);
 
    // First, check if the transformation is from/to the latlon frame
    // if conversion between UVM and LatLon coordinates is defined for this message, it may be resolved
    if constexpr (UTMLL_exists_v<Transformer, T>)
    {
      // check for transformation from LAT-LON GPS
      if (from_frame == latlon_frame_name)
      {
        const std::optional<T> tmp = LLtoUTM(what, getFramePrefix(from_frame));
        if (!tmp.has_value())
          return std::nullopt;
        return doTransform(tmp.value(), tf);
      }
      // check for transformation to LAT-LON GPS
      else if (to_frame == latlon_frame_name)
      {
        const std::optional<T> tmp = doTransform(what, tf);
        if (!tmp.has_value())
          return std::nullopt;
        return UTMtoLL(tmp.value(), getFramePrefix(to_frame));
      }
    } else
    {
      // by default, transformation from/to LATLON is undefined, so return nullopt if it's attempted
      if (from_frame == latlon_frame_name || to_frame == latlon_frame_name)
      {
        RCLCPP_ERROR_STREAM_THROTTLE(node_->get_logger(), *node_->get_clock(), 1e9,
                                     "Transformer: cannot transform message of this type (" << typeid(T).name() << ") to/from latitude/longitude coordinates!");
        return std::nullopt;
      }
    }
 
    // otherwise it's just an ol' borin' transformation
    return doTransform(what, tf);
  }
 
  //}
 
  /* doTransform() //{ */
 
  template <class T>
  std::optional<T> Transformer::doTransform(const T& what, const geometry_msgs::msg::TransformStamped& tf)
  {
    try
    {
      T result;
      tf2::doTransform(what, result, tf);
      return result;
    }
    catch (tf2::TransformException& ex)
    {
      RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 1e9, "Transformer: Error during transform from \"%s\" frame to \"%s\" frame.\n\tMSG: %s",
                           frame_from(tf).c_str(), frame_to(tf).c_str(), ex.what());
      return std::nullopt;
    }
  }
 
  //}
 
  // | ------------------ user-callable methods ----------------- |
 
  /* transformSingle() //{ */
 
  template <class T>
  std::optional<T> Transformer::transformSingle(const T& what, const std::string& to_frame_raw)
  {
    const std_msgs::msg::Header orig_header = getHeader(what);
    return transformSingle(orig_header.frame_id, what, to_frame_raw, orig_header.stamp);
  }
 
  template <class T>
  std::optional<T> Transformer::transformSingle(const std::string& from_frame_raw, const T& what, const std::string& to_frame_raw,
                                                const rclcpp::Time& time_stamp)
  {
    std::scoped_lock lck(mutex_);
 
    if (!initialized_)
    {
      RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 1e9, "Transformer: cannot transform, not initialized");
      return std::nullopt;
    }
 
    const std::string from_frame = resolveFrameImpl(from_frame_raw);
    const std::string to_frame = resolveFrameImpl(to_frame_raw);
    const std::string latlon_frame = resolveFrameImpl(LATLON_ORIGIN);
 
    // get the transform
    const auto tf_opt = getTransformImpl(from_frame, to_frame, time_stamp, latlon_frame);
    if (!tf_opt.has_value())
      return std::nullopt;
 
    const geometry_msgs::msg::TransformStamped& tf = tf_opt.value();
 
    // do the transformation
    const geometry_msgs::msg::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
 
    return transformImpl(tf_resolved, what);
  }
 
  //}
 
  /* transform() //{ */
 
  template <class T>
  std::optional<T> Transformer::transform(const T& what, const geometry_msgs::msg::TransformStamped& tf)
  {
    std::scoped_lock lck(mutex_);
 
    if (!initialized_)
    {
      RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 1e9, "Transformer: cannot transform, not initialized");
      return std::nullopt;
    }
 
    const std::string from_frame = resolveFrameImpl(frame_from(tf));
    const std::string to_frame = resolveFrameImpl(frame_to(tf));
    const geometry_msgs::msg::TransformStamped tf_resolved = create_transform(from_frame, to_frame, tf.header.stamp, tf.transform);
 
    return transformImpl(tf_resolved, what);
  }
 
  /* //} */
 
} // namespace mrs_lib
 
#endif // TRANSFORMER_HPP