|
| 1 | +// Copyright 2024 TIER IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__STATIC_TRANSFORM_BUFFER_HPP_ |
| 16 | +#define AUTOWARE__UNIVERSE_UTILS__ROS__STATIC_TRANSFORM_BUFFER_HPP_ |
| 17 | + |
| 18 | +#include "autoware/universe_utils/ros/transform_listener.hpp" |
| 19 | + |
| 20 | +#include <eigen3/Eigen/Core> |
| 21 | +#include <pcl_ros/transforms.hpp> |
| 22 | +#include <rclcpp/rclcpp.hpp> |
| 23 | + |
| 24 | +#include <sensor_msgs/point_cloud2_iterator.hpp> |
| 25 | + |
| 26 | +#include <pcl_conversions/pcl_conversions.h> |
| 27 | + |
| 28 | +#include <chrono> |
| 29 | +#include <functional> |
| 30 | +#include <memory> |
| 31 | +#include <string> |
| 32 | +#include <unordered_map> |
| 33 | +#include <utility> |
| 34 | + |
| 35 | +namespace std |
| 36 | +{ |
| 37 | +template <> |
| 38 | +struct hash<std::pair<std::string, std::string>> |
| 39 | +{ |
| 40 | + size_t operator()(const std::pair<std::string, std::string> & p) const |
| 41 | + { |
| 42 | + size_t h1 = std::hash<std::string>{}(p.first); |
| 43 | + size_t h2 = std::hash<std::string>{}(p.second); |
| 44 | + return h1 ^ (h2 << 1u); |
| 45 | + } |
| 46 | +}; |
| 47 | +} // namespace std |
| 48 | + |
| 49 | +namespace autoware::universe_utils |
| 50 | +{ |
| 51 | +using std::chrono_literals::operator""ms; |
| 52 | +using Key = std::pair<std::string, std::string>; |
| 53 | +struct PairEqual |
| 54 | +{ |
| 55 | + bool operator()(const Key & p1, const Key & p2) const |
| 56 | + { |
| 57 | + return p1.first == p2.first && p1.second == p2.second; |
| 58 | + } |
| 59 | +}; |
| 60 | +using TFMap = std::unordered_map<Key, Eigen::Matrix4f, std::hash<Key>, PairEqual>; |
| 61 | + |
| 62 | +class StaticTransformBuffer |
| 63 | +{ |
| 64 | +public: |
| 65 | + StaticTransformBuffer() = default; |
| 66 | + |
| 67 | + /** |
| 68 | + * @brief Retrieves a transform between two static frames. |
| 69 | + * |
| 70 | + * This function attempts to retrieve a transform between the target frame and the source frame. |
| 71 | + * If success, the transform matrix is set to the output parameter and the function returns true. |
| 72 | + * Otherwise, transform matrix is set to identity and the function returns false. Transform |
| 73 | + * Listener is destroyed after function call. |
| 74 | + * |
| 75 | + * @param node A pointer to the ROS node. |
| 76 | + * @param target_frame The target frame. |
| 77 | + * @param source_frame The source frame. |
| 78 | + * @param eigen_transform The output Eigen transform matrix. Is set to identity if the transform |
| 79 | + * is not found. |
| 80 | + * @return True if the transform was successfully retrieved, false otherwise. |
| 81 | + */ |
| 82 | + bool getTransform( |
| 83 | + rclcpp::Node * node, const std::string & target_frame, const std::string & source_frame, |
| 84 | + Eigen::Matrix4f & eigen_transform) |
| 85 | + { |
| 86 | + auto key = std::make_pair(target_frame, source_frame); |
| 87 | + auto key_inv = std::make_pair(source_frame, target_frame); |
| 88 | + |
| 89 | + // Check if the transform is already in the buffer |
| 90 | + auto it = buffer_.find(key); |
| 91 | + if (it != buffer_.end()) { |
| 92 | + eigen_transform = it->second; |
| 93 | + return true; |
| 94 | + } |
| 95 | + |
| 96 | + // Check if the inverse transform is already in the buffer |
| 97 | + auto it_inv = buffer_.find(key_inv); |
| 98 | + if (it_inv != buffer_.end()) { |
| 99 | + eigen_transform = it_inv->second.inverse(); |
| 100 | + buffer_[key] = eigen_transform; |
| 101 | + return true; |
| 102 | + } |
| 103 | + |
| 104 | + // Check if transform is needed |
| 105 | + if (target_frame == source_frame) { |
| 106 | + eigen_transform = Eigen::Matrix4f::Identity(); |
| 107 | + buffer_[key] = eigen_transform; |
| 108 | + return true; |
| 109 | + } |
| 110 | + |
| 111 | + // Get the transform from the TF tree |
| 112 | + auto tf_listener = std::make_unique<autoware::universe_utils::TransformListener>(node); |
| 113 | + auto tf = tf_listener->getTransform( |
| 114 | + target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration(1000ms)); |
| 115 | + RCLCPP_DEBUG( |
| 116 | + node->get_logger(), "Trying to enqueue %s -> %s transform to static TFs buffer...", |
| 117 | + target_frame.c_str(), source_frame.c_str()); |
| 118 | + if (tf == nullptr) { |
| 119 | + eigen_transform = Eigen::Matrix4f::Identity(); |
| 120 | + return false; |
| 121 | + } |
| 122 | + pcl_ros::transformAsMatrix(*tf, eigen_transform); |
| 123 | + buffer_[key] = eigen_transform; |
| 124 | + return true; |
| 125 | + } |
| 126 | + |
| 127 | + /** |
| 128 | + * Transforms a point cloud from one frame to another. |
| 129 | + * |
| 130 | + * @param node A pointer to the ROS node. |
| 131 | + * @param target_frame The target frame to transform the point cloud to. |
| 132 | + * @param cloud_in The input point cloud to be transformed. |
| 133 | + * @param cloud_out The transformed point cloud. |
| 134 | + * @return True if the transformation is successful, false otherwise. |
| 135 | + */ |
| 136 | + bool transformPointcloud( |
| 137 | + rclcpp::Node * node, const std::string & target_frame, |
| 138 | + const sensor_msgs::msg::PointCloud2 & cloud_in, sensor_msgs::msg::PointCloud2 & cloud_out) |
| 139 | + { |
| 140 | + if ( |
| 141 | + pcl::getFieldIndex(cloud_in, "x") == -1 || pcl::getFieldIndex(cloud_in, "y") == -1 || |
| 142 | + pcl::getFieldIndex(cloud_in, "z") == -1) { |
| 143 | + RCLCPP_ERROR(node->get_logger(), "Input pointcloud does not have xyz fields"); |
| 144 | + return false; |
| 145 | + } |
| 146 | + if (target_frame == cloud_in.header.frame_id) { |
| 147 | + cloud_out = cloud_in; |
| 148 | + return true; |
| 149 | + } |
| 150 | + Eigen::Matrix4f eigen_transform; |
| 151 | + if (!getTransform(node, target_frame, cloud_in.header.frame_id, eigen_transform)) { |
| 152 | + return false; |
| 153 | + } |
| 154 | + pcl_ros::transformPointCloud(eigen_transform, cloud_in, cloud_out); |
| 155 | + cloud_out.header.frame_id = target_frame; |
| 156 | + return true; |
| 157 | + } |
| 158 | + |
| 159 | +private: |
| 160 | + TFMap buffer_; |
| 161 | +}; |
| 162 | + |
| 163 | +} // namespace autoware::universe_utils |
| 164 | + |
| 165 | +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__STATIC_TRANSFORM_BUFFER_HPP_ |
0 commit comments