47
47
48
48
namespace tf2
49
49
{
50
- /* *\brief The templated function expected to be able to do a transform
50
+ /* * \brief The templated function expected to be able to do a transform.
51
51
*
52
52
* This is the method which tf2 will use to try to apply a transform for any given datatype.
53
- * \param data_in[in] The data to be transformed.
54
- * \param data_out[inout] A reference to the output data. Note this can point to data in and the method should be mutation safe.
55
- * \param transform[in] The transform to apply to data_in to fill data_out.
53
+ * \param[in] data_in The data to be transformed.
54
+ * \param[in,out] data_out A reference to the output data. Note this can point to data in and the method should be mutation safe.
55
+ * \param[in] transform The transform to apply to data_in to fill data_out.
56
+ * \tparam T The type of the data to be transformed.
56
57
*
57
58
* This method needs to be implemented by client library developers
58
59
*/
59
- template <class T >
60
+ template <class T >
60
61
void doTransform (
61
62
const T & data_in, T & data_out, const geometry_msgs::msg::TransformStamped & transform)
62
63
{
@@ -66,7 +67,16 @@ void doTransform(
66
67
data_out = t * data_in;
67
68
}
68
69
69
- template <class T >
70
+ /* * \brief The templated function expected to be able to do a transform.
71
+ *
72
+ * This is the method which tf2 will use to try to apply a transform for any given datatype.
73
+ * Overload for tf2::Stamped\<T\> types, uses doTransform(const T&, T&, const geometry_msgs::msg::TransformStamped&).
74
+ * \param[in] data_in The data to be transformed.
75
+ * \param[in,out] data_out A reference to the output data. Note this can point to data in and the method should be mutation safe.
76
+ * \param[in] transform The transform to apply to data_in to fill data_out.
77
+ * \tparam T The type of the data to be transformed.
78
+ */
79
+ template <class T >
70
80
void doTransform (
71
81
tf2::Stamped<T> const & data_in, tf2::Stamped<T> & data_out,
72
82
const geometry_msgs::msg::TransformStamped & transform)
@@ -80,20 +90,20 @@ void doTransform(
80
90
* \param[in] t The data input.
81
91
* \return The timestamp associated with the data.
82
92
*/
83
- template <class T >
93
+ template <class T >
84
94
inline tf2::TimePoint getTimestamp (const T & t)
85
95
{
86
- return impl::DefaultStampedImpl <T>::getTimestamp (t);
96
+ return impl::StampedAttributesHelper <T>::getTimestamp (t);
87
97
}
88
98
89
99
/* *\brief Get the frame_id from data
90
100
* \param[in] t The data input.
91
101
* \return The frame_id associated with the data.
92
102
*/
93
- template <class T >
103
+ template <class T >
94
104
inline std::string getFrameId (const T & t)
95
105
{
96
- return impl::DefaultStampedImpl <T>::getFrameId (t);
106
+ return impl::StampedAttributesHelper <T>::getFrameId (t);
97
107
}
98
108
99
109
/* *\brief Get the covariance matrix from data
@@ -103,7 +113,7 @@ inline std::string getFrameId(const T & t)
103
113
template <class T >
104
114
std::array<std::array<double , 6 >, 6 > getCovarianceMatrix (const T & t)
105
115
{
106
- using traits = impl::stampedMessageTraits <T>;
116
+ using traits = impl::StampedMessageTraits <T>;
107
117
return covarianceRowMajorToNested (traits::getCovariance (t));
108
118
}
109
119
@@ -124,52 +134,52 @@ std::array<std::array<double, 6>, 6> getCovarianceMatrix(const tf2::WithCovarian
124
134
* \brief Function that converts from one type to a ROS message type.
125
135
*
126
136
* The implementation of this function should be done in the tf2_* packages
127
- * for each datatypes. Preferably in a specialization of the impl::ImplDetails struct.
128
- * \param a an object of whatever type
137
+ * for each datatypes. Preferably in a specialization of the impl::ConversionImplementation struct.
138
+ * \param[in] a an object of whatever type
129
139
* \tparam A Non-message Datatype
130
- * \tparam B ROS message Datatype. The default value will be taken from impl::defaultMessage \<A\>::type.
140
+ * \tparam B ROS message Datatype. The default value will be taken from impl::DefaultMessageForDatatype \<A\>::type.
131
141
* \return the conversion as a ROS message
132
142
*/
133
- template <typename A, typename B>
143
+ template <typename A, typename B>
134
144
inline B toMsg (const A & a)
135
145
{
136
146
B b;
137
- impl::ImplDetails <A, B>::toMsg (a, b);
147
+ impl::ConversionImplementation <A, B>::toMsg (a, b);
138
148
return b;
139
149
}
140
150
141
151
/* *
142
152
* \brief Function that converts from one type to a ROS message type.
143
153
*
144
154
* The implementation of this function should be done in the tf2_* packages
145
- * for each datatypes. Preferably in a specialization of the impl::ImplDetails struct.
146
- * \param a an object of whatever type
147
- * \param b ROS message
155
+ * for each datatypes. Preferably in a specialization of the impl::ConversionImplementation struct.
156
+ * \param[in] a an object of whatever type
157
+ * \param[out] b ROS message
148
158
* \tparam A Non-message Datatype
149
159
* \tparam B Type of the ROS Message
150
160
* \return Reference to the parameter b
151
161
*/
152
- template <typename A, typename B>
162
+ template <typename A, typename B>
153
163
inline B & toMsg (const A & a, B & b)
154
164
{
155
- impl::ImplDetails <A, B>::toMsg (a, b);
165
+ impl::ConversionImplementation <A, B>::toMsg (a, b);
156
166
return b;
157
167
}
158
168
159
169
/* *
160
170
* \brief Function that converts from a ROS message type to another type.
161
171
*
162
172
* The implementation of this function should be done in the tf2_* packages
163
- * for each datatypes. Preferably in a specialization of the impl::ImplDetails struct.
164
- * \param a a ROS message to convert from
165
- * \param b the object to convert to
173
+ * for each datatypes. Preferably in a specialization of the impl::ConversionImplementation struct.
174
+ * \param[in] a a ROS message to convert from
175
+ * \param[out] b the object to convert to
166
176
* \tparam A ROS message type
167
177
* \tparam B Arbitrary type
168
178
*/
169
- template <typename A, typename B>
179
+ template <typename A, typename B>
170
180
inline void fromMsg (const A & a, B & b)
171
181
{
172
- impl::ImplDetails <B, A>::fromMsg (a, b);
182
+ impl::ConversionImplementation <B, A>::fromMsg (a, b);
173
183
}
174
184
175
185
/* *
@@ -178,34 +188,41 @@ inline void fromMsg(const A & a, B & b)
178
188
* Matching toMsg and from Msg conversion functions need to exist.
179
189
* If they don't exist or do not apply (for example, if your two
180
190
* classes are ROS messages), just write a specialization of the function.
181
- * \param a an object to convert from
182
- * \param b the object to convert to
191
+ * \param[in] a an object to convert from
192
+ * \param[in,out] b the object to convert to
183
193
* \tparam A Type of the object to convert from
184
194
* \tparam B Type of the object to convert to
185
195
*/
186
- template <class A , class B >
196
+ template <class A , class B >
187
197
inline void convert (const A & a, B & b)
188
198
{
189
199
impl::Converter<
190
200
rosidl_generator_traits::is_message<A>::value,
191
201
rosidl_generator_traits::is_message<B>::value>::convert (a, b);
192
202
}
193
203
194
- template <class A >
204
+ /* * \brief Overload of tf2::convert() for the same types.
205
+ * \param[in] a1 an object to convert from
206
+ * \param[in,out] a2 the object to convert to
207
+ * \tparam A Type of the object to convert
208
+ */
209
+ template <class A >
195
210
void convert (const A & a1, A & a2)
196
211
{
197
212
if (&a1 != &a2) {
198
213
a2 = a1;
199
214
}
200
215
}
201
216
202
- /* *\brief Function that converts from a row-major representation of a 6x6
217
+ /* * \brief Function that converts from a row-major representation of a 6x6
203
218
* covariance matrix to a nested array representation.
204
- * \param row_major A row-major array of 36 covariance values.
219
+ * \param[in] row_major A row-major array of 36 covariance values.
205
220
* \return A nested array representation of 6x6 covariance values.
206
221
*/
207
222
inline
208
- std::array<std::array<double , 6 >, 6 > covarianceRowMajorToNested (const std::array<double , 36 > & row_major)
223
+ std::array<std::array<double , 6 >, 6 > covarianceRowMajorToNested (
224
+ const std::array<double ,
225
+ 36 > & row_major)
209
226
{
210
227
std::array<std::array<double , 6 >, 6 > nested_array = {};
211
228
size_t l1 = 0 , l2 = 0 ;
@@ -222,13 +239,15 @@ std::array<std::array<double, 6>, 6> covarianceRowMajorToNested(const std::array
222
239
return nested_array;
223
240
}
224
241
225
- /* *\brief Function that converts from a nested array representation of a 6x6
242
+ /* * \brief Function that converts from a nested array representation of a 6x6
226
243
* covariance matrix to a row-major representation.
227
- * \param nested_array A nested array representation of 6x6 covariance values.
244
+ * \param[in] nested_array A nested array representation of 6x6 covariance values.
228
245
* \return A row-major array of 36 covariance values.
229
246
*/
230
247
inline
231
- std::array<double , 36 > covarianceNestedToRowMajor (const std::array<std::array<double , 6 >, 6 > & nested_array)
248
+ std::array<double , 36 > covarianceNestedToRowMajor (
249
+ const std::array<std::array<double , 6 >,
250
+ 6 > & nested_array)
232
251
{
233
252
std::array<double , 36 > row_major = {};
234
253
size_t counter = 0 ;
0 commit comments