4747
4848namespace tf2
4949{
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.
5151 *
5252 * 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.
5657 *
5758 * This method needs to be implemented by client library developers
5859 */
59- template <class T >
60+ template <class T >
6061void doTransform (
6162 const T & data_in, T & data_out, const geometry_msgs::msg::TransformStamped & transform)
6263{
@@ -66,7 +67,16 @@ void doTransform(
6667 data_out = t * data_in;
6768}
6869
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 >
7080void doTransform (
7181 tf2::Stamped<T> const & data_in, tf2::Stamped<T> & data_out,
7282 const geometry_msgs::msg::TransformStamped & transform)
@@ -80,20 +90,20 @@ void doTransform(
8090 * \param[in] t The data input.
8191 * \return The timestamp associated with the data.
8292 */
83- template <class T >
93+ template <class T >
8494inline tf2::TimePoint getTimestamp (const T & t)
8595{
86- return impl::DefaultStampedImpl <T>::getTimestamp (t);
96+ return impl::StampedAttributesHelper <T>::getTimestamp (t);
8797}
8898
8999/* *\brief Get the frame_id from data
90100 * \param[in] t The data input.
91101 * \return The frame_id associated with the data.
92102 */
93- template <class T >
103+ template <class T >
94104inline std::string getFrameId (const T & t)
95105{
96- return impl::DefaultStampedImpl <T>::getFrameId (t);
106+ return impl::StampedAttributesHelper <T>::getFrameId (t);
97107}
98108
99109/* *\brief Get the covariance matrix from data
@@ -103,7 +113,7 @@ inline std::string getFrameId(const T & t)
103113template <class T >
104114std::array<std::array<double , 6 >, 6 > getCovarianceMatrix (const T & t)
105115{
106- using traits = impl::stampedMessageTraits <T>;
116+ using traits = impl::StampedMessageTraits <T>;
107117 return covarianceRowMajorToNested (traits::getCovariance (t));
108118}
109119
@@ -124,52 +134,52 @@ std::array<std::array<double, 6>, 6> getCovarianceMatrix(const tf2::WithCovarian
124134 * \brief Function that converts from one type to a ROS message type.
125135 *
126136 * 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
129139 * \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.
131141 * \return the conversion as a ROS message
132142 */
133- template <typename A, typename B>
143+ template <typename A, typename B>
134144inline B toMsg (const A & a)
135145{
136146 B b;
137- impl::ImplDetails <A, B>::toMsg (a, b);
147+ impl::ConversionImplementation <A, B>::toMsg (a, b);
138148 return b;
139149}
140150
141151/* *
142152 * \brief Function that converts from one type to a ROS message type.
143153 *
144154 * 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
148158 * \tparam A Non-message Datatype
149159 * \tparam B Type of the ROS Message
150160 * \return Reference to the parameter b
151161 */
152- template <typename A, typename B>
162+ template <typename A, typename B>
153163inline B & toMsg (const A & a, B & b)
154164{
155- impl::ImplDetails <A, B>::toMsg (a, b);
165+ impl::ConversionImplementation <A, B>::toMsg (a, b);
156166 return b;
157167}
158168
159169/* *
160170 * \brief Function that converts from a ROS message type to another type.
161171 *
162172 * 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
166176 * \tparam A ROS message type
167177 * \tparam B Arbitrary type
168178 */
169- template <typename A, typename B>
179+ template <typename A, typename B>
170180inline void fromMsg (const A & a, B & b)
171181{
172- impl::ImplDetails <B, A>::fromMsg (a, b);
182+ impl::ConversionImplementation <B, A>::fromMsg (a, b);
173183}
174184
175185/* *
@@ -178,34 +188,41 @@ inline void fromMsg(const A & a, B & b)
178188 * Matching toMsg and from Msg conversion functions need to exist.
179189 * If they don't exist or do not apply (for example, if your two
180190 * 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
183193 * \tparam A Type of the object to convert from
184194 * \tparam B Type of the object to convert to
185195 */
186- template <class A , class B >
196+ template <class A , class B >
187197inline void convert (const A & a, B & b)
188198{
189199 impl::Converter<
190200 rosidl_generator_traits::is_message<A>::value,
191201 rosidl_generator_traits::is_message<B>::value>::convert (a, b);
192202}
193203
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 >
195210void convert (const A & a1, A & a2)
196211{
197212 if (&a1 != &a2) {
198213 a2 = a1;
199214 }
200215}
201216
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
203218 * 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.
205220 * \return A nested array representation of 6x6 covariance values.
206221 */
207222inline
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)
209226{
210227 std::array<std::array<double , 6 >, 6 > nested_array = {};
211228 size_t l1 = 0 , l2 = 0 ;
@@ -222,13 +239,15 @@ std::array<std::array<double, 6>, 6> covarianceRowMajorToNested(const std::array
222239 return nested_array;
223240}
224241
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
226243 * 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.
228245 * \return A row-major array of 36 covariance values.
229246 */
230247inline
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)
232251{
233252 std::array<double , 36 > row_major = {};
234253 size_t counter = 0 ;
0 commit comments