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
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
74 {
75
76 public:
77 /* Constructor, assignment operator and overloads //{ */
78
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
144
164 Transformer(const rclcpp::Node::SharedPtr& node, const buffer_mode_t buffer_mode);
165
175
176 //}
177
178 // | ------------------ Configuration methods ----------------- |
179
180 /* setDefaultFrame() //{ */
181
191 void setDefaultFrame(const std::string& frame_id)
192 {
193 std::scoped_lock lck(mutex_);
194 default_frame_id_ = frame_id;
195 }
196
197 //}
198
199 /* setDefaultPrefix() //{ */
200
214 void setDefaultPrefix(const std::string& prefix)
215 {
216 std::scoped_lock lck(mutex_);
217 if (prefix.empty())
218 prefix_ = "";
219 else
220 prefix_ = prefix + "/";
221 }
222
223 //}
224
225 /* setLatLon() //{ */
226
237 void setLatLon(const double lat, const double lon);
238
239 //}
240
241 /* setLookupTimeout() //{ */
242
252 void setLookupTimeout(const rclcpp::Duration timeout = rclcpp::Duration(0, 0))
253 {
254 std::scoped_lock lck(mutex_);
255 lookup_timeout_ = timeout;
256 }
257
258 //}
259
260 /* retryLookupNewest() //{ */
261
272 void retryLookupNewest(const bool retry = true)
273 {
274 std::scoped_lock lck(mutex_);
275 retry_lookup_newest_ = retry;
276 }
277
278 //}
279
280 /* beQuiet() //{ */
281
289 void beQuiet(const bool quiet = true)
290 {
291 std::scoped_lock lck(mutex_);
292 quiet_ = quiet;
293 }
294
295 //}
296
297 /* resolveFrame() //{ */
309 std::string resolveFrame(const std::string& frame_id)
310 {
311 std::scoped_lock lck(mutex_);
312 return resolveFrameImpl(frame_id);
313 }
314 //}
315
316 /* transformSingle() //{ */
317
326 template <class T>
327 [[nodiscard]] std::optional<T> transformSingle(const T& what, const std::string& to_frame);
328
339 template <class T>
340 [[nodiscard]] std::optional<std::shared_ptr<T>> transformSingle(const std::shared_ptr<T>& what, const std::string& to_frame)
341 {
342 return transformSingle(std::shared_ptr<const T>(what), to_frame);
343 }
344
355 template <class T>
356 [[nodiscard]] std::optional<std::shared_ptr<T>> transformSingle(const std::shared_ptr<const T>& what, const std::string& to_frame)
357 {
358 auto ret = transformSingle(*what, to_frame);
359 if (ret == std::nullopt)
360 return std::nullopt;
361 else
362 return std::make_shared<T>(std::move(ret.value()));
363 }
364
377 template <class T>
378 [[nodiscard]] std::optional<T> transformSingle(const std::string& from_frame, const T& what, const std::string& to_frame,
379 const rclcpp::Time& time_stamp = rclcpp::Time(0));
380
393 template <class T>
394 [[nodiscard]] std::optional<std::shared_ptr<T>> transformSingle(const std::string& from_frame, const std::shared_ptr<T>& what, const std::string& to_frame,
395 const rclcpp::Time& time_stamp = rclcpp::Time(0))
396 {
397 return transformSingle(from_frame, std::shared_ptr<const T>(what), to_frame, time_stamp);
398 }
399
412 template <class T>
413 [[nodiscard]] std::optional<std::shared_ptr<T>> transformSingle(const std::string& from_frame, const std::shared_ptr<const T>& what,
414 const std::string& to_frame, const rclcpp::Time& time_stamp = rclcpp::Time(0))
415 {
416 auto ret = transformSingle(from_frame, *what, to_frame, time_stamp);
417 if (ret == std::nullopt)
418 return std::nullopt;
419 else
420 return std::make_shared<T>(std::move(ret.value()));
421 }
422
423 //}
424
425 /* transform() //{ */
426
435 template <class T>
436 [[nodiscard]] std::optional<T> transform(const T& what, const geometry_msgs::msg::TransformStamped& tf);
437
448 template <class T>
449 [[nodiscard]] std::optional<std::shared_ptr<T>> transform(const std::shared_ptr<const T>& what, const geometry_msgs::msg::TransformStamped& tf)
450 {
451 auto ret = transform(*what, tf);
452 if (ret == std::nullopt)
453 return std::nullopt;
454 else
455 return std::make_shared<T>(std::move(ret.value()));
456 }
457
468 template <class T>
469 [[nodiscard]] std::optional<std::shared_ptr<T>> transform(const std::shared_ptr<T>& what, const geometry_msgs::msg::TransformStamped& tf)
470 {
471 return transform(std::shared_ptr<const T>(what), tf);
472 }
473
474 //}
475
476 /* transformAsVector() method //{ */
487 [[nodiscard]] std::optional<Eigen::Vector3d> transformAsVector(const Eigen::Vector3d& what, const geometry_msgs::msg::TransformStamped& tf);
488
501 [[nodiscard]] std::optional<Eigen::Vector3d> transformAsVector(const std::string& from_frame, const Eigen::Vector3d& what, const std::string& to_frame,
502 const rclcpp::Time& time_stamp = rclcpp::Time(0));
503 //}
504
505 /* transformAsPoint() method //{ */
516 [[nodiscard]] std::optional<Eigen::Vector3d> transformAsPoint(const Eigen::Vector3d& what, const geometry_msgs::msg::TransformStamped& tf);
517
530 [[nodiscard]] std::optional<Eigen::Vector3d> transformAsPoint(const std::string& from_frame, const Eigen::Vector3d& what, const std::string& to_frame,
531 const rclcpp::Time& time_stamp = rclcpp::Time(0));
532 //}
533
534 /* getTransform() //{ */
535
545 [[nodiscard]] std::optional<geometry_msgs::msg::TransformStamped> getTransform(const std::string& from_frame, const std::string& to_frame,
546 const rclcpp::Time& time_stamp = rclcpp::Time(0));
547
563 [[nodiscard]] std::optional<geometry_msgs::msg::TransformStamped> getTransform(const std::string& from_frame, const rclcpp::Time& from_stamp,
564 const std::string& to_frame, const rclcpp::Time& to_stamp,
565 const std::string& fixed_frame);
566
567 //}
568
569 private:
570 /* private members, methods etc //{ */
571
572 std::mutex mutex_;
573
574 // keeps track whether a non-basic constructor was called and the transform listener is initialized
575 bool initialized_ = false;
576 rclcpp::Node::SharedPtr node_;
577
578 // own or shared buffer, empty until bound in the shared mode
579 std::shared_ptr<tf2_ros::BufferInterface> tf_buffer_;
580 // only with an own buffer, destroyed before tf_buffer_
581 std::unique_ptr<tf2_ros::TransformListener> tf_listener_ptr_;
582 buffer_mode_t buffer_mode_ = buffer_mode_t::own;
583
584 // user-configurable options
585 std::string default_frame_id_ = "";
586 std::string prefix_ = ""; // if not empty, includes the forward slash
587 bool quiet_ = false;
588 rclcpp::Duration lookup_timeout_ = rclcpp::Duration(0, 0);
589 bool retry_lookup_newest_ = false;
590
591 bool got_utm_zone_ = false;
592 std::array<char, 10> utm_zone_ = {};
593
594 // returns the first namespace prefix of the frame (if any) includin the forward slash
595 std::string getFramePrefix(const std::string& frame_id);
596
597 template <class T>
598 std::optional<T> transformImpl(const geometry_msgs::msg::TransformStamped& tf, const T& what);
599 std::optional<mrs_msgs::msg::ReferenceStamped> transformImpl(const geometry_msgs::msg::TransformStamped& tf, const mrs_msgs::msg::ReferenceStamped& what);
600 std::optional<Eigen::Vector3d> transformImpl(const geometry_msgs::msg::TransformStamped& tf, const Eigen::Vector3d& what);
601
602 [[nodiscard]] std::optional<geometry_msgs::msg::TransformStamped> getTransformImpl(const std::string& from_frame, const std::string& to_frame,
603 const rclcpp::Time& time_stamp, const std::string& latlon_frame);
604
605 [[nodiscard]] std::optional<geometry_msgs::msg::TransformStamped> getTransformImpl(const std::string& from_frame, const rclcpp::Time& from_stamp,
606 const std::string& to_frame, const rclcpp::Time& to_stamp,
607 const std::string& fixed_frame, const std::string& latlon_frame);
608
609 std::string resolveFrameImpl(const std::string& frame_id);
610
611 // binds to the shared buffer, returns false if it has no owner yet
612 bool bindSharedBufferImpl();
613
614 template <class T>
615 std::optional<T> doTransform(const T& what, const geometry_msgs::msg::TransformStamped& tf);
616
617 //}
618
619 // | ------------------- some helper methods ------------------ |
620 public:
621 /* frame_from(), frame_to() and inverse() methods //{ */
622
629 static constexpr const std::string& frame_from(const geometry_msgs::msg::TransformStamped& msg)
630 {
631 return msg.child_frame_id;
632 }
633
642 static constexpr std::string& frame_from(geometry_msgs::msg::TransformStamped& msg)
643 {
644 return msg.child_frame_id;
645 }
646
653 static constexpr const std::string& frame_to(const geometry_msgs::msg::TransformStamped& msg)
654 {
655 return msg.header.frame_id;
656 }
657
666 static constexpr std::string& frame_to(geometry_msgs::msg::TransformStamped& msg)
667 {
668 return msg.header.frame_id;
669 }
670
677 static geometry_msgs::msg::TransformStamped inverse(const geometry_msgs::msg::TransformStamped& msg)
678 {
679 tf2::Transform tf2;
680 tf2::fromMsg(msg.transform, tf2);
681 tf2 = tf2.inverse();
682 return create_transform(frame_to(msg), frame_from(msg), msg.header.stamp, tf2::toMsg(tf2));
683 }
684 //}
685
686 private:
687 /* create_transform() method //{ */
688 static geometry_msgs::msg::TransformStamped create_transform(const std::string& from_frame, const std::string& to_frame, const rclcpp::Time& time_stamp)
689 {
690 geometry_msgs::msg::TransformStamped ret;
691 frame_from(ret) = from_frame;
692 frame_to(ret) = to_frame;
693 ret.header.stamp = time_stamp;
694 return ret;
695 }
696
697 static geometry_msgs::msg::TransformStamped create_transform(const std::string& from_frame, const std::string& to_frame, const rclcpp::Time& time_stamp,
698 const geometry_msgs::msg::Transform& tf)
699 {
700 geometry_msgs::msg::TransformStamped ret;
701 frame_from(ret) = from_frame;
702 frame_to(ret) = to_frame;
703 ret.header.stamp = time_stamp;
704 ret.transform = tf;
705 return ret;
706 }
707 //}
708
709 /* copyChangeFrame() and related methods //{ */
710
711 // helper type and member for detecting whether a message has a header using SFINAE
712 template <typename T>
713 using has_header_member_chk = decltype(std::declval<T&>().header);
714
715 template <typename T>
716 static constexpr bool has_header_member_v = std::experimental::is_detected<has_header_member_chk, T>::value;
717
718 template <typename msg_t>
719 std_msgs::msg::Header getHeader(const msg_t& msg);
720
721#ifdef PCL_SPECIALIZATION
722 template <typename pt_t>
723 std_msgs::msg::Header getHeader(const pcl::PointCloud<pt_t>& cloud);
724
725 template <typename pt_t>
726 void setHeader(pcl::PointCloud<pt_t>& cloud, const std_msgs::msg::Header& header);
727#endif
728
729 template <typename msg_t>
730 void setHeader(msg_t& msg, const std_msgs::msg::Header& header);
731
732 template <typename T>
733 T copyChangeFrame(const T& what, const std::string& frame_id);
734
735 //}
736
737 /* methods for converting between lattitude/longitude and UTM coordinates //{ */
738 geometry_msgs::msg::Point LLtoUTM(const geometry_msgs::msg::Point& what, const std::string& prefix);
739 geometry_msgs::msg::PointStamped LLtoUTM(const geometry_msgs::msg::PointStamped& what, const std::string& prefix);
740 geometry_msgs::msg::Pose LLtoUTM(const geometry_msgs::msg::Pose& what, const std::string& prefix);
741 geometry_msgs::msg::PoseStamped LLtoUTM(const geometry_msgs::msg::PoseStamped& what, const std::string& prefix);
742
743 std::optional<geometry_msgs::msg::Point> UTMtoLL(const geometry_msgs::msg::Point& what, const std::string& prefix);
744 std::optional<geometry_msgs::msg::PointStamped> UTMtoLL(const geometry_msgs::msg::PointStamped& what, const std::string& prefix);
745 std::optional<geometry_msgs::msg::Pose> UTMtoLL(const geometry_msgs::msg::Pose& what, const std::string& prefix);
746 std::optional<geometry_msgs::msg::PoseStamped> UTMtoLL(const geometry_msgs::msg::PoseStamped& what, const std::string& prefix);
747
748 // helper types and member for detecting whether the UTMtoLL and LLtoUTM methods are defined for a certain message
749 template <class Class, typename Message>
750 using UTMLL_method_chk = decltype(std::declval<Class>().UTMtoLL(std::declval<const Message&>(), ""));
751
752 template <class Class, typename Message>
753 using LLUTM_method_chk = decltype(std::declval<Class>().LLtoUTM(std::declval<const Message&>(), ""));
754
755 template <class Class, typename Message>
756 static constexpr bool UTMLL_exists_v =
757 std::experimental::is_detected<UTMLL_method_chk, Class, Message>::value && std::experimental::is_detected<LLUTM_method_chk, Class, Message>::value;
758 //}
759 };
760
761 //}
762
763} // namespace mrs_lib
764
765#ifndef TRANSFORMER_HPP
766#include <mrs_lib/impl/transformer.hpp>
767#endif
768
769#endif // TRANSFORMER_H
A convenience wrapper class for ROS's native TF2 API to simplify transforming of various messages.
Definition transformer.h:74
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:356
void setDefaultFrame(const std::string &frame_id)
Sets the default frame ID to be used instead of any empty frame ID.
Definition transformer.h:191
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
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:653
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:413
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:394
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:677
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:223
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:642
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:469
void setLatLon(const double lat, const double lon)
Sets the curret lattitude and longitude for UTM zone calculation.
Definition transformer.cpp:265
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:335
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:214
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
Transformer & operator=(Transformer &&other)
A convenience move assignment operator.
Definition transformer.cpp:195
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:449
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:278
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:629
void setLookupTimeout(const rclcpp::Duration timeout=rclcpp::Duration(0, 0))
Set a timeout for transform lookup.
Definition transformer.h:252
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:666
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:309
void beQuiet(const bool quiet=true)
Enable/disable some prints that may be too noisy.
Definition transformer.h:289
void retryLookupNewest(const bool retry=true)
Enable/disable retry of a failed transform lookup with rclcpp::Time(0).
Definition transformer.h:272
Transformer()
A convenience constructor that doesn't initialize anything.
Definition transformer.cpp:68
buffer_mode_t
Selects where a Transformer keeps its transformations.
Definition transformer.h:139
@ shared_owner
like own, but the buffer is also offered to all Transformers in the shared mode within this process
Definition transformer.h:141
@ shared
the Transformer uses the buffer of the shared_owner Transformer in this process
Definition transformer.h:142
@ own
the Transformer has its own TF2 buffer and transform listener, same as with the other constructors
Definition transformer.h:140
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:340
Defines useful geometry utilities and functions.
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24