Branch data Line data Source code
1 : : // Copyright 2020, The Autoware Foundation.
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 : : /// \file
15 : : /// \brief This file includes common transform functionality for
16 : : /// autoware_auto_geometry_msgs and autoware_auto_perception_msgs
17 : :
18 : : #ifndef AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_
19 : : #define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_
20 : :
21 : : #include <tf2/convert.h>
22 : : #include <tf2/time.h>
23 : : #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
24 : : #include <autoware_auto_perception_msgs/msg/bounding_box_array.hpp>
25 : : #include <autoware_auto_perception_msgs/msg/bounding_box.hpp>
26 : : #include <autoware_auto_perception_msgs/msg/point_clusters.hpp>
27 : : #include <geometry_msgs/msg/transform_stamped.hpp>
28 : : #include <autoware_auto_geometry_msgs/msg/quaternion32.hpp>
29 : : #include <geometry_msgs/msg/polygon.hpp>
30 : : #include <geometry_msgs/msg/point32.hpp>
31 : : #include <kdl/frames.hpp>
32 : : #include <common/types.hpp>
33 : : #include <string>
34 : :
35 : :
36 : : using autoware::common::types::float32_t;
37 : : using autoware::common::types::float64_t;
38 : : using BoundingBoxArray = autoware_auto_perception_msgs::msg::BoundingBoxArray;
39 : : using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox;
40 : : using Clusters = autoware_auto_perception_msgs::msg::PointClusters;
41 : :
42 : : namespace tf2
43 : : {
44 : :
45 : :
46 : : /*************/
47 : : /** Point32 **/
48 : : /*************/
49 : :
50 : : /** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Point32 type.
51 : : * This function is a specialization of the doTransform template defined in tf2/convert.h.
52 : : * \param t_in The point to transform, as a Point32 message.
53 : : * \param t_out The transformed point, as a Point32 message.
54 : : * \param transform The timestamped transform to apply, as a TransformStamped message.
55 : : */
56 : : template<>
57 : : inline
58 : 27 : void doTransform(
59 : : const geometry_msgs::msg::Point32 & t_in, geometry_msgs::msg::Point32 & t_out,
60 : : const geometry_msgs::msg::TransformStamped & transform)
61 : : {
62 : 27 : const KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.x, t_in.y, t_in.z);
63 : 27 : t_out.x = static_cast<float>(v_out[0]);
64 : 27 : t_out.y = static_cast<float>(v_out[1]);
65 : 27 : t_out.z = static_cast<float>(v_out[2]);
66 : 27 : }
67 : :
68 : : /**************/
69 : : /** Clusters **/
70 : : /**************/
71 : :
72 : : /** \brief Apply a geometry_msgs TransformStamped to a Clusters type.
73 : : * This function is a specialization of the doTransform template defined in tf2/convert.h.
74 : : * \param t_in The Clusters to transform.
75 : : * \param t_out The transformed Clusters.
76 : : * \param transform The timestamped transform to apply, as a TransformStamped message.
77 : : */
78 : : // template<>
79 : : inline
80 : 1 : void doTransform(
81 : : const Clusters & t_in, Clusters & t_out,
82 : : const geometry_msgs::msg::TransformStamped & transform)
83 : : {
84 : : geometry_msgs::msg::Point32 tmp_in, tmp_out;
85 : 1 : t_out.points.resize(t_in.points.size());
86 [ + + ]: 2 : for (size_t i = 0; i < t_in.points.size(); ++i) {
87 : 1 : tmp_in.x = t_in.points[i].x;
88 : 1 : tmp_in.y = t_in.points[i].y;
89 : 1 : tmp_in.z = t_in.points[i].z;
90 : 1 : doTransform(tmp_in, tmp_out, transform);
91 : 1 : t_out.points[i].x = tmp_out.x;
92 : 1 : t_out.points[i].y = tmp_out.y;
93 : 1 : t_out.points[i].z = tmp_out.z;
94 : : }
95 : 1 : }
96 : :
97 : : /*************/
98 : : /** Polygon **/
99 : : /*************/
100 : :
101 : : /** \brief Apply a geometry_msgs TransformStamped to a geometry_msgs Polygon type.
102 : : * This function is a specialization of the doTransform template defined in tf2/convert.h.
103 : : * \param t_in The polygon to transform.
104 : : * \param t_out The transformed polygon.
105 : : * \param transform The timestamped transform to apply, as a TransformStamped message.
106 : : */
107 : : template<>
108 : : inline
109 : 1 : void doTransform(
110 : : const geometry_msgs::msg::Polygon & t_in, geometry_msgs::msg::Polygon & t_out,
111 : : const geometry_msgs::msg::TransformStamped & transform)
112 : : {
113 : : // Don't call the Point32 doTransform to avoid doing this conversion every time
114 : 1 : const auto kdl_frame = gmTransformToKDL(transform);
115 : : // We don't use std::back_inserter to allow aliasing between t_in and t_out
116 : 1 : t_out.points.resize(t_in.points.size());
117 [ + + ]: 2 : for (size_t i = 0; i < t_in.points.size(); ++i) {
118 : 1 : const KDL::Vector v_out = kdl_frame * KDL::Vector(
119 : 1 : t_in.points[i].x, t_in.points[i].y, t_in.points[i].z);
120 : 1 : t_out.points[i].x = static_cast<float>(v_out[0]);
121 : 1 : t_out.points[i].y = static_cast<float>(v_out[1]);
122 : 1 : t_out.points[i].z = static_cast<float>(v_out[2]);
123 : : }
124 : 1 : }
125 : :
126 : : /******************/
127 : : /** Quaternion32 **/
128 : : /******************/
129 : :
130 : : /** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_geometry_msgs Quaternion32
131 : : * type.
132 : : * This function is a specialization of the doTransform template defined in tf2/convert.h.
133 : : * \param t_in The Quaternion32 message to transform.
134 : : * \param t_out The transformed Quaternion32 message.
135 : : * \param transform The timestamped transform to apply, as a TransformStamped message.
136 : : */
137 : : template<>
138 : : inline
139 : 6 : void doTransform(
140 : : const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in,
141 : : autoware_auto_geometry_msgs::msg::Quaternion32 & t_out,
142 : : const geometry_msgs::msg::TransformStamped & transform)
143 : : {
144 : 6 : KDL::Rotation r_in = KDL::Rotation::Quaternion(t_in.x, t_in.y, t_in.z, t_in.w);
145 : 6 : KDL::Rotation out = gmTransformToKDL(transform).M * r_in;
146 : :
147 : : double qx, qy, qz, qw;
148 : 6 : out.GetQuaternion(qx, qy, qz, qw);
149 : 6 : t_out.x = static_cast<float32_t>(qx);
150 : 6 : t_out.y = static_cast<float32_t>(qy);
151 : 6 : t_out.z = static_cast<float32_t>(qz);
152 : 6 : t_out.w = static_cast<float32_t>(qw);
153 : 6 : }
154 : :
155 : :
156 : : /******************/
157 : : /** BoundingBox **/
158 : : /******************/
159 : :
160 : : /** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_perception_msgs BoundingBox
161 : : * type.
162 : : * This function is a specialization of the doTransform template defined in tf2/convert.h.
163 : : * \param t_in The BoundingBox message to transform.
164 : : * \param t_out The transformed BoundingBox message.
165 : : * \param transform The timestamped transform to apply, as a TransformStamped message.
166 : : */
167 : : template<>
168 : : inline
169 : 5 : void doTransform(
170 : : const BoundingBox & t_in, BoundingBox & t_out,
171 : : const geometry_msgs::msg::TransformStamped & transform)
172 : : {
173 : 5 : t_out = t_in;
174 : 5 : doTransform(t_in.orientation, t_out.orientation, transform);
175 : 5 : doTransform(t_in.centroid, t_out.centroid, transform);
176 : 5 : doTransform(t_in.corners[0], t_out.corners[0], transform);
177 : 5 : doTransform(t_in.corners[1], t_out.corners[1], transform);
178 : 5 : doTransform(t_in.corners[2], t_out.corners[2], transform);
179 : 5 : doTransform(t_in.corners[3], t_out.corners[3], transform);
180 : : // TODO(jitrc): add conversion for other fields of BoundingBox, such as heading, variance, size
181 : 5 : }
182 : :
183 : :
184 : : /**********************/
185 : : /** BoundingBoxArray **/
186 : : /**********************/
187 : :
188 : : /** \brief Extract a timestamp from the header of a BoundingBoxArray message.
189 : : * This function is a specialization of the getTimestamp template defined in tf2/convert.h.
190 : : * \param t A timestamped BoundingBoxArray message to extract the timestamp from.
191 : : * \return The timestamp of the message.
192 : : */
193 : : template<>
194 : : inline
195 : : tf2::TimePoint getTimestamp(const BoundingBoxArray & t)
196 : : {
197 : 2 : return tf2_ros::fromMsg(t.header.stamp);
198 : : }
199 : :
200 : : /** \brief Extract a frame ID from the header of a BoundingBoxArray message.
201 : : * This function is a specialization of the getFrameId template defined in tf2/convert.h.
202 : : * \param t A timestamped BoundingBoxArray message to extract the frame ID from.
203 : : * \return A string containing the frame ID of the message.
204 : : */
205 : : template<>
206 : : inline
207 : : std::string getFrameId(const BoundingBoxArray & t) {return t.header.frame_id;}
208 : :
209 : : /** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_perception_msgs
210 : : * BoundingBoxArray type.
211 : : * This function is a specialization of the doTransform template defined in tf2/convert.h.
212 : : * \param t_in The BoundingBoxArray to transform, as a timestamped BoundingBoxArray message.
213 : : * \param t_out The transformed BoundingBoxArray, as a timestamped BoundingBoxArray message.
214 : : * \param transform The timestamped transform to apply, as a TransformStamped message.
215 : : */
216 : : template<>
217 : : inline
218 : 2 : void doTransform(
219 : : const BoundingBoxArray & t_in,
220 : : BoundingBoxArray & t_out,
221 : : const geometry_msgs::msg::TransformStamped & transform)
222 : : {
223 : : t_out = t_in;
224 [ + + ]: 6 : for (auto idx = 0U; idx < t_in.boxes.size(); idx++) {
225 : 4 : doTransform(t_out.boxes[idx], t_out.boxes[idx], transform);
226 : : }
227 : 2 : t_out.header.stamp = transform.header.stamp;
228 : 2 : t_out.header.frame_id = transform.header.frame_id;
229 : 2 : }
230 : :
231 : : } // namespace tf2
232 : :
233 : : #endif // AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_
|
