Skip to content

Commit

Permalink
more meaningful names
Browse files Browse the repository at this point in the history
  • Loading branch information
gleichdick committed Jan 29, 2021
1 parent 6abb6b5 commit de72a43
Show file tree
Hide file tree
Showing 9 changed files with 616 additions and 302 deletions.
91 changes: 55 additions & 36 deletions tf2/include/tf2/convert.h
Original file line number Diff line number Diff line change
Expand Up @@ -47,16 +47,17 @@

namespace tf2
{
/**\brief The templated function expected to be able to do a transform
/** \brief The templated function expected to be able to do a transform.
*
* This is the method which tf2 will use to try to apply a transform for any given datatype.
* \param data_in[in] The data to be transformed.
* \param data_out[inout] A reference to the output data. Note this can point to data in and the method should be mutation safe.
* \param transform[in] The transform to apply to data_in to fill data_out.
* \param[in] data_in The data to be transformed.
* \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.
* \param[in] transform The transform to apply to data_in to fill data_out.
* \tparam T The type of the data to be transformed.
*
* This method needs to be implemented by client library developers
*/
template <class T>
template<class T>
void doTransform(
const T & data_in, T & data_out, const geometry_msgs::msg::TransformStamped & transform)
{
Expand All @@ -66,7 +67,16 @@ void doTransform(
data_out = t * data_in;
}

template <class T>
/** \brief The templated function expected to be able to do a transform.
*
* This is the method which tf2 will use to try to apply a transform for any given datatype.
* Overload for tf2::Stamped\<T\> types, uses doTransform(const T&, T&, const geometry_msgs::msg::TransformStamped&).
* \param[in] data_in The data to be transformed.
* \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.
* \param[in] transform The transform to apply to data_in to fill data_out.
* \tparam T The type of the data to be transformed.
*/
template<class T>
void doTransform(
tf2::Stamped<T> const & data_in, tf2::Stamped<T> & data_out,
const geometry_msgs::msg::TransformStamped & transform)
Expand All @@ -80,20 +90,20 @@ void doTransform(
* \param[in] t The data input.
* \return The timestamp associated with the data.
*/
template <class T>
template<class T>
inline tf2::TimePoint getTimestamp(const T & t)
{
return impl::DefaultStampedImpl<T>::getTimestamp(t);
return impl::StampedAttributesHelper<T>::getTimestamp(t);
}

/**\brief Get the frame_id from data
* \param[in] t The data input.
* \return The frame_id associated with the data.
*/
template <class T>
template<class T>
inline std::string getFrameId(const T & t)
{
return impl::DefaultStampedImpl<T>::getFrameId(t);
return impl::StampedAttributesHelper<T>::getFrameId(t);
}

/**\brief Get the covariance matrix from data
Expand All @@ -103,7 +113,7 @@ inline std::string getFrameId(const T & t)
template<class T>
std::array<std::array<double, 6>, 6> getCovarianceMatrix(const T & t)
{
using traits = impl::stampedMessageTraits<T>;
using traits = impl::StampedMessageTraits<T>;
return covarianceRowMajorToNested(traits::getCovariance(t));
}

Expand All @@ -124,52 +134,52 @@ std::array<std::array<double, 6>, 6> getCovarianceMatrix(const tf2::WithCovarian
* \brief Function that converts from one type to a ROS message type.
*
* The implementation of this function should be done in the tf2_* packages
* for each datatypes. Preferably in a specialization of the impl::ImplDetails struct.
* \param a an object of whatever type
* for each datatypes. Preferably in a specialization of the impl::ConversionImplementation struct.
* \param[in] a an object of whatever type
* \tparam A Non-message Datatype
* \tparam B ROS message Datatype. The default value will be taken from impl::defaultMessage\<A\>::type.
* \tparam B ROS message Datatype. The default value will be taken from impl::DefaultMessageForDatatype\<A\>::type.
* \return the conversion as a ROS message
*/
template <typename A, typename B>
template<typename A, typename B>
inline B toMsg(const A & a)
{
B b;
impl::ImplDetails<A, B>::toMsg(a, b);
impl::ConversionImplementation<A, B>::toMsg(a, b);
return b;
}

/**
* \brief Function that converts from one type to a ROS message type.
*
* The implementation of this function should be done in the tf2_* packages
* for each datatypes. Preferably in a specialization of the impl::ImplDetails struct.
* \param a an object of whatever type
* \param b ROS message
* for each datatypes. Preferably in a specialization of the impl::ConversionImplementation struct.
* \param[in] a an object of whatever type
* \param[out] b ROS message
* \tparam A Non-message Datatype
* \tparam B Type of the ROS Message
* \return Reference to the parameter b
*/
template <typename A, typename B>
template<typename A, typename B>
inline B & toMsg(const A & a, B & b)
{
impl::ImplDetails<A, B>::toMsg(a, b);
impl::ConversionImplementation<A, B>::toMsg(a, b);
return b;
}

/**
* \brief Function that converts from a ROS message type to another type.
*
* The implementation of this function should be done in the tf2_* packages
* for each datatypes. Preferably in a specialization of the impl::ImplDetails struct.
* \param a a ROS message to convert from
* \param b the object to convert to
* for each datatypes. Preferably in a specialization of the impl::ConversionImplementation struct.
* \param[in] a a ROS message to convert from
* \param[out] b the object to convert to
* \tparam A ROS message type
* \tparam B Arbitrary type
*/
template <typename A, typename B>
template<typename A, typename B>
inline void fromMsg(const A & a, B & b)
{
impl::ImplDetails<B, A>::fromMsg(a, b);
impl::ConversionImplementation<B, A>::fromMsg(a, b);
}

/**
Expand All @@ -178,34 +188,41 @@ inline void fromMsg(const A & a, B & b)
* Matching toMsg and from Msg conversion functions need to exist.
* If they don't exist or do not apply (for example, if your two
* classes are ROS messages), just write a specialization of the function.
* \param a an object to convert from
* \param b the object to convert to
* \param[in] a an object to convert from
* \param[in,out] b the object to convert to
* \tparam A Type of the object to convert from
* \tparam B Type of the object to convert to
*/
template <class A, class B>
template<class A, class B>
inline void convert(const A & a, B & b)
{
impl::Converter<
rosidl_generator_traits::is_message<A>::value,
rosidl_generator_traits::is_message<B>::value>::convert(a, b);
}

template <class A>
/** \brief Overload of tf2::convert() for the same types.
* \param[in] a1 an object to convert from
* \param[in,out] a2 the object to convert to
* \tparam A Type of the object to convert
*/
template<class A>
void convert(const A & a1, A & a2)
{
if (&a1 != &a2) {
a2 = a1;
}
}

/**\brief Function that converts from a row-major representation of a 6x6
/** \brief Function that converts from a row-major representation of a 6x6
* covariance matrix to a nested array representation.
* \param row_major A row-major array of 36 covariance values.
* \param[in] row_major A row-major array of 36 covariance values.
* \return A nested array representation of 6x6 covariance values.
*/
inline
std::array<std::array<double, 6>, 6> covarianceRowMajorToNested(const std::array<double, 36> & row_major)
std::array<std::array<double, 6>, 6> covarianceRowMajorToNested(
const std::array<double,
36> & row_major)
{
std::array<std::array<double, 6>, 6> nested_array = {};
size_t l1 = 0, l2 = 0;
Expand All @@ -222,13 +239,15 @@ std::array<std::array<double, 6>, 6> covarianceRowMajorToNested(const std::array
return nested_array;
}

/**\brief Function that converts from a nested array representation of a 6x6
/** \brief Function that converts from a nested array representation of a 6x6
* covariance matrix to a row-major representation.
* \param nested_array A nested array representation of 6x6 covariance values.
* \param[in] nested_array A nested array representation of 6x6 covariance values.
* \return A row-major array of 36 covariance values.
*/
inline
std::array<double, 36> covarianceNestedToRowMajor(const std::array<std::array<double, 6>, 6> & nested_array)
std::array<double, 36> covarianceNestedToRowMajor(
const std::array<std::array<double, 6>,
6> & nested_array)
{
std::array<double, 36> row_major = {};
size_t counter = 0;
Expand Down
Loading

0 comments on commit de72a43

Please sign in to comment.