mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
transformer.h
1 // clang: MatousFormat
2 
8 #ifndef TRANSFORMER_H
9 #define TRANSFORMER_H
10 
11 /* #define EIGEN_DONT_VECTORIZE */
12 /* #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT */
13 
14 /* includes //{ */
15 
16 #include <ros/ros.h>
17 
18 #include <tf2_ros/transform_listener.h>
19 #include <tf2_ros/buffer.h>
20 #include <tf2_eigen/tf2_eigen.h>
21 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
22 #include <tf/transform_datatypes.h>
23 #include <tf_conversions/tf_eigen.h>
24 
25 #include <mrs_msgs/ReferenceStamped.h>
26 
27 #include <geometry_msgs/PoseStamped.h>
28 #include <geometry_msgs/Vector3Stamped.h>
29 #include <std_msgs/Header.h>
30 
31 #include <mrs_lib/geometry/misc.h>
32 
33 #include <pcl_ros/point_cloud.h>
34 #include <pcl_ros/transforms.h>
35 #include <pcl_conversions/pcl_conversions.h>
36 
37 #include <mutex>
38 #include <experimental/type_traits>
39 
40 //}
41 
42 namespace tf2
43 {
44 
45  template <typename pt_t>
46  void doTransform(const pcl::PointCloud<pt_t>& cloud_in, pcl::PointCloud<pt_t>& cloud_out, const geometry_msgs::TransformStamped& transform)
47  {
48  pcl_ros::transformPointCloud(cloud_in, cloud_out, transform.transform);
49  pcl_conversions::toPCL(transform.header.stamp, cloud_out.header.stamp);
50  cloud_out.header.frame_id = transform.header.frame_id;
51  }
52 
53 } // namespace tf2
54 
55 namespace mrs_lib
56 {
57 
58  static const std::string UTM_ORIGIN = "utm_origin";
59  static const std::string LATLON_ORIGIN = "latlon_origin";
60 
66  /* class Transformer //{ */
67 
68  class Transformer
69  {
70 
71  public:
72  /* Constructor, assignment operator and overloads //{ */
73 
82  Transformer();
83 
92  Transformer(const std::string& node_name, const ros::Duration& cache_time = ros::Duration(tf2_ros::Buffer::DEFAULT_CACHE_TIME));
93 
103  Transformer(const ros::NodeHandle& nh, const std::string& node_name = std::string(), const ros::Duration& cache_time = ros::Duration(tf2_ros::Buffer::DEFAULT_CACHE_TIME));
104 
113  Transformer& operator=(Transformer&& other);
114 
115  //}
116 
117  // | ------------------ Configuration methods ----------------- |
118 
119  /* setDefaultFrame() //{ */
120 
130  void setDefaultFrame(const std::string& frame_id)
131  {
132  std::scoped_lock lck(mutex_);
133  default_frame_id_ = frame_id;
134  }
135 
136  //}
137 
138  /* setDefaultPrefix() //{ */
139 
153  void setDefaultPrefix(const std::string& prefix)
154  {
155  std::scoped_lock lck(mutex_);
156  if (prefix.empty())
157  prefix_ = "";
158  else
159  prefix_ = prefix + "/";
160  }
161 
162  //}
163 
164  /* setLatLon() //{ */
165 
176  void setLatLon(const double lat, const double lon);
177 
178  //}
179 
180  /* setLookupTimeout() //{ */
181 
191  void setLookupTimeout(const ros::Duration timeout = ros::Duration(0))
192  {
193  std::scoped_lock lck(mutex_);
194  lookup_timeout_ = timeout;
195  }
196 
197  //}
198 
199  /* retryLookupNewest() //{ */
200 
211  void retryLookupNewest(const bool retry = true)
212  {
213  std::scoped_lock lck(mutex_);
214  retry_lookup_newest_ = retry;
215  }
216 
217  //}
218 
219  /* beQuiet() //{ */
220 
228  void beQuiet(const bool quiet = true)
229  {
230  std::scoped_lock lck(mutex_);
231  quiet_ = quiet;
232  }
233 
234  //}
235 
236  /* resolveFrame() //{ */
248  std::string resolveFrame(const std::string& frame_id)
249  {
250  std::scoped_lock lck(mutex_);
251  return resolveFrameImpl(frame_id);
252  }
253  //}
254 
255  /* transformSingle() //{ */
256 
265  template <class T>
266  [[nodiscard]] std::optional<T> transformSingle(const T& what, const std::string& to_frame);
267 
278  template <class T>
279  [[nodiscard]] std::optional<boost::shared_ptr<T>> transformSingle(const boost::shared_ptr<T>& what, const std::string& to_frame)
280  {
281  return transformSingle(boost::shared_ptr<const T>(what), to_frame);
282  }
283 
294  template <class T>
295  [[nodiscard]] std::optional<boost::shared_ptr<T>> transformSingle(const boost::shared_ptr<const T>& what, const std::string& to_frame)
296  {
297  auto ret = transformSingle(*what, to_frame);
298  if (ret == std::nullopt)
299  return std::nullopt;
300  else
301  return boost::make_shared<T>(std::move(ret.value()));
302  }
303 
316  template <class T>
317  [[nodiscard]] std::optional<T> transformSingle(const std::string& from_frame, const T& what, const std::string& to_frame, const ros::Time& time_stamp = ros::Time(0));
318 
331  template <class T>
332  [[nodiscard]] std::optional<boost::shared_ptr<T>> transformSingle(const std::string& from_frame, const boost::shared_ptr<T>& what, const std::string& to_frame, const ros::Time& time_stamp = ros::Time(0))
333  {
334  return transformSingle(from_frame, boost::shared_ptr<const T>(what), to_frame, time_stamp);
335  }
336 
349  template <class T>
350  [[nodiscard]] std::optional<boost::shared_ptr<T>> transformSingle(const std::string& from_frame, const boost::shared_ptr<const T>& what, const std::string& to_frame, const ros::Time& time_stamp = ros::Time(0))
351  {
352  auto ret = transformSingle(from_frame, *what, to_frame, time_stamp);
353  if (ret == std::nullopt)
354  return std::nullopt;
355  else
356  return boost::make_shared<T>(std::move(ret.value()));
357  }
358 
359  //}
360 
361  /* transform() //{ */
362 
371  template <class T>
372  [[nodiscard]] std::optional<T> transform(const T& what, const geometry_msgs::TransformStamped& tf);
373 
384  template <class T>
385  [[nodiscard]] std::optional<boost::shared_ptr<T>> transform(const boost::shared_ptr<const T>& what, const geometry_msgs::TransformStamped& tf)
386  {
387  auto ret = transform(*what, tf);
388  if (ret == std::nullopt)
389  return std::nullopt;
390  else
391  return boost::make_shared<T>(std::move(ret.value()));
392  }
393 
404  template <class T>
405  [[nodiscard]] std::optional<boost::shared_ptr<T>> transform(const boost::shared_ptr<T>& what, const geometry_msgs::TransformStamped& tf)
406  {
407  return transform(boost::shared_ptr<const T>(what), tf);
408  }
409 
410  //}
411 
412  /* transformAsVector() method //{ */
423  [[nodiscard]] std::optional<Eigen::Vector3d> transformAsVector(const Eigen::Vector3d& what, const geometry_msgs::TransformStamped& tf);
424 
437  [[nodiscard]] std::optional<Eigen::Vector3d> transformAsVector(const std::string& from_frame, const Eigen::Vector3d& what, const std::string& to_frame, const ros::Time& time_stamp = ros::Time(0));
438  //}
439 
440  /* transformAsPoint() method //{ */
451  [[nodiscard]] std::optional<Eigen::Vector3d> transformAsPoint(const Eigen::Vector3d& what, const geometry_msgs::TransformStamped& tf);
452 
465  [[nodiscard]] std::optional<Eigen::Vector3d> transformAsPoint(const std::string& from_frame, const Eigen::Vector3d& what, const std::string& to_frame, const ros::Time& time_stamp = ros::Time(0));
466  //}
467 
468  /* getTransform() //{ */
469 
479  [[nodiscard]] std::optional<geometry_msgs::TransformStamped> getTransform(const std::string& from_frame, const std::string& to_frame, const ros::Time& time_stamp = ros::Time(0));
480 
496  [[nodiscard]] std::optional<geometry_msgs::TransformStamped> getTransform(const std::string& from_frame, const ros::Time& from_stamp, const std::string& to_frame, const ros::Time& to_stamp, const std::string& fixed_frame);
497 
498  //}
499 
500  private:
501  /* private members, methods etc //{ */
502 
503  std::mutex mutex_;
504 
505  // keeps track whether a non-basic constructor was called and the transform listener is initialized
506  bool initialized_ = false;
507  std::string node_name_;
508 
509  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
510  std::unique_ptr<tf2_ros::TransformListener> tf_listener_ptr_;
511 
512  // user-configurable options
513  std::string default_frame_id_ = "";
514  std::string prefix_ = ""; // if not empty, includes the forward slash
515  bool quiet_ = false;
516  ros::Duration lookup_timeout_ = ros::Duration(0);
517  bool retry_lookup_newest_ = false;
518 
519  bool got_utm_zone_ = false;
520  std::array<char, 10> utm_zone_ = {};
521 
522  // returns the first namespace prefix of the frame (if any) includin the forward slash
523  std::string getFramePrefix(const std::string& frame_id);
524 
525  template <class T>
526  std::optional<T> transformImpl(const geometry_msgs::TransformStamped& tf, const T& what);
527  std::optional<mrs_msgs::ReferenceStamped> transformImpl(const geometry_msgs::TransformStamped& tf, const mrs_msgs::ReferenceStamped& what);
528  std::optional<Eigen::Vector3d> transformImpl(const geometry_msgs::TransformStamped& tf, const Eigen::Vector3d& what);
529 
530  [[nodiscard]] std::optional<geometry_msgs::TransformStamped> getTransformImpl(const std::string& from_frame, const std::string& to_frame, const ros::Time& time_stamp, const std::string& latlon_frame);
531  [[nodiscard]] std::optional<geometry_msgs::TransformStamped> getTransformImpl(const std::string& from_frame, const ros::Time& from_stamp, const std::string& to_frame, const ros::Time& to_stamp, const std::string& fixed_frame, const std::string& latlon_frame);
532 
533  std::string resolveFrameImpl(const std::string& frame_id);
534 
535  template <class T>
536  std::optional<T> doTransform(const T& what, const geometry_msgs::TransformStamped& tf);
537 
538  //}
539 
540  // | ------------------- some helper methods ------------------ |
541  public:
542  /* frame_from(), frame_to() and inverse() methods //{ */
543 
550  static constexpr const std::string& frame_from(const geometry_msgs::TransformStamped& msg)
551  {
552  return msg.child_frame_id;
553  }
554 
563  static constexpr std::string& frame_from(geometry_msgs::TransformStamped& msg)
564  {
565  return msg.child_frame_id;
566  }
567 
574  static constexpr const std::string& frame_to(const geometry_msgs::TransformStamped& msg)
575  {
576  return msg.header.frame_id;
577  }
578 
587  static constexpr std::string& frame_to(geometry_msgs::TransformStamped& msg)
588  {
589  return msg.header.frame_id;
590  }
591 
598  static geometry_msgs::TransformStamped inverse(const geometry_msgs::TransformStamped& msg)
599  {
600  tf2::Transform tf2;
601  tf2::fromMsg(msg.transform, tf2);
602  tf2 = tf2.inverse();
603  return create_transform(frame_to(msg), frame_from(msg), msg.header.stamp, tf2::toMsg(tf2));
604  }
605  //}
606 
607  private:
608  /* create_transform() method //{ */
609  static geometry_msgs::TransformStamped create_transform(const std::string& from_frame, const std::string& to_frame, const ros::Time& time_stamp)
610  {
611  geometry_msgs::TransformStamped ret;
612  frame_from(ret) = from_frame;
613  frame_to(ret) = to_frame;
614  ret.header.stamp = time_stamp;
615  return ret;
616  }
617 
618  static geometry_msgs::TransformStamped create_transform(const std::string& from_frame, const std::string& to_frame, const ros::Time& time_stamp, const geometry_msgs::Transform& tf)
619  {
620  geometry_msgs::TransformStamped ret;
621  frame_from(ret) = from_frame;
622  frame_to(ret) = to_frame;
623  ret.header.stamp = time_stamp;
624  ret.transform = tf;
625  return ret;
626  }
627  //}
628 
629  /* copyChangeFrame() and related methods //{ */
630 
631  // helper type and member for detecting whether a message has a header using SFINAE
632  template<typename T>
633  using has_header_member_chk = decltype( std::declval<T&>().header );
634  template<typename T>
635  static constexpr bool has_header_member_v = std::experimental::is_detected<has_header_member_chk, T>::value;
636 
637  template <typename msg_t>
638  std_msgs::Header getHeader(const msg_t& msg);
639  template <typename pt_t>
640  std_msgs::Header getHeader(const pcl::PointCloud<pt_t>& cloud);
641 
642  template <typename msg_t>
643  void setHeader(msg_t& msg, const std_msgs::Header& header);
644  template <typename pt_t>
645  void setHeader(pcl::PointCloud<pt_t>& cloud, const std_msgs::Header& header);
646 
647  template <typename T>
648  T copyChangeFrame(const T& what, const std::string& frame_id);
649 
650  //}
651 
652  /* methods for converting between lattitude/longitude and UTM coordinates //{ */
653  geometry_msgs::Point LLtoUTM(const geometry_msgs::Point& what, const std::string& prefix);
654  geometry_msgs::PointStamped LLtoUTM(const geometry_msgs::PointStamped& what, const std::string& prefix);
655  geometry_msgs::Pose LLtoUTM(const geometry_msgs::Pose& what, const std::string& prefix);
656  geometry_msgs::PoseStamped LLtoUTM(const geometry_msgs::PoseStamped& what, const std::string& prefix);
657 
658  std::optional<geometry_msgs::Point> UTMtoLL(const geometry_msgs::Point& what, const std::string& prefix);
659  std::optional<geometry_msgs::PointStamped> UTMtoLL(const geometry_msgs::PointStamped& what, const std::string& prefix);
660  std::optional<geometry_msgs::Pose> UTMtoLL(const geometry_msgs::Pose& what, const std::string& prefix);
661  std::optional<geometry_msgs::PoseStamped> UTMtoLL(const geometry_msgs::PoseStamped& what, const std::string& prefix);
662 
663  // helper types and member for detecting whether the UTMtoLL and LLtoUTM methods are defined for a certain message
664  template<class Class, typename Message>
665  using UTMLL_method_chk = decltype(std::declval<Class>().UTMtoLL(std::declval<const Message&>(), ""));
666  template<class Class, typename Message>
667  using LLUTM_method_chk = decltype(std::declval<Class>().LLtoUTM(std::declval<const Message&>(), ""));
668  template<class Class, typename Message>
669  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;
670  //}
671 
672  };
673 
674  //}
675 
676 } // namespace mrs_lib
677 
678 #include <mrs_lib/impl/transformer.hpp>
679 
680 #endif // TRANSFORMER_H
mrs_lib::Transformer::transform
std::optional< boost::shared_ptr< T > > transform(const boost::shared_ptr< T > &what, const geometry_msgs::TransformStamped &tf)
Transform a variable to new frame using a particular transformation.
Definition: transformer.h:405
mrs_lib::Transformer::resolveFrame
std::string resolveFrame(const std::string &frame_id)
Deduce the full frame ID from a shortened or empty string using current default prefix and default fr...
Definition: transformer.h:248
mrs_lib::Transformer::transformSingle
std::optional< T > transformSingle(const T &what, const std::string &to_frame)
Transforms a single variable to a new frame and returns it or std::nullopt if transformation fails.
Definition: transformer.hpp:138
mrs_lib::Transformer::frame_from
static constexpr std::string & frame_from(geometry_msgs::TransformStamped &msg)
A convenience function that returns the frame from which the message transforms.
Definition: transformer.h:563
mrs_lib::Transformer::inverse
static geometry_msgs::TransformStamped inverse(const geometry_msgs::TransformStamped &msg)
A convenience function implements returns the inverse of the transform message as a one-liner.
Definition: transformer.h:598
mrs_lib::Transformer::Transformer
Transformer()
A convenience constructor that doesn't initialize anything.
Definition: transformer.cpp:39
mrs_lib::Transformer::transform
std::optional< boost::shared_ptr< T > > transform(const boost::shared_ptr< const T > &what, const geometry_msgs::TransformStamped &tf)
Transform a variable to new frame using a particular transformation.
Definition: transformer.h:385
misc.h
Defines useful geometry utilities and functions.
mrs_lib::Transformer::transformSingle
std::optional< boost::shared_ptr< T > > transformSingle(const std::string &from_frame, const boost::shared_ptr< T > &what, const std::string &to_frame, const ros::Time &time_stamp=ros::Time(0))
Transforms a single variable to a new frame and returns it or std::nullopt if transformation fails.
Definition: transformer.h:332
mrs_lib::Transformer::retryLookupNewest
void retryLookupNewest(const bool retry=true)
Enable/disable retry of a failed transform lookup with ros::Time(0).
Definition: transformer.h:211
mrs_lib::Transformer
A convenience wrapper class for ROS's native TF2 API to simplify transforming of various messages.
Definition: transformer.h:68
mrs_lib::Transformer::setLatLon
void setLatLon(const double lat, const double lon)
Sets the curret lattitude and longitude for UTM zone calculation.
Definition: transformer.cpp:117
mrs_lib
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition: attitude_converter.h:29
mrs_lib::Transformer::setDefaultFrame
void setDefaultFrame(const std::string &frame_id)
Sets the default frame ID to be used instead of any empty frame ID.
Definition: transformer.h:130
mrs_lib::Transformer::transform
std::optional< T > transform(const T &what, const geometry_msgs::TransformStamped &tf)
Transform a variable to new frame using a particular transformation.
Definition: transformer.hpp:175
mrs_lib::Transformer::frame_to
static constexpr std::string & frame_to(geometry_msgs::TransformStamped &msg)
A convenience function that returns the frame to which the message transforms.
Definition: transformer.h:587
mrs_lib::Transformer::frame_to
static constexpr const std::string & frame_to(const geometry_msgs::TransformStamped &msg)
A convenience function that returns the frame to which the message transforms.
Definition: transformer.h:574
mrs_lib::Transformer::frame_from
static constexpr const std::string & frame_from(const geometry_msgs::TransformStamped &msg)
A convenience function that returns the frame from which the message transforms.
Definition: transformer.h:550
mrs_lib::Transformer::transformAsPoint
std::optional< Eigen::Vector3d > transformAsPoint(const Eigen::Vector3d &what, const geometry_msgs::TransformStamped &tf)
Transform an Eigen::Vector3d (interpreting it as a point).
Definition: transformer.cpp:186
mrs_lib::Transformer::beQuiet
void beQuiet(const bool quiet=true)
Enable/disable some prints that may be too noisy.
Definition: transformer.h:228
mrs_lib::Transformer::setLookupTimeout
void setLookupTimeout(const ros::Duration timeout=ros::Duration(0))
Set a timeout for transform lookup.
Definition: transformer.h:191
mrs_lib::Transformer::transformSingle
std::optional< boost::shared_ptr< T > > transformSingle(const boost::shared_ptr< const T > &what, const std::string &to_frame)
Transforms a single variable to a new frame.
Definition: transformer.h:295
mrs_lib::Transformer::transformSingle
std::optional< boost::shared_ptr< T > > transformSingle(const boost::shared_ptr< T > &what, const std::string &to_frame)
Transforms a single variable to a new frame.
Definition: transformer.h:279
mrs_lib::Transformer::transformAsVector
std::optional< Eigen::Vector3d > transformAsVector(const Eigen::Vector3d &what, const geometry_msgs::TransformStamped &tf)
Transform an Eigen::Vector3d (interpreting it as a vector).
Definition: transformer.cpp:130
mrs_lib::Transformer::operator=
Transformer & operator=(Transformer &&other)
A convenience move assignment operator.
Definition: transformer.cpp:53
mrs_lib::Transformer::getTransform
std::optional< geometry_msgs::TransformStamped > getTransform(const std::string &from_frame, const std::string &to_frame, const ros::Time &time_stamp=ros::Time(0))
Obtains a transform between two frames in a given time.
Definition: transformer.cpp:78
mrs_lib::Transformer::setDefaultPrefix
void setDefaultPrefix(const std::string &prefix)
Sets the default frame ID prefix to be used if no prefix is present in the frame.
Definition: transformer.h:153
mrs_lib::Transformer::transformSingle
std::optional< boost::shared_ptr< T > > transformSingle(const std::string &from_frame, const boost::shared_ptr< const T > &what, const std::string &to_frame, const ros::Time &time_stamp=ros::Time(0))
Transforms a single variable to a new frame and returns it or std::nullopt if transformation fails.
Definition: transformer.h:350