diff --git a/tf2/include/tf2/convert.h b/tf2/include/tf2/convert.h index 5c6d55fa7..36efd5be8 100644 --- a/tf2/include/tf2/convert.h +++ b/tf2/include/tf2/convert.h @@ -32,11 +32,76 @@ #ifndef TF2_CONVERT_H #define TF2_CONVERT_H -#include + +#include +#include +#include #include namespace tf2 { +/**\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 The data to be transformed. + * \param data_out A reference to the output data. Note this can point to data in and the method should be mutation safe. + * \param transform The transform to apply to data_in to fill data_out. + * + * This method needs to be implemented by client library developers + */ +template + void doTransform(const T& data_in, T& data_out, const geometry_msgs::TransformStamped& transform); + +/**\brief Get the timestamp from data + * \param t The data input. + * \return The timestamp associated with the data. The lifetime of the returned + * reference is bound to the lifetime of the argument. + */ +template + const ros::Time& getTimestamp(const T& t); + +/**\brief Get the frame_id from data + * \param t The data input. + * \return The frame_id associated with the data. The lifetime of the returned + * reference is bound to the lifetime of the argument. + */ +template + const std::string& getFrameId(const T& t); + + + +/* An implementation for Stamped

datatypes */ +template + const ros::Time& getTimestamp(const tf2::Stamped

& t) + { + return t.stamp_; + } + +/* An implementation for Stamped

datatypes */ +template + const std::string& getFrameId(const tf2::Stamped

& t) + { + return t.frame_id_; + } + +/** Function that converts from one type to a ROS message type. It has to be + * implemented by each data type in tf2_* (except ROS messages) as it is + * used in the "convert" function. + * \param a an object of whatever type + * \return the conversion as a ROS message + */ +template + B toMsg(const A& a); + +/** Function that converts from a ROS message type to another type. It has to be + * implemented by each data type in tf2_* (except ROS messages) as it is used + * in the "convert" function. + * \param a a ROS message to convert from + * \param b the object to convert to + */ +template + void fromMsg(const A&, B& b); + /** Function that converts any type to any type (messages or not). * Matching toMsg and from Msg conversion functions need to exist. * If they don't exist or do not apply (for example, if your two @@ -59,6 +124,7 @@ template a2 = a1; } + } #endif //TF2_CONVERT_H diff --git a/tf2/include/tf2/transform_functions.h b/tf2/include/tf2/transform_functions.h deleted file mode 100644 index 7b49ad37a..000000000 --- a/tf2/include/tf2/transform_functions.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright (c) 2013, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** \author Tully Foote */ - -#ifndef TF2_TRANSFORM_FUNCTIONS_H -#define TF2_TRANSFORM_FUNCTIONS_H - -#include -#include -#include - -namespace tf2 { - -/**\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 The data to be transformed. - * \param data_out A reference to the output data. Note this can point to data in and the method should be mutation safe. - * \param transform The transform to apply to data_in to fill data_out. - * - * This method needs to be implemented by client library developers - */ -template - void doTransform(const T& data_in, T& data_out, const geometry_msgs::TransformStamped& transform); - -/**\brief Get the timestamp from data - * \param t The data input. - * \return The timestamp associated with the data. The lifetime of the returned - * reference is bound to the lifetime of the argument. - */ -template - const ros::Time& getTimestamp(const T& t); - -/**\brief Get the frame_id from data - * \param t The data input. - * \return The frame_id associated with the data. The lifetime of the returned - * reference is bound to the lifetime of the argument. - */ -template - const std::string& getFrameId(const T& t); - -/* An implementation for Stamped

datatypes */ -template - const ros::Time& getTimestamp(const tf2::Stamped

& t) - { - return t.stamp_; - } - -/* An implementation for Stamped

datatypes */ -template - const std::string& getFrameId(const tf2::Stamped

& t) - { - return t.frame_id_; - } - -/** Function that converts from one type to a ROS message type. It has to be - * implemented by each data type in tf2_* (except ROS messages) as it is - * used in the "convert" function. - * \param a an object of whatever type - * \return the conversion as a ROS message - */ -template - B toMsg(const A& a); - -/** Function that converts from a ROS message type to another type. It has to be - * implemented by each data type in tf2_* (except ROS messages) as it is used - * in the "convert" function. - * \param a a ROS message to convert from - * \param b the object to convert to - */ -template - void fromMsg(const A&, B& b); - -} - -#endif //TF2_TRANSFORM_FUNCTIONS_H diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index f2bc67120..00c789ba7 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -29,7 +29,7 @@ #ifndef TF2_EIGEN_H #define TF2_EIGEN_H -#include +#include #include #include #include @@ -520,7 +520,66 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped + +namespace Eigen { +// This is needed to make the usage of the following conversion functions usable in tf2::convert(). +// According to clangs error note 'fromMsg'/'toMsg' should be declared prior to the call site or +// in an associated namespace of one of its arguments. The stamped versions of this conversion +// functions work because they have tf2::Stamped as an argument which is the same namespace as +// which 'fromMsg'/'toMsg' is defined in. The non-stamped versions have no argument which is +// defined in tf2, so it take the following definitions in Eigen namespace to make them usable in +// tf2::convert(). + +inline +geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { + return tf2::toMsg(in); +} + +inline +geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) { + tf2::fromMsg(msg, out); +} + +inline +geometry_msgs::Point toMsg(const Eigen::Vector3d& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { + tf2::fromMsg(msg, out); +} + +inline +void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) { + tf2::fromMsg(msg, out); +} + +inline +geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) { + tf2::fromMsg(msg, out); +} + +inline +geometry_msgs::Twist toMsg(const Eigen::Matrix& in) { + return tf2::toMsg(in); +} + +inline +void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { + tf2::fromMsg(msg, out); +} + +} // namespace #endif // TF2_EIGEN_H