17#include <rclcpp/rclcpp.hpp>
19#include <tf2_ros/transform_listener.hpp>
20#include <tf2_ros/buffer.hpp>
21#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
23#include <mrs_msgs/msg/reference_stamped.hpp>
24#include <geometry_msgs/msg/pose_stamped.hpp>
25#include <geometry_msgs/msg/vector3_stamped.hpp>
27#include <std_msgs/msg/header.hpp>
31#ifdef PCL_SPECIALIZATION
32#include <pcl_ros/point_cloud.hpp>
33#include <pcl_ros/transforms.hpp>
34#include <pcl_conversions/pcl_conversions.h>
38#include <experimental/type_traits>
42#ifdef PCL_SPECIALIZATION
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)
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;
62 static const std::string UTM_ORIGIN =
"utm_origin";
63 static const std::string LATLON_ORIGIN =
"latlon_origin";
104 Transformer(
const rclcpp::Node::SharedPtr& node,
const bool spin_thread =
true,
const rclcpp::CallbackGroup::SharedPtr& callback_group =
nullptr);
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);
193 std::scoped_lock lck(mutex_);
194 default_frame_id_ = frame_id;
216 std::scoped_lock lck(mutex_);
220 prefix_ = prefix +
"/";
237 void setLatLon(
const double lat,
const double lon);
254 std::scoped_lock lck(mutex_);
255 lookup_timeout_ = timeout;
274 std::scoped_lock lck(mutex_);
275 retry_lookup_newest_ = retry;
291 std::scoped_lock lck(mutex_);
311 std::scoped_lock lck(mutex_);
312 return resolveFrameImpl(frame_id);
327 [[nodiscard]] std::optional<T>
transformSingle(
const T& what,
const std::string& to_frame);
340 [[nodiscard]] std::optional<std::shared_ptr<T>>
transformSingle(
const std::shared_ptr<T>& what,
const std::string& to_frame)
356 [[nodiscard]] std::optional<std::shared_ptr<T>>
transformSingle(
const std::shared_ptr<const T>& what,
const std::string& to_frame)
359 if (ret == std::nullopt)
362 return std::make_shared<T>(std::move(ret.value()));
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));
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))
397 return transformSingle(from_frame, std::shared_ptr<const T>(what), to_frame, time_stamp);
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))
417 if (ret == std::nullopt)
420 return std::make_shared<T>(std::move(ret.value()));
436 [[nodiscard]] std::optional<T>
transform(
const T& what,
const geometry_msgs::msg::TransformStamped& tf);
449 [[nodiscard]] std::optional<std::shared_ptr<T>>
transform(
const std::shared_ptr<const T>& what,
const geometry_msgs::msg::TransformStamped& tf)
452 if (ret == std::nullopt)
455 return std::make_shared<T>(std::move(ret.value()));
469 [[nodiscard]] std::optional<std::shared_ptr<T>>
transform(
const std::shared_ptr<T>& what,
const geometry_msgs::msg::TransformStamped& tf)
471 return transform(std::shared_ptr<const T>(what), tf);
487 [[nodiscard]] std::optional<Eigen::Vector3d>
transformAsVector(
const Eigen::Vector3d& what,
const geometry_msgs::msg::TransformStamped& tf);
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));
516 [[nodiscard]] std::optional<Eigen::Vector3d>
transformAsPoint(
const Eigen::Vector3d& what,
const geometry_msgs::msg::TransformStamped& tf);
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));
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));
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);
575 bool initialized_ =
false;
576 rclcpp::Node::SharedPtr node_;
579 std::shared_ptr<tf2_ros::BufferInterface> tf_buffer_;
581 std::unique_ptr<tf2_ros::TransformListener> tf_listener_ptr_;
585 std::string default_frame_id_ =
"";
586 std::string prefix_ =
"";
588 rclcpp::Duration lookup_timeout_ = rclcpp::Duration(0, 0);
589 bool retry_lookup_newest_ =
false;
591 bool got_utm_zone_ =
false;
592 std::array<char, 10> utm_zone_ = {};
595 std::string getFramePrefix(
const std::string& frame_id);
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);
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);
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);
609 std::string resolveFrameImpl(
const std::string& frame_id);
612 bool bindSharedBufferImpl();
615 std::optional<T> doTransform(
const T& what,
const geometry_msgs::msg::TransformStamped& tf);
629 static constexpr const std::string&
frame_from(
const geometry_msgs::msg::TransformStamped& msg)
631 return msg.child_frame_id;
642 static constexpr std::string&
frame_from(geometry_msgs::msg::TransformStamped& msg)
644 return msg.child_frame_id;
653 static constexpr const std::string&
frame_to(
const geometry_msgs::msg::TransformStamped& msg)
655 return msg.header.frame_id;
666 static constexpr std::string&
frame_to(geometry_msgs::msg::TransformStamped& msg)
668 return msg.header.frame_id;
677 static geometry_msgs::msg::TransformStamped
inverse(
const geometry_msgs::msg::TransformStamped& msg)
680 tf2::fromMsg(msg.transform, tf2);
682 return create_transform(
frame_to(msg),
frame_from(msg), msg.header.stamp, tf2::toMsg(tf2));
688 static geometry_msgs::msg::TransformStamped create_transform(
const std::string& from_frame,
const std::string& to_frame,
const rclcpp::Time& time_stamp)
690 geometry_msgs::msg::TransformStamped ret;
693 ret.header.stamp = time_stamp;
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)
700 geometry_msgs::msg::TransformStamped ret;
703 ret.header.stamp = time_stamp;
712 template <
typename T>
713 using has_header_member_chk =
decltype(std::declval<T&>().header);
715 template <
typename T>
716 static constexpr bool has_header_member_v = std::experimental::is_detected<has_header_member_chk, T>::value;
718 template <
typename msg_t>
719 std_msgs::msg::Header getHeader(
const msg_t& msg);
721#ifdef PCL_SPECIALIZATION
722 template <
typename pt_t>
723 std_msgs::msg::Header getHeader(
const pcl::PointCloud<pt_t>& cloud);
725 template <
typename pt_t>
726 void setHeader(pcl::PointCloud<pt_t>& cloud,
const std_msgs::msg::Header& header);
729 template <
typename msg_t>
730 void setHeader(msg_t& msg,
const std_msgs::msg::Header& header);
732 template <
typename T>
733 T copyChangeFrame(
const T& what,
const std::string& frame_id);
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);
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);
749 template <
class Class,
typename Message>
750 using UTMLL_method_chk =
decltype(std::declval<Class>().UTMtoLL(std::declval<const Message&>(),
""));
752 template <
class Class,
typename Message>
753 using LLUTM_method_chk =
decltype(std::declval<Class>().LLtoUTM(std::declval<const Message&>(),
""));
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;
765#ifndef TRANSFORMER_HPP
766#include <mrs_lib/impl/transformer.hpp>
Defines useful geometry utilities and functions.
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24