@@ -87,15 +87,15 @@ inline geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Isomet
8787
8888namespace impl
8989{
90- /* * \brief Template for Eigen::Isometry3d and Eigen::Affine3d conversion
90+ /* * \brief Template for Eigen::Isometry3d and Eigen::Affine3d conversion.
9191 * \tparam mode Mode argument for Eigen::Transform template
9292 */
9393template <int mode>
9494struct EigenTransformImpl
9595{
9696 /* * \brief Convert a Transform message to an Eigen type.
97- * \param msg The message to convert
98- * \param out The converted message, as an Eigen type
97+ * \param[in] msg The message to convert.
98+ * \param[out] out The converted message, as an Eigen type.
9999 */
100100 static void fromMsg (
101101 const geometry_msgs::msg::Transform & msg, Eigen::Transform<double , 3 , mode> & out)
@@ -107,9 +107,9 @@ struct EigenTransformImpl
107107 out = Eigen::Translation3d (v) * q;
108108 }
109109
110- /* * \brief Convert an Eigen Transform to a Transform message
111- * \param in The Eigen Transform to convert
112- * \param msg The converted Transform, as a message
110+ /* * \brief Convert an Eigen Transform to a Transform message.
111+ * \param[in] in The Eigen Transform to convert.
112+ * \param[out] msg The converted Transform, as a message.
113113 */
114114 static void toMsg (
115115 const Eigen::Transform<double , 3 , mode> & in, geometry_msgs::msg::Transform & msg)
@@ -119,55 +119,59 @@ struct EigenTransformImpl
119119 }
120120};
121121
122- // / \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Affine3d
122+ // / \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Affine3d.
123123template <>
124124struct ImplDetails <Eigen::Affine3d, geometry_msgs::msg::Transform>
125125 : EigenTransformImpl<Eigen::Affine>
126126{
127127};
128128
129- // / \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Isometry3d
129+ // / \brief Conversion implementation for geometry_msgs::msg::Transform and Eigen::Isometry3d.
130130template <>
131131struct ImplDetails <Eigen::Isometry3d, geometry_msgs::msg::Transform>
132132 : EigenTransformImpl<Eigen::Isometry>
133133{
134134};
135135
136- // / \brief Conversion implementation for geometry_msgs::msg::Point and Eigen::Vector3d
136+ // / \brief Conversion implementation for geometry_msgs::msg::Point and Eigen::Vector3d.
137137template <>
138138struct ImplDetails <Eigen::Vector3d, geometry_msgs::msg::Point>
139139 : DefaultVectorImpl<Eigen::Vector3d, geometry_msgs::msg::Point>
140140{
141141};
142142
143- // / \brief Conversion implementation for geometry_msgs::msg::Vector3d and Eigen::Vector3d
143+ // / \brief Conversion implementation for geometry_msgs::msg::Vector3d and Eigen::Vector3d.
144144template <>
145145struct ImplDetails <Eigen::Vector3d, geometry_msgs::msg::Vector3>
146146 : DefaultVectorImpl<Eigen::Vector3d, geometry_msgs::msg::Vector3>
147147{
148148};
149149
150150
151+ // / \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Vector3d.
151152template <>
152153struct DefaultTransformType <Eigen::Vector3d>
153154{
155+ // / \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Vector3d.
154156 using type = Eigen::Isometry3d;
155157};
156158
159+ // / \brief Default return type of tf2::toMsg() for Eigen::Vector3d.
157160template <>
158161struct defaultMessage <Eigen::Vector3d>
159162{
163+ // / \brief Default return type of tf2::toMsg() for Eigen::Vector3d.
160164 using type = geometry_msgs::msg::Point;
161165};
162166
163- // / \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond
167+ // / \brief Conversion implementation for geometry_msgs::msg::Quaternion and Eigen::Quaterniond.
164168template <>
165169struct ImplDetails <Eigen::Quaterniond, geometry_msgs::msg::Quaternion>
166170{
167171
168172 /* * \brief Convert a Eigen Quaterniond type to a Quaternion message.
169- * \param in The Eigen Quaterniond to convert.
170- * \param msg The quaternion converted to a Quaterion message.
173+ * \param[in] in The Eigen Quaterniond to convert.
174+ * \param[out] msg The quaternion converted to a Quaterion message.
171175 */
172176 static void toMsg (const Eigen::Quaterniond & in, geometry_msgs::msg::Quaternion & msg)
173177 {
@@ -178,18 +182,20 @@ struct ImplDetails<Eigen::Quaterniond, geometry_msgs::msg::Quaternion>
178182 }
179183
180184 /* * \brief Convert a Quaternion message type to a Eigen-specific Quaterniond type.
181- * \param msg The Quaternion message to convert.
182- * \param out The quaternion converted to a Eigen Quaterniond.
185+ * \param[in] msg The Quaternion message to convert.
186+ * \param[out] out The quaternion converted to a Eigen Quaterniond.
183187 */
184188 static void fromMsg (const geometry_msgs::msg::Quaternion & msg, Eigen::Quaterniond & out)
185189 {
186190 out = Eigen::Quaterniond (msg.w , msg.x , msg.y , msg.z );
187191 }
188192};
189193
194+ // / \brief Default return type of tf2::toMsg() for Eigen::Quaterniond.
190195template <>
191196struct defaultMessage <Eigen::Quaterniond>
192197{
198+ // / \brief Default return type of tf2::toMsg() for Eigen::Quaterniond.
193199 using type = geometry_msgs::msg::Quaternion;
194200};
195201} // namespace impl
@@ -217,13 +223,16 @@ void doTransform(
217223
218224namespace impl
219225{
226+
227+ /* * \brief Pose conversion template for Eigen types.
228+ * \tparam T Eigen transform type.
229+ */
220230template <typename T>
221- struct PoseImplDetails
231+ struct EigenPoseImpl
222232{
223- /* * \brief Convert a Eigen Affine3d transform type to a Pose message.
224- * This function is a specialization of the toMsg template defined in tf2/convert.h.
225- * \param in The Eigen Affine3d to convert.
226- * \return The Eigen transform converted to a Pose message.
233+ /* * \brief Convert a Eigen transform type to a Pose message.
234+ * \param[in] in The Eigen Affine3d to convert.
235+ * \param[out] msg The Eigen transform converted to a Pose message.
227236 */
228237 static void toMsg (const T & in, geometry_msgs::msg::Pose & msg)
229238 {
@@ -233,10 +242,9 @@ struct PoseImplDetails
233242 tf2::toMsg<>(q, msg.orientation );
234243 }
235244
236- /* * \brief Convert a Pose message transform type to a Eigen Affine3d.
237- * This function is a specialization of the toMsg template defined in tf2/convert.h.
238- * \param msg The Pose message to convert.
239- * \param out The pose converted to a Eigen Affine3d.
245+ /* * \brief Convert a Pose message transform type to a Eigen transform type.
246+ * \param[in] msg The Pose message to convert.
247+ * \param[out] out The pose converted to a Eigen transform.
240248 */
241249 static void fromMsg (const geometry_msgs::msg::Pose & msg, T & out)
242250 {
@@ -248,37 +256,44 @@ struct PoseImplDetails
248256 }
249257};
250258
259+ // / \brief Conversion implementation for geometry_msgs::msg::Pose and Eigen::Affine3d.
251260template <>
252261struct ImplDetails <Eigen::Affine3d, geometry_msgs::msg::Pose>
253- : public PoseImplDetails <Eigen::Affine3d>
262+ : public EigenPoseImpl <Eigen::Affine3d>
254263{
255264};
256265
266+ // / \brief Conversion implementation for geometry_msgs::msg::Pose and Eigen::Isometry3d.
257267template <>
258268struct ImplDetails <Eigen::Isometry3d, geometry_msgs::msg::Pose>
259- : public PoseImplDetails <Eigen::Isometry3d>
269+ : public EigenPoseImpl <Eigen::Isometry3d>
260270{
261271};
262272
273+ // / \brief Default return type of tf2::toMsg() for Eigen::Affine3d.
263274template <>
264275struct defaultMessage <Eigen::Affine3d>
265276{
277+ // / \brief Default return type of tf2::toMsg() for Eigen::Affine3d.
266278 using type = geometry_msgs::msg::Pose;
267279};
268280
281+ // / \brief Default return type of tf2::toMsg() for Eigen::Affine3d.
269282template <>
270283struct defaultMessage <Eigen::Isometry3d>
271284{
285+ // / \brief Default return type of tf2::toMsg() for Eigen::Affine3d.
272286 using type = geometry_msgs::msg::Pose;
273287};
274- /* * \brief Convert a Eigen 6x1 Matrix type to a Twist message.
275- * This function is a specialization of the toMsg template defined in tf2/convert.h.
276- * \param in The 6x1 Eigen Matrix to convert.
277- * \return The Eigen Matrix converted to a Twist message.
278- */
288+
289+ // / \brief Conversion implementation for geometry_msgs::msg::Twist and Eigen::Matrix<double, 6, 1>.
279290template <>
280291struct ImplDetails <Eigen::Matrix<double , 6 , 1 >, geometry_msgs::msg::Twist>
281292{
293+ /* * \brief Convert a Eigen 6x1 Matrix type to a Twist message.
294+ * \param[in] in The 6x1 Eigen Matrix to convert.
295+ * \param[out] msg The Eigen Matrix converted to a Twist message.
296+ */
282297 static void toMsg (const Eigen::Matrix<double , 6 , 1 > & in, geometry_msgs::msg::Twist & msg)
283298 {
284299 msg.linear .x = in[0 ];
@@ -290,9 +305,8 @@ struct ImplDetails<Eigen::Matrix<double, 6, 1>, geometry_msgs::msg::Twist>
290305 }
291306
292307 /* * \brief Convert a Twist message transform type to a Eigen 6x1 Matrix.
293- * This function is a specialization of the toMsg template defined in tf2/convert.h.
294- * \param msg The Twist message to convert.
295- * \param out The twist converted to a Eigen 6x1 Matrix.
308+ * \param[in] msg The Twist message to convert.
309+ * \param[out] out The twist converted to a Eigen 6x1 Matrix.
296310 */
297311 static void fromMsg (const geometry_msgs::msg::Twist & msg, Eigen::Matrix<double , 6 , 1 > & out)
298312 {
@@ -305,15 +319,22 @@ struct ImplDetails<Eigen::Matrix<double, 6, 1>, geometry_msgs::msg::Twist>
305319 }
306320};
307321
322+ // / \brief Default return type of tf2::toMsg() for Eigen::Matrix<double, 6, 1>.
308323template <>
309324struct defaultMessage <Eigen::Matrix<double , 6 , 1 >>
310325{
326+ // / \brief Default return type of tf2::toMsg() for Eigen::Matrix<double, 6, 1>.
311327 using type = geometry_msgs::msg::Twist;
312328};
313329
330+ // / \brief Conversion implementation for geometry_msgs::msg::Wrench and Eigen::Matrix<double, 6, 1>.
314331template <>
315332struct ImplDetails <Eigen::Matrix<double , 6 , 1 >, geometry_msgs::msg::Wrench>
316333{
334+ /* * \brief Convert a Eigen 6x1 Matrix type to a Wrench message.
335+ * \param[in] in The 6x1 Eigen Matrix to convert.
336+ * \param[out] msg The Eigen Matrix converted to a Wrench message.
337+ */
317338 static void toMsg (const Eigen::Matrix<double , 6 , 1 > & in, geometry_msgs::msg::Wrench & msg)
318339 {
319340 msg.force .x = in[0 ];
@@ -324,9 +345,8 @@ struct ImplDetails<Eigen::Matrix<double, 6, 1>, geometry_msgs::msg::Wrench>
324345 msg.torque .z = in[5 ];
325346 }
326347
327- /* * \brief Convert a Twist message transform type to a Eigen 6x1 Matrix.
328- * This function is a specialization of the toMsg template defined in tf2/convert.h.
329- * \param msg The Twist message to convert.
348+ /* * \brief Convert a Wrench message transform type to a Eigen 6x1 Matrix.
349+ * \param msg The Wrench message to convert.
330350 * \param out The twist converted to a Eigen 6x1 Matrix.
331351 */
332352 static void fromMsg (const geometry_msgs::msg::Wrench & msg, Eigen::Matrix<double , 6 , 1 > & out)
@@ -340,16 +360,20 @@ struct ImplDetails<Eigen::Matrix<double, 6, 1>, geometry_msgs::msg::Wrench>
340360 }
341361};
342362
363+ // / \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Affine3d.
343364template <>
344365struct DefaultTransformType <Eigen::Affine3d>
345366{
367+ // / \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Affine3d.
346368 using type = Eigen::Isometry3d;
347369};
348370
349371
372+ // / \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Isometry3d.
350373template <>
351374struct DefaultTransformType <Eigen::Isometry3d>
352375{
376+ // / \brief Default Type for automatic tf2::doTransform() implementation for Eigen::Isometry3d.
353377 using type = Eigen::Isometry3d;
354378};
355379
0 commit comments