mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Loading...
Searching...
No Matches
transformer.h
1
6#ifndef TRANSFORMER_H
7#define TRANSFORMER_H
8
9/* #define PCL_SPECIALIZATION true */
10/* #define OPENCV_SPECIALIZATION true */
11
12/* #define EIGEN_DONT_VECTORIZE */
13/* #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT */
14
15/* includes //{ */
16
17#include <rclcpp/rclcpp.hpp>
18
19#include <tf2_ros/transform_listener.hpp>
20#include <tf2_ros/buffer.hpp>
21#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
22
23#include <mrs_msgs/msg/reference_stamped.hpp>
24#include <geometry_msgs/msg/pose_stamped.hpp>
25#include <geometry_msgs/msg/vector3_stamped.hpp>
26
27#include <std_msgs/msg/header.hpp>
28
29#include <mrs_lib/geometry/misc.h>
30
31#ifdef PCL_SPECIALIZATION
32#include <pcl_ros/point_cloud.hpp>
33#include <pcl_ros/transforms.hpp>
34#include <pcl_conversions/pcl_conversions.h>
35#endif
36
37#include <mutex>
38#include <experimental/type_traits>
39
40//}
41
42#ifdef PCL_SPECIALIZATION
43
44namespace tf2
45{
46
47 template <typename pt_t>
48 void doTransform(const pcl::PointCloud<pt_t>& cloud_in, pcl::PointCloud<pt_t>& cloud_out, const geometry_msgs::msg::TransformStamped& transform)
49 {
50 pcl_ros::transformPointCloud(cloud_in, cloud_out, transform.transform);
51 pcl_conversions::toPCL(transform.header.stamp, cloud_out.header.stamp);
52 cloud_out.header.frame_id = transform.header.frame_id;
53 }
54
55} // namespace tf2
56
57#endif
58
59namespace mrs_lib
60{
61
62 static const std::string UTM_ORIGIN = "utm_origin";
63 static const std::string LATLON_ORIGIN = "latlon_origin";
64
71 /* class Transformer //{ */
72
73 class Transformer
74 {
75
76 public:
77 /* Constructor, assignment operator and overloads //{ */
78
88 Transformer();
89
104 Transformer(const rclcpp::Node::SharedPtr& node, const bool spin_thread = true, const rclcpp::CallbackGroup::SharedPtr& callback_group = nullptr);
105
114 /* Transformer(const rclcpp::Node::SharedPtr& node, const rclcpp::Duration& cache_time = rclcpp::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME)); */
115
131 Transformer(const rclcpp::Node::SharedPtr& node, const rclcpp::Clock::SharedPtr& clock,
132 const rclcpp::Duration& cache_time = rclcpp::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), const rclcpp::QoS& qos = rclcpp::ServicesQoS(),
133 const bool spin_thread = true, const rclcpp::CallbackGroup::SharedPtr& callback_group = nullptr);
134
143 Transformer& operator=(Transformer&& other);
144
145 //}
146
147 // | ------------------ Configuration methods ----------------- |
148
149 /* setDefaultFrame() //{ */
150
160 void setDefaultFrame(const std::string& frame_id)
161 {
162 std::scoped_lock lck(mutex_);
163 default_frame_id_ = frame_id;
164 }
165
166 //}
167
168 /* setDefaultPrefix() //{ */
169
183 void setDefaultPrefix(const std::string& prefix)
184 {
185 std::scoped_lock lck(mutex_);
186 if (prefix.empty())
187 prefix_ = "";
188 else
189 prefix_ = prefix + "/";
190 }
191
192 //}
193
194 /* setLatLon() //{ */
195
206 void setLatLon(const double lat, const double lon);
207
208 //}
209
210 /* setLookupTimeout() //{ */
211
221 void setLookupTimeout(const rclcpp::Duration timeout = rclcpp::Duration(0, 0))
222 {
223 std::scoped_lock lck(mutex_);
224 lookup_timeout_ = timeout;
225 }
226
227 //}
228
229 /* retryLookupNewest() //{ */
230
241 void retryLookupNewest(const bool retry = true)
242 {
243 std::scoped_lock lck(mutex_);
244 retry_lookup_newest_ = retry;
245 }
246
247 //}
248
249 /* beQuiet() //{ */
250
258 void beQuiet(const bool quiet = true)
259 {
260 std::scoped_lock lck(mutex_);
261 quiet_ = quiet;
262 }
263
264 //}
265
266 /* resolveFrame() //{ */
278 std::string resolveFrame(const std::string& frame_id)
279 {
280 std::scoped_lock lck(mutex_);
281 return resolveFrameImpl(frame_id);
282 }
283 //}
284
285 /* transformSingle() //{ */
286
295 template <class T>
296 [[nodiscard]] std::optional<T> transformSingle(const T& what, const std::string& to_frame);
297
308 template <class T>
309 [[nodiscard]] std::optional<std::shared_ptr<T>> transformSingle(const std::shared_ptr<T>& what, const std::string& to_frame)
310 {
311 return transformSingle(std::shared_ptr<const T>(what), to_frame);
312 }
313
324 template <class T>
325 [[nodiscard]] std::optional<std::shared_ptr<T>> transformSingle(const std::shared_ptr<const T>& what, const std::string& to_frame)
326 {
327 auto ret = transformSingle(*what, to_frame);
328 if (ret == std::nullopt)
329 return std::nullopt;
330 else
331 return std::make_shared<T>(std::move(ret.value()));
332 }
333
346 template <class T>
347 [[nodiscard]] std::optional<T> transformSingle(const std::string& from_frame, const T& what, const std::string& to_frame,
348 const rclcpp::Time& time_stamp = rclcpp::Time(0));
349
362 template <class T>
363 [[nodiscard]] std::optional<std::shared_ptr<T>> transformSingle(const std::string& from_frame, const std::shared_ptr<T>& what, const std::string& to_frame,
364 const rclcpp::Time& time_stamp = rclcpp::Time(0))
365 {
366 return transformSingle(from_frame, std::shared_ptr<const T>(what), to_frame, time_stamp);
367 }
368
381 template <class T>
382 [[nodiscard]] std::optional<std::shared_ptr<T>> transformSingle(const std::string& from_frame, const std::shared_ptr<const T>& what,
383 const std::string& to_frame, const rclcpp::Time& time_stamp = rclcpp::Time(0))
384 {
385 auto ret = transformSingle(from_frame, *what, to_frame, time_stamp);
386 if (ret == std::nullopt)
387 return std::nullopt;
388 else
389 return std::make_shared<T>(std::move(ret.value()));
390 }
391
392 //}
393
394 /* transform() //{ */
395
404 template <class T>
405 [[nodiscard]] std::optional<T> transform(const T& what, const geometry_msgs::msg::TransformStamped& tf);
406
417 template <class T>
418 [[nodiscard]] std::optional<std::shared_ptr<T>> transform(const std::shared_ptr<const T>& what, const geometry_msgs::msg::TransformStamped& tf)
419 {
420 auto ret = transform(*what, tf);
421 if (ret == std::nullopt)
422 return std::nullopt;
423 else
424 return std::make_shared<T>(std::move(ret.value()));
425 }
426
437 template <class T>
438 [[nodiscard]] std::optional<std::shared_ptr<T>> transform(const std::shared_ptr<T>& what, const geometry_msgs::msg::TransformStamped& tf)
439 {
440 return transform(std::shared_ptr<const T>(what), tf);
441 }
442
443 //}
444
445 /* transformAsVector() method //{ */
456 [[nodiscard]] std::optional<Eigen::Vector3d> transformAsVector(const Eigen::Vector3d& what, const geometry_msgs::msg::TransformStamped& tf);
457
470 [[nodiscard]] std::optional<Eigen::Vector3d> transformAsVector(const std::string& from_frame, const Eigen::Vector3d& what, const std::string& to_frame,
471 const rclcpp::Time& time_stamp = rclcpp::Time(0));
472 //}
473
474 /* transformAsPoint() method //{ */
485 [[nodiscard]] std::optional<Eigen::Vector3d> transformAsPoint(const Eigen::Vector3d& what, const geometry_msgs::msg::TransformStamped& tf);
486
499 [[nodiscard]] std::optional<Eigen::Vector3d> transformAsPoint(const std::string& from_frame, const Eigen::Vector3d& what, const std::string& to_frame,
500 const rclcpp::Time& time_stamp = rclcpp::Time(0));
501 //}
502
503 /* getTransform() //{ */
504
514 [[nodiscard]] std::optional<geometry_msgs::msg::TransformStamped> getTransform(const std::string& from_frame, const std::string& to_frame,
515 const rclcpp::Time& time_stamp = rclcpp::Time(0));
516
532 [[nodiscard]] std::optional<geometry_msgs::msg::TransformStamped> getTransform(const std::string& from_frame, const rclcpp::Time& from_stamp,
533 const std::string& to_frame, const rclcpp::Time& to_stamp,
534 const std::string& fixed_frame);
535
536 //}
537
538 private:
539 /* private members, methods etc //{ */
540
541 std::mutex mutex_;
542
543 // keeps track whether a non-basic constructor was called and the transform listener is initialized
544 bool initialized_ = false;
545 rclcpp::Node::SharedPtr node_;
546
547 std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
548 std::unique_ptr<tf2_ros::TransformListener> tf_listener_ptr_;
549
550 // user-configurable options
551 std::string default_frame_id_ = "";
552 std::string prefix_ = ""; // if not empty, includes the forward slash
553 bool quiet_ = false;
554 rclcpp::Duration lookup_timeout_ = rclcpp::Duration(0, 0);
555 bool retry_lookup_newest_ = false;
556
557 bool got_utm_zone_ = false;
558 std::array<char, 10> utm_zone_ = {};
559
560 // returns the first namespace prefix of the frame (if any) includin the forward slash
561 std::string getFramePrefix(const std::string& frame_id);
562
563 template <class T>
564 std::optional<T> transformImpl(const geometry_msgs::msg::TransformStamped& tf, const T& what);
565 std::optional<mrs_msgs::msg::ReferenceStamped> transformImpl(const geometry_msgs::msg::TransformStamped& tf, const mrs_msgs::msg::ReferenceStamped& what);
566 std::optional<Eigen::Vector3d> transformImpl(const geometry_msgs::msg::TransformStamped& tf, const Eigen::Vector3d& what);
567
568 [[nodiscard]] std::optional<geometry_msgs::msg::TransformStamped> getTransformImpl(const std::string& from_frame, const std::string& to_frame,
569 const rclcpp::Time& time_stamp, const std::string& latlon_frame);
570
571 [[nodiscard]] std::optional<geometry_msgs::msg::TransformStamped> getTransformImpl(const std::string& from_frame, const rclcpp::Time& from_stamp,
572 const std::string& to_frame, const rclcpp::Time& to_stamp,
573 const std::string& fixed_frame, const std::string& latlon_frame);
574
575 std::string resolveFrameImpl(const std::string& frame_id);
576
577 template <class T>
578 std::optional<T> doTransform(const T& what, const geometry_msgs::msg::TransformStamped& tf);
579
580 //}
581
582 // | ------------------- some helper methods ------------------ |
583 public:
584 /* frame_from(), frame_to() and inverse() methods //{ */
585
592 static constexpr const std::string& frame_from(const geometry_msgs::msg::TransformStamped& msg)
593 {
594 return msg.child_frame_id;
595 }
596
605 static constexpr std::string& frame_from(geometry_msgs::msg::TransformStamped& msg)
606 {
607 return msg.child_frame_id;
608 }
609
616 static constexpr const std::string& frame_to(const geometry_msgs::msg::TransformStamped& msg)
617 {
618 return msg.header.frame_id;
619 }
620
629 static constexpr std::string& frame_to(geometry_msgs::msg::TransformStamped& msg)
630 {
631 return msg.header.frame_id;
632 }
633
640 static geometry_msgs::msg::TransformStamped inverse(const geometry_msgs::msg::TransformStamped& msg)
641 {
642 tf2::Transform tf2;
643 tf2::fromMsg(msg.transform, tf2);
644 tf2 = tf2.inverse();
645 return create_transform(frame_to(msg), frame_from(msg), msg.header.stamp, tf2::toMsg(tf2));
646 }
647 //}
648
649 private:
650 /* create_transform() method //{ */
651 static geometry_msgs::msg::TransformStamped create_transform(const std::string& from_frame, const std::string& to_frame, const rclcpp::Time& time_stamp)
652 {
653 geometry_msgs::msg::TransformStamped ret;
654 frame_from(ret) = from_frame;
655 frame_to(ret) = to_frame;
656 ret.header.stamp = time_stamp;
657 return ret;
658 }
659
660 static geometry_msgs::msg::TransformStamped create_transform(const std::string& from_frame, const std::string& to_frame, const rclcpp::Time& time_stamp,
661 const geometry_msgs::msg::Transform& tf)
662 {
663 geometry_msgs::msg::TransformStamped ret;
664 frame_from(ret) = from_frame;
665 frame_to(ret) = to_frame;
666 ret.header.stamp = time_stamp;
667 ret.transform = tf;
668 return ret;
669 }
670 //}
671
672 /* copyChangeFrame() and related methods //{ */
673
674 // helper type and member for detecting whether a message has a header using SFINAE
675 template <typename T>
676 using has_header_member_chk = decltype(std::declval<T&>().header);
677
678 template <typename T>
679 static constexpr bool has_header_member_v = std::experimental::is_detected<has_header_member_chk, T>::value;
680
681 template <typename msg_t>
682 std_msgs::msg::Header getHeader(const msg_t& msg);
683
684#ifdef PCL_SPECIALIZATION
685 template <typename pt_t>
686 std_msgs::msg::Header getHeader(const pcl::PointCloud<pt_t>& cloud);
687
688 template <typename pt_t>
689 void setHeader(pcl::PointCloud<pt_t>& cloud, const std_msgs::msg::Header& header);
690#endif
691
692 template <typename msg_t>
693 void setHeader(msg_t& msg, const std_msgs::msg::Header& header);
694
695 template <typename T>
696 T copyChangeFrame(const T& what, const std::string& frame_id);
697
698 //}
699
700 /* methods for converting between lattitude/longitude and UTM coordinates //{ */
701 geometry_msgs::msg::Point LLtoUTM(const geometry_msgs::msg::Point& what, const std::string& prefix);
702 geometry_msgs::msg::PointStamped LLtoUTM(const geometry_msgs::msg::PointStamped& what, const std::string& prefix);
703 geometry_msgs::msg::Pose LLtoUTM(const geometry_msgs::msg::Pose& what, const std::string& prefix);
704 geometry_msgs::msg::PoseStamped LLtoUTM(const geometry_msgs::msg::PoseStamped& what, const std::string& prefix);
705
706 std::optional<geometry_msgs::msg::Point> UTMtoLL(const geometry_msgs::msg::Point& what, const std::string& prefix);
707 std::optional<geometry_msgs::msg::PointStamped> UTMtoLL(const geometry_msgs::msg::PointStamped& what, const std::string& prefix);
708 std::optional<geometry_msgs::msg::Pose> UTMtoLL(const geometry_msgs::msg::Pose& what, const std::string& prefix);
709 std::optional<geometry_msgs::msg::PoseStamped> UTMtoLL(const geometry_msgs::msg::PoseStamped& what, const std::string& prefix);
710
711 // helper types and member for detecting whether the UTMtoLL and LLtoUTM methods are defined for a certain message
712 template <class Class, typename Message>
713 using UTMLL_method_chk = decltype(std::declval<Class>().UTMtoLL(std::declval<const Message&>(), ""));
714
715 template <class Class, typename Message>
716 using LLUTM_method_chk = decltype(std::declval<Class>().LLtoUTM(std::declval<const Message&>(), ""));
717
718 template <class Class, typename Message>
719 static constexpr bool UTMLL_exists_v =
720 std::experimental::is_detected<UTMLL_method_chk, Class, Message>::value && std::experimental::is_detected<LLUTM_method_chk, Class, Message>::value;
721 //}
722 };
723
724 //}
725
726} // namespace mrs_lib
727
728#ifndef TRANSFORMER_HPP
729#include <mrs_lib/impl/transformer.hpp>
730#endif
731
732#endif // TRANSFORMER_H
mrs_lib::Transformer::transformSingle
std::optional< std::shared_ptr< T > > transformSingle(const std::shared_ptr< const T > &what, const std::string &to_frame)
Transforms a single variable to a new frame.
Definition transformer.h:325
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:160
mrs_lib::Transformer::transform
std::optional< T > transform(const T &what, const geometry_msgs::msg::TransformStamped &tf)
Transform a variable to new frame using a particular transformation.
Definition transformer.hpp:185
mrs_lib::Transformer::frame_to
static constexpr const std::string & frame_to(const geometry_msgs::msg::TransformStamped &msg)
A convenience function that returns the frame to which the message transforms.
Definition transformer.h:616
mrs_lib::Transformer::transformSingle
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))
Transforms a single variable to a new frame and returns it or std::nullopt if transformation fails.
Definition transformer.h:382
mrs_lib::Transformer::transformSingle
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))
Transforms a single variable to a new frame and returns it or std::nullopt if transformation fails.
Definition transformer.h:363
mrs_lib::Transformer::inverse
static geometry_msgs::msg::TransformStamped inverse(const geometry_msgs::msg::TransformStamped &msg)
A convenience function implements returns the inverse of the transform message as a one-liner.
Definition transformer.h:640
mrs_lib::Transformer::getTransform
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))
Obtains a transform between two frames in a given time.
Definition transformer.cpp:122
mrs_lib::Transformer::frame_from
static constexpr std::string & frame_from(geometry_msgs::msg::TransformStamped &msg)
A convenience function that returns the frame from which the message transforms.
Definition transformer.h:605
mrs_lib::Transformer::transform
std::optional< std::shared_ptr< T > > transform(const std::shared_ptr< T > &what, const geometry_msgs::msg::TransformStamped &tf)
Transform a variable to new frame using a particular transformation.
Definition transformer.h:438
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:164
mrs_lib::Transformer::transformAsPoint
std::optional< Eigen::Vector3d > transformAsPoint(const Eigen::Vector3d &what, const geometry_msgs::msg::TransformStamped &tf)
Transform an Eigen::Vector3d (interpreting it as a point).
Definition transformer.cpp:234
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:183
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:145
mrs_lib::Transformer::operator=
Transformer & operator=(Transformer &&other)
A convenience move assignment operator.
Definition transformer.cpp:97
mrs_lib::Transformer::transform
std::optional< std::shared_ptr< T > > transform(const std::shared_ptr< const T > &what, const geometry_msgs::msg::TransformStamped &tf)
Transform a variable to new frame using a particular transformation.
Definition transformer.h:418
mrs_lib::Transformer::transformAsVector
std::optional< Eigen::Vector3d > transformAsVector(const Eigen::Vector3d &what, const geometry_msgs::msg::TransformStamped &tf)
Transform an Eigen::Vector3d (interpreting it as a vector).
Definition transformer.cpp:177
mrs_lib::Transformer::frame_from
static constexpr const std::string & frame_from(const geometry_msgs::msg::TransformStamped &msg)
A convenience function that returns the frame from which the message transforms.
Definition transformer.h:592
mrs_lib::Transformer::setLookupTimeout
void setLookupTimeout(const rclcpp::Duration timeout=rclcpp::Duration(0, 0))
Set a timeout for transform lookup.
Definition transformer.h:221
mrs_lib::Transformer::frame_to
static constexpr std::string & frame_to(geometry_msgs::msg::TransformStamped &msg)
A convenience function that returns the frame to which the message transforms.
Definition transformer.h:629
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:278
mrs_lib::Transformer::beQuiet
void beQuiet(const bool quiet=true)
Enable/disable some prints that may be too noisy.
Definition transformer.h:258
mrs_lib::Transformer::retryLookupNewest
void retryLookupNewest(const bool retry=true)
Enable/disable retry of a failed transform lookup with rclcpp::Time(0).
Definition transformer.h:241
mrs_lib::Transformer::Transformer
Transformer()
A convenience constructor that doesn't initialize anything.
Definition transformer.cpp:44
mrs_lib::Transformer::transformSingle
std::optional< std::shared_ptr< T > > transformSingle(const std::shared_ptr< T > &what, const std::string &to_frame)
Transforms a single variable to a new frame.
Definition transformer.h:309
misc.h
Defines useful geometry utilities and functions.
mrs_lib
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24