diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index 3329f1a08..a2543ca9e 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3) project(test_tf2) -find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rostest tf tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs) +find_package(catkin REQUIRED COMPONENTS rosconsole roscpp rostest tf tf2 tf2_bullet tf2_ros tf2_geometry_msgs tf2_kdl tf2_msgs tf2_eigen) find_package(Boost REQUIRED COMPONENTS thread) find_package(orocos_kdl REQUIRED) diff --git a/test_tf2/package.xml b/test_tf2/package.xml index 1f258125f..4169f854d 100644 --- a/test_tf2/package.xml +++ b/test_tf2/package.xml @@ -23,6 +23,7 @@ tf2_geometry_msgs tf2_kdl tf2_msgs + tf2_eigen rosconsole roscpp @@ -34,6 +35,7 @@ tf2_geometry_msgs tf2_kdl tf2_msgs + tf2_eigen rosunit rosbash diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp index d5fb2629e..fca6b0e27 100644 --- a/test_tf2/test/test_convert.cpp +++ b/test_tf2/test/test_convert.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include TEST(tf2Convert, kdlToBullet) @@ -95,7 +96,88 @@ TEST(tf2Convert, kdlBulletROSConversions) EXPECT_NEAR(b1.x(), b4.x(), epsilon); EXPECT_NEAR(b1.y(), b4.y(), epsilon); EXPECT_NEAR(b1.z(), b4.z(), epsilon); -} +} + +TEST(tf2Convert, PoseStampedConversions) { + double epsilon = 1e-9; + + const tf2::Stamped p_tf2_1(tf2::Transform(tf2::Quaternion(1.0,0.0,0.0,2.0), tf2::Vector3(1,2,3)), ros::Time(), "my_frame"); + tf2::Stamped p_tf2_2; + geometry_msgs::PoseStamped msg; + tf2::toMsg(p_tf2_1, msg); + + tf2::Stamped p_e_iso; + tf2::Stamped p_e_aff; + tf2::Stamped p_f; + + tf2::convert(p_tf2_1, p_e_iso); + tf2::convert(p_e_iso, p_f); + tf2::convert(p_f, p_e_aff); + tf2::convert(p_e_aff, p_tf2_2); + + tf2::toMsg(p_e_aff, msg); + + EXPECT_EQ(p_tf2_1.frame_id_, p_tf2_2.frame_id_); + EXPECT_NEAR(p_tf2_1.stamp_.toSec(), p_tf2_2.stamp_.toSec(), epsilon); + + const auto& q1(p_tf2_1.getRotation()), q2(p_tf2_2.getRotation()); + EXPECT_NEAR(q1.x(), q2.x(), epsilon); + EXPECT_NEAR(q1.y(), q2.y(), epsilon); + EXPECT_NEAR(q1.z(), q2.z(), epsilon); + EXPECT_NEAR(q1.w(), q2.w(), epsilon); + + const auto& o1(p_tf2_1.getOrigin()), o2(p_tf2_2.getOrigin()); + EXPECT_NEAR(o1.x(), o2.x(), epsilon); + EXPECT_NEAR(o1.y(), o2.y(), epsilon); + EXPECT_NEAR(o1.z(), o2.z(), epsilon); +} + +TEST(tf2Convert, QuaternionStampedConversations) +{ + const double epsilon = 1e-9; + const tf2::Stamped q_e_1(Eigen::Quaterniond(2.0, 4.0, 0.25, -1), + ros::Time(), "my_frame"); + tf2::Stamped q_tf_1; + tf2::convert(q_e_1, q_tf_1); + + EXPECT_EQ(q_e_1.frame_id_, q_tf_1.frame_id_); + EXPECT_NEAR(q_e_1.stamp_.toSec(), q_tf_1.stamp_.toSec(), epsilon); + EXPECT_NEAR(q_e_1.x(), q_tf_1.x(), epsilon); + EXPECT_NEAR(q_e_1.y(), q_tf_1.y(), epsilon); + EXPECT_NEAR(q_e_1.z(), q_tf_1.z(), epsilon); + EXPECT_NEAR(q_e_1.w(), q_tf_1.w(), epsilon); + + tf2::Stamped q_e_2; + tf2::convert(q_tf_1, q_e_2); + + EXPECT_EQ(q_e_2.frame_id_, q_tf_1.frame_id_); + EXPECT_NEAR(q_e_2.stamp_.toSec(), q_tf_1.stamp_.toSec(), epsilon); + EXPECT_NEAR(q_e_2.x(), q_tf_1.x(), epsilon); + EXPECT_NEAR(q_e_2.y(), q_tf_1.y(), epsilon); + EXPECT_NEAR(q_e_2.z(), q_tf_1.z(), epsilon); + EXPECT_NEAR(q_e_2.w(), q_tf_1.w(), epsilon); +} + +TEST(tf2Convert, QuaternionConversations) +{ + const double epsilon = 1e-9; + const Eigen::Quaterniond q_e_1(2.0, 4.0, 0.25, -1); + tf2::Quaternion q_tf_1; + tf2::convert(q_e_1, q_tf_1); + + EXPECT_NEAR(q_e_1.x(), q_tf_1.x(), epsilon); + EXPECT_NEAR(q_e_1.y(), q_tf_1.y(), epsilon); + EXPECT_NEAR(q_e_1.z(), q_tf_1.z(), epsilon); + EXPECT_NEAR(q_e_1.w(), q_tf_1.w(), epsilon); + + Eigen::Quaterniond q_e_2; + tf2::convert(q_tf_1, q_e_2); + + EXPECT_NEAR(q_e_2.x(), q_tf_1.x(), epsilon); + EXPECT_NEAR(q_e_2.y(), q_tf_1.y(), epsilon); + EXPECT_NEAR(q_e_2.z(), q_tf_1.z(), epsilon); + EXPECT_NEAR(q_e_2.w(), q_tf_1.w(), epsilon); +} int main(int argc, char** argv) { diff --git a/tf2/CMakeLists.txt b/tf2/CMakeLists.txt index 7a6da342e..83d1886ec 100644 --- a/tf2/CMakeLists.txt +++ b/tf2/CMakeLists.txt @@ -5,13 +5,28 @@ find_package(console_bridge REQUIRED) find_package(catkin REQUIRED COMPONENTS geometry_msgs rostime tf2_msgs) find_package(Boost REQUIRED COMPONENTS system thread) +# generate header cross_convert.h + +find_package(PythonInterp REQUIRED) + +set(TF2_CROSS_CONVERT_DIR "${CMAKE_CURRENT_BINARY_DIR}/include") +file(MAKE_DIRECTORY ${TF2_CROSS_CONVERT_DIR}) +set(TF2_CROSS_CONVERT_HDR "${TF2_CROSS_CONVERT_DIR}/cross_convert.h") +set(TF2_CROSS_CONVERT_SCRIPT "${CMAKE_CURRENT_SOURCE_DIR}/scripts/generate_cross_convert_h.py") +add_custom_command(OUTPUT ${TF2_CROSS_CONVERT_HDR} + COMMAND ${PYTHON_EXECUTABLE} ARGS ${TF2_CROSS_CONVERT_SCRIPT} ${TF2_CROSS_CONVERT_HDR} + DEPENDS ${TF2_CROSS_CONVERT_SCRIPT} +) +add_custom_target(build_header ALL DEPENDS ${TF2_CROSS_CONVERT_HDR}) + catkin_package( - INCLUDE_DIRS include + INCLUDE_DIRS include ${TF2_CROSS_CONVERT_DIR} LIBRARIES tf2 DEPENDS console_bridge - CATKIN_DEPENDS geometry_msgs tf2_msgs rostime) + CATKIN_DEPENDS geometry_msgs tf2_msgs rostime geometry_msgs) -include_directories(include ${catkin_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS}) + +include_directories(include ${TF2_CROSS_CONVERT_DIR} ${catkin_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS}) # export user definitions @@ -30,6 +45,9 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) +# generated header +install(FILES ${TF2_CROSS_CONVERT_HDR} DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}/impl/) + # Tests if(CATKIN_ENABLE_TESTING) @@ -45,6 +63,9 @@ catkin_add_gtest(test_simple test/simple_tf2_core.cpp) target_link_libraries(test_simple tf2 ${console_bridge_LIBRARIES}) add_dependencies(test_simple ${catkin_EXPORTED_TARGETS}) +catkin_add_gtest(test_three_way_convert test/three_way_convert.cpp) +add_dependencies(test_three_way_convert ${catkin_EXPORTED_TARGETS}) + add_executable(speed_test EXCLUDE_FROM_ALL test/speed_test.cpp) target_link_libraries(speed_test tf2 ${console_bridge_LIBRARIES}) add_dependencies(tests speed_test) diff --git a/tf2/include/tf2/impl/convert.h b/tf2/include/tf2/impl/convert.h index 6ccca3128..615b8c0b4 100644 --- a/tf2/include/tf2/impl/convert.h +++ b/tf2/include/tf2/impl/convert.h @@ -30,9 +30,93 @@ #ifndef TF2_IMPL_CONVERT_H #define TF2_IMPL_CONVERT_H +#include "cross_convert.h" +#include + namespace tf2 { namespace impl { +//checks whether (A and B) and (B and A) have no common type +template + struct has_no_common_msgs : public std::integral_constant::type, std::nullptr_t>::value && + std::is_same::type, std::nullptr_t>::value && + std::is_same::type, std::nullptr_t>::value> {}; + +// implementation details for sfinae +namespace traits { + +template +struct get_common_bidirectional_type{}; + +template +struct get_common_bidirectional_type {}; + +template +struct get_common_bidirectional_type { + using type = typename tf2::BidirectionalTypeMap::type; +}; + +template +struct get_common_unidirectional_type{}; + +template +struct get_common_unidirectional_type {}; + +template +struct get_common_unidirectional_type { + using type = typename tf2::UnidirectionalTypeMap::type; +}; +} // namespace traits + +// get bidirectional common geometry_msgs type for (A and B) +// Note: check also in reverse order! (get_common_bidirectional_type) +template +struct get_common_bidirectional_type : public traits::get_common_bidirectional_type::type, std::nullptr_t>::value> {}; + +// get unidirectional common geometry_msgs type for (A and B) +template +struct get_common_unidirectional_type : public traits::get_common_unidirectional_type::type, std::nullptr_t>::value> {}; + +// do three-way convert, look up common type for (A and B) +template +inline void convertViaMessage(const A& a, B& b,typename get_common_bidirectional_type::type *c_ptr = nullptr) +{ + // SFINAE will bring the geometry_msgs type as third parameter, extract it + typename std::remove_pointer::type c; + fromMsg(toMsg(a, c), b); +} + +// do three-way convert, look up common type for (B and A) +template +inline void convertViaMessage(const A& a, B& b,typename get_common_bidirectional_type::type *c_ptr = nullptr) +{ + typename std::remove_pointer::type c; + fromMsg(toMsg(a, c), b); +} + +// do three-way convert, look up common type for (A and B), unidirectional +template +inline void convertViaMessage(const A& a, B& b,typename get_common_unidirectional_type::type *c_ptr = nullptr) +{ + // SFINAE will bring the geometry_msgs type as third parameter, extract it + typename std::remove_pointer::type c; + fromMsg(toMsg(a, c), b); +} + +struct common_type_lookup_failed : std::false_type {}; + +// Print a nice message if no common type was defined +// use custom return type to make the selection of this overload testable via decltype +template +common_type_lookup_failed convertViaMessage(const A&, B&, typename std::enable_if::value, void*>::type = nullptr) +{ + static_assert(! has_no_common_msgs::value, + "Please add a tf2::BidirectionalTypeMap or tf2::UnidirectionalTypeMap specialisation for types A and B."); + return common_type_lookup_failed(); +} + + template class Converter { public: @@ -62,17 +146,17 @@ template <> template inline void Converter::convert(const A& a, B& b) { - b = toMsg(a); + toMsg(a, b); } template <> template inline void Converter::convert(const A& a, B& b) { - fromMsg(toMsg(a), b); + convertViaMessage(a,b); } -} -} +} // namespace impl +} // namespace tf2 #endif //TF2_IMPL_CONVERT_H diff --git a/tf2/include/tf2/impl/utils.h b/tf2/include/tf2/impl/utils.h index 5b545e54b..9971bc513 100644 --- a/tf2/include/tf2/impl/utils.h +++ b/tf2/include/tf2/impl/utils.h @@ -59,7 +59,8 @@ tf2::Quaternion toQuaternion(const geometry_msgs::QuaternionStamped& q) { */ template tf2::Quaternion toQuaternion(const tf2::Stamped& t) { - geometry_msgs::QuaternionStamped q = toMsg(t); + geometry_msgs::QuaternionStamped q; + toMsg(t, q); return toQuaternion(q); } @@ -70,7 +71,8 @@ template */ template tf2::Quaternion toQuaternion(const T& t) { - geometry_msgs::Quaternion q = toMsg(t); + geometry_msgs::Quaternion q; + toMsg(t, q); return toQuaternion(q); } diff --git a/tf2/include/tf2/transform_functions.h b/tf2/include/tf2/transform_functions.h index 7b49ad37a..84fa45fc1 100644 --- a/tf2/include/tf2/transform_functions.h +++ b/tf2/include/tf2/transform_functions.h @@ -84,10 +84,11 @@ template * 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 + * \param b the ROS message to convert to + * \return Reference to parameter b */ template - B toMsg(const A& a); + B& toMsg(const A& a, B& b); /** 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 @@ -98,6 +99,49 @@ template template void fromMsg(const A&, B& b); +/** Helper struct which holds a common bidirectional geometry_msgs type which can act as + * proxy to convert two non-message objects using tf2::convert(). + * Say, you want to convert an \c Eigen::Vector3d to \c tf2::Vector3. + * The methods template<> void fromMsg(const geometry_msgs::Vector3&, tf2::Vector3&) + * and template<> void geometry_msgs::Vector3& + * toMsg(const Eigen::Vector3d&, geometry_msgs::Vector3 &) + * are implemented, also with \c Eigen::Vector3d and \c tf2::Vector3 swapped. + * So we specialise \c BidirectionalTypeMap: + * \code + * template<> + * struct BidirectionalTypeMap { + * using type = geometry_msgs::Vector3; + * }; + * \endcode + * Now we can use tf2::convert() to convert back and forth. + */ +template + struct BidirectionalTypeMap{ + using type = std::nullptr_t; + }; + +/** Helper struct which holds a common unidirectional geometry_msgs type which can act as + * proxy to convert two non-message objects using tf2::convert(). + * Say, you want to convert an \c Eigen::Vector3d to \c tf2::Vector3. + * The methods template<> void fromMsg(const geometry_msgs::Vector3&, tf2::Vector3&) + * and template<> void geometry_msgs::Vector3& + * toMsg(const Eigen::Vector3d&, geometry_msgs::Vector3 &) + * are implemented, but the methods the other way around are missing. + * So we specialise \c UnidirectionalTypeMap: + * \code + * template<> + * struct UnidirectionalTypeMap { + * using type = geometry_msgs::Vector3; + * }; + * \endcode + * Now we can use tf2::convert() to convert an \c Eigen::Vector3d to \c tf2::Vector3 + * bot not to convert a \c tf2::Vector3 to \c Eigen::Vector3d. + */ +template + struct UnidirectionalTypeMap{ + using type = std::nullptr_t; + }; + } #endif //TF2_TRANSFORM_FUNCTIONS_H diff --git a/tf2/include/tf2/utils.h b/tf2/include/tf2/utils.h index 54805cc67..3fcfd33d2 100644 --- a/tf2/include/tf2/utils.h +++ b/tf2/include/tf2/utils.h @@ -18,6 +18,7 @@ #include #include #include +#include namespace tf2 { /** Return the yaw, pitch, roll of anything that can be converted to a tf2::Quaternion diff --git a/tf2/scripts/generate_cross_convert_h.py b/tf2/scripts/generate_cross_convert_h.py new file mode 100644 index 000000000..fce68cca0 --- /dev/null +++ b/tf2/scripts/generate_cross_convert_h.py @@ -0,0 +1,410 @@ +# Copyright (c) 2020, 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 Open Source Robotics Foundation 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. + + +# make string literals unicode default in Python 2 +from __future__ import unicode_literals +from sys import argv + + +class Namespace(object): + """ + This class represents a C++ namespace. + If the name is empty, the namespace block is skipped + (e.g. for the global namespace) + """ + def __init__(self, name): + self._name = name + self._members = [] + + def write(self, out_file): + """Write namespace including all members into a file""" + if self._name == "": + # global namespace + self._write_members(out_file) + out_file.write("\n\n") + else: + out_file.write("namespace " + self._name + " {\n") + self._write_members(out_file) + out_file.write("}\n\n") + + def _write_members(self, out_file): + for i in self._members: + i.write(out_file) + + def add_member(self, a_member): + """Add a namespace member""" + self._members.append(a_member) + return self + + def write_name(self, out_file): + """Write the namespace name and two colons into a file""" + out_file.write(self._name + "::") + + def write_includes(self, out_file): + """Write all header includes of the members into a file (sorted)""" + includes = set() + for i in self._members: + includes = i.add_include_header_to_set(includes) + out_file.write("\n".join(sorted(includes))) + out_file.write("\n\n") + + def _load_cxx_class(self, varname, *args, **kwargs): + """ + Load a C++ class and store it in self. + The name passed to the C++ Class is set to varname + but can be overwritten with a name kwarg. + All other arguments are passed on. + """ + self.__setattr__(varname, CxxClass(kwargs.pop("name", varname), self, *args, **kwargs)) + + +class Template(object): + """This class represents a C++ template""" + def __init__(self, parameters): + """ + construct a new C++ template wrapper + parameters: list of tuples with (type, name) of each parameter + """ + self._parameters = parameters + + def write_declaration(self, out_file): + """ + Write the template declaration (including names and types, + like `template `) into a file + """ + params = ["%s %s" % i for i in self._parameters] + if len(params) == 0: + return + out_file.write("template<" + ", ".join(params) + ">\n") + + def write_params_for_specialisation(self, out_file, param_range=slice(None)): + """ + Write the names of the parameters, like `` into a file. + Used for defining or using template specializations. + out_file: File object to write into + param_range: range for names to pick, default all + """ + params = [i[1] for i in self._parameters[param_range]] + if len(params) == 0: + return + out_file.write("<" + ",".join(params) + ">") + + def get_types(self): + """Returns a tuple with all C++ template parameter types""" + return tuple(i[0] for i in self._parameters) + + +class CxxClassBase(object): + """Base class for C++ classes""" + def __init__(self, name, namespace=None, stamped=False): + self._name = name + if namespace is None: + self._namespace = None + else: + self._namespace = namespace.add_member(self) + self._stamped = stamped + + def add_include_header_to_set(self, includes): + """Add an `#include` statement to a set()""" + return includes + + def stamped(self): + """ + Returns True when only conversions to/from stamped message are available, + False when only conversions to/from unstamped are available and "both" + when both stamped and unstamped conversions are available. + """ + return self._stamped + + def compare_stamped(self, other): + """ + Compare the availability of (un)stamped conversions of two classes. + It returns either "both", True, False or None + """ + if self.stamped() == other.stamped() == "both": + # both stamped and unstamped + return "both" + elif self.stamped() and other.stamped(): + # (stamped only, both) or (stamped only, stamped only) + return True + elif not (self.stamped() or other.stamped()): + # (unstamped only, unstamped only) + return False + else: + # (unstamped only, stamped only) -> no match + return None + + +class CxxClass(CxxClassBase): + """This class represents a forward declaration of a C++ class""" + def __init__(self, name, namespace=None, template=None, stamped=False): + super(CxxClass, self).__init__(name, namespace, stamped) + self._template = template + + def write(self, out_file): + """Write forward declaration into a file (without namespace prefix)""" + if self._template is not None: + self._template.write_declaration(out_file) + out_file.write("class " + self._name + ";\n") + + def get_template_types(self): + """Return a tuple with the template parameter C++ types""" + if self._template is None: + return tuple() + else: + return self._template.get_types() + + def write_name(self, out_file): + """write the name into a file (with namespace prefix but without template parameters)""" + if self._namespace is None: + out_file.write("::") + else: + self._namespace.write_name(out_file) + out_file.write(self._name) + + +class TypeMap(CxxClassBase): + """ + This class represents a mapping between two C++ classes and a geometry_msgs type + which can be converted into each other + """ + def __init__(self, msgs_name, class_a, class_b, namespace, stamped=False): + """ + Creates a new mapping between the C++ classes class_a and class_b + with the message type msgs_name. + If stamped is true, "Stamped" is appended to msgs_name. + """ + if not isinstance(stamped, bool): + raise ValueError("stamped must be eighter True or False") + super(TypeMap, self).__init__("BidirectionalTypeMap", namespace, stamped) + self._msgs_name = msgs_name + if stamped: + self._msgs_name += "Stamped" + self._classes = (class_a, class_b) + # build template arguments if necessary by looking at both classes + self._templates_count = [0, 0] + template_types = [] + for i, c in enumerate(self._classes): + needed_types = list(c.get_template_types()) + template_types += needed_types + self._templates_count[i] = len(needed_types) + if len(template_types) == 0: + self._template = None + else: + # build template with names T1, T2 etc. + self._template = Template([(t, "T%d" % i) for i, t in enumerate(template_types)]) + + def write(self, out_file): + """Write the mapping into a file""" + if self._template: + self._template.write_declaration(out_file) + else: + out_file.write("template<>\n") + out_file.write("struct %s<" % self._name) + + self._write_class_name(out_file, 0) + out_file.write(", ") + self._write_class_name(out_file, 1) + + out_file.write("> {\nusing type = ::geometry_msgs::" + self._msgs_name + ";\n};\n\n") + + def _write_class_name(self, out_file, idx): + """ + Write the full name including template parameters + of the first (second) mapped class into a file. + idx: {0 = first, 1 = second} mapped class + """ + if self.stamped(): + out_file.write("::tf2::Stamped<") + self._classes[idx].write_name(out_file) + if self._template: + if idx == 0: + s = slice(0, self._templates_count[0]) + else: + s = slice(self._templates_count[0], None) + self._template.write_params_for_specialisation(out_file, s) + if self.stamped(): + out_file.write(">") + + def add_include_header_to_set(self, includes): + includes.add("#include " % self._msgs_name) + return includes + + +def TypeMapIterator(msgs_name, namespace, classes): + """ + Iterate pairwise over classes and create TypeMap for each pair. + For example, for classes=[a, b, c, d] it yields [ Typemap(a,b), Typemap(a,c), + Typemap(a,d), Typemap(b,c), Typemap(b,d), Typemap(c,d)]. + The stamped attribute of each class is honored. + """ + for i in range(0, len(classes)): + for next_class in classes[i+1:]: + is_stamped = next_class.compare_stamped(classes[i]) + if is_stamped is None: + continue + if is_stamped: # True or "both" + yield TypeMap(msgs_name, classes[i], next_class, namespace, True) + if (not is_stamped) or (is_stamped == "both"): # False or "both" + yield TypeMap(msgs_name, classes[i], next_class, namespace, False) + + +# Known classes from other libraries # + +class Bullet(Namespace): + def __init__(self): + super(Bullet, self).__init__("") + + self._load_cxx_class("Vector3", name="btVector3", stamped=True) + + +class Eigen(Namespace): + def __init__(self): + super(Eigen, self).__init__("Eigen") + + self._load_cxx_class("Vector3", + Template([("typename", "T"), ("int", "_rows"), ("int", "_cols"), + ("int", "_options"), ("int", "_maxrows"), + ("int", "maxcols")]), + stamped="both", name="Matrix") + self._load_cxx_class("Quaternion", + Template([("typename", "T"), ("int", "_options")]), + stamped="both") + self._load_cxx_class("Transform", + Template([("typename", "T"), ("int", "_dim"), + ("int", "_mode"), ("int", "_options")]), + stamped="both") + + +class KDL(Namespace): + def __init__(self): + super(KDL, self).__init__("KDL") + + self._load_cxx_class("Frame", stamped="both") + self._load_cxx_class("Vector3", name="Vector", stamped=True) + self.Twist = CxxClass("Twist", self, stamped=True) + self.Wrench = CxxClass("Wrench", self, stamped=True) + + +class TF2_Forward(Namespace): + def __init__(self): + super(TF2_Forward, self).__init__("tf2") + + self._load_cxx_class("Vector3", stamped="both") + self._load_cxx_class("Quaternion", stamped="both") + self._load_cxx_class("Transform", stamped="both") + self._load_cxx_class("Wrench", stamped=True) + + +# generate mappings # + +def build_mappings(): + bullet = Bullet() + eigen = Eigen() + kdl = KDL() + tf2_fwd = TF2_Forward() + + maps = { + "Pose": [ + tf2_fwd.Transform, + eigen.Transform, + kdl.Frame, + ], + # "Vector3": [eigen.Vector3, tf2_fwd.Vector3], + "Point": [ + eigen.Vector3, + tf2_fwd.Vector3, + kdl.Vector3, + bullet.Vector3, + ], + "Quaternion": [ + eigen.Quaternion, + tf2_fwd.Quaternion, + ], + "Wrench": [ + tf2_fwd.Wrench, + kdl.Wrench, + ], + } + + ns_tf2 = Namespace("tf2") + + for msgs_name, classes in sorted(maps.items()): + for mapping in TypeMapIterator(msgs_name, ns_tf2, classes): + pass + + return (bullet, tf2_fwd, eigen, kdl, ns_tf2) + + +hdr_begin = """/* + * Copyright (c) 2020, 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 Open Source Robotics Foundation 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. + */ + + + // NOTE: This file is generated automatically, do not edit. + +#pragma once + +#include +#include + +""" + +if __name__ == "__main__": + with open(argv[1], "w") as f: + f.write(hdr_begin) + namespaces = build_mappings() + namespaces[-1].write_includes(f) + + for ns in namespaces: + ns.write(f) diff --git a/tf2/test/three_way_convert.cpp b/tf2/test/three_way_convert.cpp new file mode 100644 index 000000000..6170ea2f9 --- /dev/null +++ b/tf2/test/three_way_convert.cpp @@ -0,0 +1,261 @@ +/* + * 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. + */ + +#include +#include +#include + +// some test data + +namespace Foo { + struct Vec3 { + double x,y,z; + }; + + struct Quaternion {}; +} + +namespace Bar { + struct Vec3 { + double x,y,z; + }; + + struct Quaternion {}; +} + +namespace Baz { + struct Vec3 { + double x,y,z; + }; +} + +namespace tf2 { + + +template<> +inline +void fromMsg(const geometry_msgs::Vector3& msg, Foo::Vec3 &out) { + out.x = msg.x; + out.y = msg.y; + out.z = msg.z; +} + +template<> +inline +void fromMsg(const geometry_msgs::Vector3& msg, Bar::Vec3 &out) { + out.x = msg.x; + out.y = msg.y; + out.z = msg.z; +} + +template<> +inline +void fromMsg(const geometry_msgs::Vector3& msg, Baz::Vec3 &out) { + out.x = msg.x; + out.y = msg.y; + out.z = msg.z; +} + +template<> +inline +geometry_msgs::Vector3& toMsg(const Foo::Vec3 &in, geometry_msgs::Vector3& msg) { + msg.x = in.x; + msg.y = in.y; + msg.z = in.z; + return msg; +} + +template<> +inline +geometry_msgs::Vector3& toMsg(const Bar::Vec3 &in, geometry_msgs::Vector3& msg) { + msg.x = in.x; + msg.y = in.y; + msg.z = in.z; + return msg; +} + +template<> +struct BidirectionalTypeMap { + using type = geometry_msgs::Vector3; +}; + +template<> +struct UnidirectionalTypeMap { + using type = geometry_msgs::Vector3; +}; + +} // namespace tf2 + +// init vector with integers +template +V initVec() { + V v; + v.x = 2.0; v.y = 3.0; v.z = -4.0; + return v; +} + +template +using void_t = void; + +// helper to check tf2::impl::get_common_bidirectional_type +template +struct get_bi_type_returns_type : std::false_type {}; + +template +struct get_bi_type_returns_type::type>> +: std::true_type{}; + +// helper to check tf2::impl::get_common_unidirectional_type +template +struct get_uni_type_returns_type : std::false_type {}; + +template +struct get_uni_type_returns_type::type>> +: std::true_type{}; + +// helper to check whether error message would be shown with static_assert +template +using would_show_error_msg = std::is_same< + decltype(tf2::impl::convertViaMessage(std::declval(), std::declval())), + tf2::impl::common_type_lookup_failed>; + + +TEST(ThreeWayConvert, BidirectionalTypeMap) { + // check raw structs + ::testing::StaticAssertTypeEq::type, + geometry_msgs::Vector3>(); + ::testing::StaticAssertTypeEq::type, + std::nullptr_t>(); + ::testing::StaticAssertTypeEq::type, + std::nullptr_t>(); + ::testing::StaticAssertTypeEq::type, + std::nullptr_t>(); + + // check getters + EXPECT_TRUE((get_bi_type_returns_type::value)); + EXPECT_FALSE((get_bi_type_returns_type::value)); + EXPECT_FALSE((get_uni_type_returns_type::value)); + EXPECT_FALSE((get_uni_type_returns_type::value)); + + EXPECT_FALSE((tf2::impl::has_no_common_msgs::value)); + EXPECT_FALSE((tf2::impl::has_no_common_msgs::value)); + + EXPECT_FALSE((would_show_error_msg::value)); + + const auto v1 = initVec(); + Foo::Vec3 v3; + + + Bar::Vec3 v2; + + tf2::convert(v1, v3); + tf2::convert(v3, v2); + tf2::convert(v2, v2); + tf2::convert(v2, v3); + + EXPECT_EQ(v1.x, v3.x); + EXPECT_EQ(v1.y, v3.y); + EXPECT_EQ(v1.z, v3.z); +} + +TEST(ThreeWayConvert, UnidirectionalTypeMap) { + // check raw structs + ::testing::StaticAssertTypeEq::type, + std::nullptr_t>(); + ::testing::StaticAssertTypeEq::type, + std::nullptr_t>(); + ::testing::StaticAssertTypeEq::type, + std::nullptr_t>(); + ::testing::StaticAssertTypeEq::type, + geometry_msgs::Vector3>(); + + // check getters + EXPECT_FALSE((get_bi_type_returns_type::value)); + EXPECT_FALSE((get_bi_type_returns_type::value)); + EXPECT_FALSE((get_uni_type_returns_type::value)); + EXPECT_TRUE((get_uni_type_returns_type::value)); + + + EXPECT_FALSE((tf2::impl::has_no_common_msgs::value)); + EXPECT_TRUE((tf2::impl::has_no_common_msgs::value)); + + EXPECT_FALSE((would_show_error_msg::value)); + EXPECT_TRUE((would_show_error_msg::value)); + + const auto v1 = initVec(); + Baz::Vec3 v2; + + tf2::convert(v1, v2); + + EXPECT_EQ(v1.x, v2.x); + EXPECT_EQ(v1.y, v2.y); + EXPECT_EQ(v1.z, v2.z); +} + +TEST(ThreeWayConvert, UnrelatedTypes) { + ::testing::StaticAssertTypeEq::type, + std::nullptr_t>(); + ::testing::StaticAssertTypeEq::type, + std::nullptr_t>(); + ::testing::StaticAssertTypeEq::type, + std::nullptr_t>(); + ::testing::StaticAssertTypeEq::type, + std::nullptr_t>(); + + // check getters + EXPECT_FALSE((get_bi_type_returns_type::value)); + EXPECT_FALSE((get_bi_type_returns_type::value)); + EXPECT_FALSE((get_uni_type_returns_type::value)); + EXPECT_FALSE((get_uni_type_returns_type::value)); + + EXPECT_TRUE((tf2::impl::has_no_common_msgs::value)); + EXPECT_TRUE((tf2::impl::has_no_common_msgs::value)); + + EXPECT_TRUE((would_show_error_msg::value)); + EXPECT_TRUE((would_show_error_msg::value)); +} + +int main(int argc, char **argv){ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tf2_bullet/include/tf2_bullet/tf2_bullet.h b/tf2_bullet/include/tf2_bullet/tf2_bullet.h index 63e88f5ac..19b395b44 100644 --- a/tf2_bullet/include/tf2_bullet/tf2_bullet.h +++ b/tf2_bullet/include/tf2_bullet/tf2_bullet.h @@ -32,7 +32,8 @@ #ifndef TF2_BULLET_H #define TF2_BULLET_H -#include +#include +#include #include #include @@ -53,7 +54,7 @@ btTransform transformToBullet(const geometry_msgs::TransformStamped& t) /** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Vector3 type. - * This function is a specialization of the doTransform template defined in tf2/convert.h + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h * \param t_in The vector to transform, as a timestamped Bullet btVector3 data type. * \param t_out The transformed vector, as a timestamped Bullet btVector3 data type. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -66,14 +67,15 @@ inline } /** \brief Convert a stamped Bullet Vector3 type to a PointStamped message. - * This function is a specialization of the toMsg template defined in tf2/convert.h + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h * \param in The timestamped Bullet btVector3 to convert. - * \return The vector converted to a PointStamped message. + * \param msg PointStamped Message to write to + * \return Reference to Parameter msg, the vector converted to a PointStamped message. */ +template <> inline -geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) +geometry_msgs::PointStamped& toMsg(const tf2::Stamped& in, geometry_msgs::PointStamped& msg) { - geometry_msgs::PointStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.point.x = in[0]; @@ -83,10 +85,11 @@ geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) } /** \brief Convert a PointStamped message type to a stamped Bullet-specific Vector3 type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h * \param msg The PointStamped message to convert. * \param out The point converted to a timestamped Bullet Vector3. */ +template <> inline void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) { @@ -99,7 +102,7 @@ void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& ou /** \brief Apply a geometry_msgs TransformStamped to a Bullet-specific Transform data type. - * This function is a specialization of the doTransform template defined in tf2/convert.h + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h * \param t_in The frame to transform, as a timestamped Bullet btTransform. * \param t_out The transformed frame, as a timestamped Bullet btTransform. * \param transform The timestamped transform to apply, as a TransformStamped message. diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.h b/tf2_eigen/include/tf2_eigen/tf2_eigen.h index f2bc67120..0fd0e3ac2 100644 --- a/tf2_eigen/include/tf2_eigen/tf2_eigen.h +++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.h @@ -30,11 +30,13 @@ #define TF2_EIGEN_H #include +#include #include #include #include #include #include +#include namespace tf2 @@ -102,7 +104,7 @@ geometry_msgs::TransformStamped eigenToTransform(const Eigen::Isometry3d& T) } /** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Vector3d type. - * This function is a specialization of the doTransform template defined in tf2/convert.h, + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h, * although it can not be used in tf2_ros::BufferInterface::transform because this * functions rely on the existence of a time stamp and a frame id in the type which should * get transformed. @@ -118,14 +120,15 @@ void doTransform(const Eigen::Vector3d& t_in, Eigen::Vector3d& t_out, const geom } /** \brief Convert a Eigen Vector3d type to a Point message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The timestamped Eigen Vector3d to convert. - * \return The vector converted to a Point message. + * \param msg The vector converted to a Point message. + * \return Reference to \c msg parameter. */ +template <> inline -geometry_msgs::Point toMsg(const Eigen::Vector3d& in) +geometry_msgs::Point& toMsg(const Eigen::Vector3d& in, geometry_msgs::Point& msg) { - geometry_msgs::Point msg; msg.x = in.x(); msg.y = in.y(); msg.z = in.z(); @@ -133,10 +136,11 @@ geometry_msgs::Point toMsg(const Eigen::Vector3d& in) } /** \brief Convert a Point message type to a Eigen-specific Vector3d type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h * \param msg The Point message to convert. * \param out The point converted to a Eigen Vector3d. */ +template <> inline void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) { @@ -146,10 +150,12 @@ void fromMsg(const geometry_msgs::Point& msg, Eigen::Vector3d& out) } /** \brief Convert an Eigen Vector3d type to a Vector3 message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The Eigen Vector3d to convert. - * \return The vector converted to a Vector3 message. + * \param out The vector converted to a Vector3 message. + * \return Reference to \c msg parameter. */ +template <> inline geometry_msgs::Vector3& toMsg(const Eigen::Vector3d& in, geometry_msgs::Vector3& out) { @@ -160,10 +166,11 @@ geometry_msgs::Vector3& toMsg(const Eigen::Vector3d& in, geometry_msgs::Vector3& } /** \brief Convert a Vector3 message type to a Eigen-specific Vector3d type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h * \param msg The Vector3 message to convert. * \param out The vector converted to a Eigen Vector3d. */ +template <> inline void fromMsg(const geometry_msgs::Vector3& msg, Eigen::Vector3d& out) { @@ -172,8 +179,39 @@ void fromMsg(const geometry_msgs::Vector3& msg, Eigen::Vector3d& out) out.z() = msg.z; } +/** \brief Convert a stamped Eigen Vector3d type to a Vector3Stamped message. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. + * \param in The timestamped Eigen Vector3d to convert. + * \param msg The vector converted to a Vector3Stamped message. + * \return Reference to \c msg parameter. + */ +template <> +inline +geometry_msgs::Vector3Stamped& toMsg(const tf2::Stamped& in, geometry_msgs::Vector3Stamped& msg) +{ + msg.header.stamp = in.stamp_; + msg.header.frame_id = in.frame_id_; + toMsg(static_cast(in), msg.vector); + return msg; +} + +/** \brief Convert a Vector3Stamped message type to a stamped Eigen-specific Vector3d type. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h + * \param msg The Vector3Stamped message to convert. + * \param out The point converted to a timestamped Eigen Vector3d. + */ +template <> +inline +void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf2::Stamped& out) { + out.stamp_ = msg.header.stamp; + out.frame_id_ = msg.header.frame_id; + fromMsg(msg.vector, static_cast(out)); +} + + + /** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Vector3d type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The vector to transform, as a timestamped Eigen Vector3d data type. * \param t_out The transformed vector, as a timestamped Eigen Vector3d data type. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -189,25 +227,27 @@ void doTransform(const tf2::Stamped& t_in, } /** \brief Convert a stamped Eigen Vector3d type to a PointStamped message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The timestamped Eigen Vector3d to convert. - * \return The vector converted to a PointStamped message. + * \param msg The vector converted to a PointStamped message. + * \return Reference to \c msg parameter. */ +template <> inline -geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) +geometry_msgs::PointStamped& toMsg(const tf2::Stamped& in, geometry_msgs::PointStamped& msg) { - geometry_msgs::PointStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; - msg.point = toMsg(static_cast(in)); + toMsg(static_cast(in), msg.point); return msg; } /** \brief Convert a PointStamped message type to a stamped Eigen-specific Vector3d type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h * \param msg The PointStamped message to convert. * \param out The point converted to a timestamped Eigen Vector3d. */ +template <> inline void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; @@ -216,7 +256,7 @@ void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped inline -geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) { - geometry_msgs::Quaternion msg; +geometry_msgs::Quaternion& toMsg(const Eigen::Quaterniond& in, geometry_msgs::Quaternion& msg) { msg.w = in.w(); msg.x = in.x(); msg.y = in.y(); @@ -256,17 +297,18 @@ geometry_msgs::Quaternion toMsg(const Eigen::Quaterniond& in) { } /** \brief Convert a Quaternion message type to a Eigen-specific Quaterniond type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h * \param msg The Quaternion message to convert. * \param out The quaternion converted to a Eigen Quaterniond. */ +template <> inline void fromMsg(const geometry_msgs::Quaternion& msg, Eigen::Quaterniond& out) { out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z); } /** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type. - * This function is a specialization of the doTransform template defined in tf2/convert.h, + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h, * although it can not be used in tf2_ros::BufferInterface::transform because this * functions rely on the existence of a time stamp and a frame id in the type which should * get transformed. @@ -285,24 +327,26 @@ void doTransform(const Eigen::Quaterniond& t_in, } /** \brief Convert a stamped Eigen Quaterniond type to a QuaternionStamped message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The timestamped Eigen Quaterniond to convert. - * \return The quaternion converted to a QuaternionStamped message. + * \param msg The quaternion converted to a QuaternionStamped message. + * \return Reference to \c msg parameter. */ +template <> inline -geometry_msgs::QuaternionStamped toMsg(const Stamped& in) { - geometry_msgs::QuaternionStamped msg; +geometry_msgs::QuaternionStamped& toMsg(const Stamped& in, geometry_msgs::QuaternionStamped& msg) { msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; - msg.quaternion = toMsg(static_cast(in)); + toMsg(static_cast(in), msg.quaternion); return msg; } /** \brief Convert a QuaternionStamped message type to a stamped Eigen-specific Quaterniond type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h * \param msg The QuaternionStamped message to convert. * \param out The quaternion converted to a timestamped Eigen Quaterniond. */ +template <> inline void fromMsg(const geometry_msgs::QuaternionStamped& msg, Stamped& out) { out.frame_id_ = msg.header.frame_id; @@ -311,7 +355,7 @@ void fromMsg(const geometry_msgs::QuaternionStamped& msg, Stamped& t_in, } /** \brief Convert a Eigen Affine3d transform type to a Pose message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The Eigen Affine3d to convert. - * \return The Eigen transform converted to a Pose message. + * \param msg The Eigen transform converted to a Pose message. + * \return Reference to \c msg parameter. */ +template <> inline -geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { - geometry_msgs::Pose msg; +geometry_msgs::Pose& toMsg(const Eigen::Affine3d& in, geometry_msgs::Pose& msg) { msg.position.x = in.translation().x(); msg.position.y = in.translation().y(); msg.position.z = in.translation().z(); @@ -352,13 +397,14 @@ geometry_msgs::Pose toMsg(const Eigen::Affine3d& in) { } /** \brief Convert a Eigen Isometry3d transform type to a Pose message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The Eigen Isometry3d to convert. - * \return The Eigen transform converted to a Pose message. + * \param msg The Eigen transform converted to a Pose message. + * \return Reference to \c msg parameter. */ +template <> inline -geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) { - geometry_msgs::Pose msg; +geometry_msgs::Pose& toMsg(const Eigen::Isometry3d& in,geometry_msgs::Pose& msg ) { msg.position.x = in.translation().x(); msg.position.y = in.translation().y(); msg.position.z = in.translation().z(); @@ -377,10 +423,11 @@ geometry_msgs::Pose toMsg(const Eigen::Isometry3d& in) { } /** \brief Convert a Pose message transform type to a Eigen Affine3d. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param msg The Pose message to convert. * \param out The pose converted to a Eigen Affine3d. */ +template <> inline void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { out = Eigen::Affine3d( @@ -392,10 +439,11 @@ void fromMsg(const geometry_msgs::Pose& msg, Eigen::Affine3d& out) { } /** \brief Convert a Pose message transform type to a Eigen Isometry3d. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param msg The Pose message to convert. * \param out The pose converted to a Eigen Isometry3d. */ +template <> inline void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) { out = Eigen::Isometry3d( @@ -407,13 +455,14 @@ void fromMsg(const geometry_msgs::Pose& msg, Eigen::Isometry3d& out) { } /** \brief Convert a Eigen 6x1 Matrix type to a Twist message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The 6x1 Eigen Matrix to convert. - * \return The Eigen Matrix converted to a Twist message. + * \param msg The Eigen Matrix converted to a Twist message. + * \return Reference to \c msg parameter. */ +template <> inline -geometry_msgs::Twist toMsg(const Eigen::Matrix& in) { - geometry_msgs::Twist msg; +geometry_msgs::Twist& toMsg(const Eigen::Matrix& in, geometry_msgs::Twist& msg) { msg.linear.x = in[0]; msg.linear.y = in[1]; msg.linear.z = in[2]; @@ -424,10 +473,11 @@ geometry_msgs::Twist toMsg(const Eigen::Matrix& in) { } /** \brief Convert a Twist message transform type to a Eigen 6x1 Matrix. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param msg The Twist message to convert. * \param out The twist converted to a Eigen 6x1 Matrix. */ +template <> inline void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { out[0] = msg.linear.x; @@ -439,7 +489,7 @@ void fromMsg(const geometry_msgs::Twist &msg, Eigen::Matrix& out) { } /** \brief Apply a geometry_msgs TransformStamped to an Eigen Affine3d transform. - * This function is a specialization of the doTransform template defined in tf2/convert.h, + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h, * although it can not be used in tf2_ros::BufferInterface::transform because this * function relies on the existence of a time stamp and a frame id in the type which should * get transformed. @@ -456,7 +506,7 @@ void doTransform(const tf2::Stamped& t_in, } /** \brief Apply a geometry_msgs TransformStamped to an Eigen Isometry transform. - * This function is a specialization of the doTransform template defined in tf2/convert.h, + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h, * although it can not be used in tf2_ros::BufferInterface::transform because this * function relies on the existence of a time stamp and a frame id in the type which should * get transformed. @@ -473,35 +523,43 @@ void doTransform(const tf2::Stamped& t_in, } /** \brief Convert a stamped Eigen Affine3d transform type to a Pose message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The timestamped Eigen Affine3d to convert. - * \return The Eigen transform converted to a PoseStamped message. + * \param msg The Eigen transform converted to a PoseStamped message. + * \return Reference to \c msg parameter. */ +template <> inline -geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) +geometry_msgs::PoseStamped& toMsg(const tf2::Stamped& in, geometry_msgs::PoseStamped& msg) { - geometry_msgs::PoseStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; - msg.pose = toMsg(static_cast(in)); + toMsg(static_cast(in), msg.pose); return msg; } +/** \brief Convert a stamped Eigen Isometry transform type to a Pose message. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. + * \param in The timestamped Eigen Isometry3d to convert. + * \param msg The Eigen transform converted to a PoseStamped message. + * \return Reference to \c msg parameter. + */ +template <> inline -geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) +geometry_msgs::PoseStamped& toMsg(const tf2::Stamped& in, geometry_msgs::PoseStamped& msg) { - geometry_msgs::PoseStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; - msg.pose = toMsg(static_cast(in)); + toMsg(static_cast(in), msg.pose); return msg; } /** \brief Convert a Pose message transform type to a stamped Eigen Affine3d. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param msg The PoseStamped message to convert. * \param out The pose converted to a timestamped Eigen Affine3d. */ +template <> inline void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) { @@ -510,6 +568,12 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped(out)); } +/** \brief Convert a Pose message transform type to a stamped Eigen Isometry3d. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. + * \param msg The PoseStamped message to convert. + * \param out The pose converted to a timestamped Eigen Isometry3d. + */ +template <> inline void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) { @@ -520,7 +584,4 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped - #endif // TF2_EIGEN_H diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h index 717b5eb82..1c1953a6e 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.h @@ -32,7 +32,8 @@ #ifndef TF2_GEOMETRY_MSGS_H #define TF2_GEOMETRY_MSGS_H -#include +#include +#include #include #include #include @@ -74,14 +75,15 @@ KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t) /*************/ /** \brief Convert a tf2 Vector3 type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in A tf2 Vector3 object. - * \return The Vector3 converted to a geometry_msgs message type. + * \param out The Vector3 converted to a geometry_msgs message type. + * \return Reference to \c out parameter. */ +template <> inline -geometry_msgs::Vector3 toMsg(const tf2::Vector3& in) +geometry_msgs::Vector3& toMsg(const tf2::Vector3& in, geometry_msgs::Vector3& out) { - geometry_msgs::Vector3 out; out.x = in.getX(); out.y = in.getY(); out.z = in.getZ(); @@ -89,10 +91,11 @@ geometry_msgs::Vector3 toMsg(const tf2::Vector3& in) } /** \brief Convert a Vector3 message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h. * \param in A Vector3 message type. * \param out The Vector3 converted to a tf2 type. */ +template <> inline void fromMsg(const geometry_msgs::Vector3& in, tf2::Vector3& out) { @@ -105,7 +108,7 @@ void fromMsg(const geometry_msgs::Vector3& in, tf2::Vector3& out) /********************/ /** \brief Extract a timestamp from the header of a Vector message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * This function is a specialization of the getTimestamp template defined in tf2/transform_functions.h. * \param t VectorStamped message to extract the timestamp from. * \return The timestamp of the message. The lifetime of the returned reference * is bound to the lifetime of the argument. @@ -115,7 +118,7 @@ inline const ros::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return t.header.stamp;} /** \brief Extract a frame ID from the header of a Vector message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * This function is a specialization of the getFrameId template defined in tf2/transform_functions.h. * \param t VectorStamped message to extract the frame ID from. * \return A string containing the frame ID of the message. The lifetime of the * returned reference is bound to the lifetime of the argument. @@ -124,57 +127,34 @@ template <> inline const std::string& getFrameId(const geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;} - -/** \brief Trivial "conversion" function for Vector3 message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A Vector3Stamped message. - * \return The input argument. - */ -inline -geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in) -{ - return in; -} - -/** \brief Trivial "conversion" function for Vector3 message type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg A Vector3Stamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out) -{ - out = msg; -} - /** \brief Convert as stamped tf2 Vector3 type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in An instance of the tf2::Vector3 specialization of the tf2::Stamped template. - * \return The Vector3Stamped converted to a geometry_msgs Vector3Stamped message type. + * \param out The Vector3Stamped converted to a geometry_msgs Vector3Stamped message type. + * \return Reference to \c out parameter. */ +template <> inline -geometry_msgs::Vector3Stamped toMsg(const tf2::Stamped& in) +geometry_msgs::Vector3Stamped& toMsg(const tf2::Stamped& in, geometry_msgs::Vector3Stamped& out) { - geometry_msgs::Vector3Stamped out; out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; - out.vector.x = in.getX(); - out.vector.y = in.getY(); - out.vector.z = in.getZ(); + toMsg(static_cast(in), out.vector); return out; } /** \brief Convert a Vector3Stamped message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h. * \param msg A Vector3Stamped message. * \param out The Vector3Stamped converted to the equivalent tf2 type. */ +template <> inline void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; - out.setData(tf2::Vector3(msg.vector.x, msg.vector.y, msg.vector.z)); + fromMsg(msg.vector, static_cast(out)); } @@ -183,10 +163,12 @@ void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf2::Stamped inline geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out) { @@ -197,10 +179,11 @@ geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out) } /** \brief Convert a Vector3 message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h. * \param in A Vector3 message type. * \param out The Vector3 converted to a tf2 type. */ +template <> inline void fromMsg(const geometry_msgs::Point& in, tf2::Vector3& out) { @@ -213,7 +196,7 @@ void fromMsg(const geometry_msgs::Point& in, tf2::Vector3& out) /******************/ /** \brief Extract a timestamp from the header of a Point message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * This function is a specialization of the getTimestamp template defined in tf2/transform_functions.h. * \param t PointStamped message to extract the timestamp from. * \return The timestamp of the message. The lifetime of the returned reference * is bound to the lifetime of the argument. @@ -223,7 +206,7 @@ inline const ros::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return t.header.stamp;} /** \brief Extract a frame ID from the header of a Point message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * This function is a specialization of the getFrameId template defined in tf2/transform_functions.h. * \param t PointStamped message to extract the frame ID from. * \return A string containing the frame ID of the message. The lifetime of the * returned reference is bound to the lifetime of the argument. @@ -232,55 +215,34 @@ template <> inline const std::string& getFrameId(const geometry_msgs::PointStamped& t) {return t.header.frame_id;} -/** \brief Trivial "conversion" function for Point message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A PointStamped message. - * \return The input argument. - */ -inline -geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in) -{ - return in; -} - -/** \brief Trivial "conversion" function for Point message type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg A PointStamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out) -{ - out = msg; -} - /** \brief Convert as stamped tf2 Vector3 type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in An instance of the tf2::Vector3 specialization of the tf2::Stamped template. - * \return The Vector3Stamped converted to a geometry_msgs PointStamped message type. + * \param out The Vector3Stamped converted to a geometry_msgs PointStamped message type. + * \return Reference to \c out parameter. */ +template <> inline -geometry_msgs::PointStamped toMsg(const tf2::Stamped& in, geometry_msgs::PointStamped & out) +geometry_msgs::PointStamped& toMsg(const tf2::Stamped& in, geometry_msgs::PointStamped & out) { out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; - out.point.x = in.getX(); - out.point.y = in.getY(); - out.point.z = in.getZ(); + toMsg(static_cast(in), out.point); return out; } /** \brief Convert a PointStamped message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h. * \param msg A PointStamped message. * \param out The PointStamped converted to the equivalent tf2 type. */ +template <> inline void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; - out.setData(tf2::Vector3(msg.point.x, msg.point.y, msg.point.z)); + fromMsg(msg.point, static_cast(out)); } @@ -289,14 +251,15 @@ void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& /****************/ /** \brief Convert a tf2 Quaternion type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in A tf2 Quaternion object. - * \return The Quaternion converted to a geometry_msgs message type. + * \param out The Quaternion converted to a geometry_msgs message type. + * \return Reference to \c out parameter. */ +template <> inline -geometry_msgs::Quaternion toMsg(const tf2::Quaternion& in) +geometry_msgs::Quaternion& toMsg(const tf2::Quaternion& in, geometry_msgs::Quaternion& out) { - geometry_msgs::Quaternion out; out.w = in.getW(); out.x = in.getX(); out.y = in.getY(); @@ -305,10 +268,11 @@ geometry_msgs::Quaternion toMsg(const tf2::Quaternion& in) } /** \brief Convert a Quaternion message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h. * \param in A Quaternion message type. * \param out The Quaternion converted to a tf2 type. */ +template <> inline void fromMsg(const geometry_msgs::Quaternion& in, tf2::Quaternion& out) { @@ -322,7 +286,7 @@ void fromMsg(const geometry_msgs::Quaternion& in, tf2::Quaternion& out) /***********************/ /** \brief Extract a timestamp from the header of a Quaternion message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * This function is a specialization of the getTimestamp template defined in tf2/transform_functions.h. * \param t QuaternionStamped message to extract the timestamp from. * \return The timestamp of the message. The lifetime of the returned reference * is bound to the lifetime of the argument. @@ -332,7 +296,7 @@ inline const ros::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {return t.header.stamp;} /** \brief Extract a frame ID from the header of a Quaternion message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * This function is a specialization of the getFrameId template defined in tf2/transform_functions.h. * \param t QuaternionStamped message to extract the frame ID from. * \return A string containing the frame ID of the message. The lifetime of the * returned reference is bound to the lifetime of the argument. @@ -341,85 +305,36 @@ template <> inline const std::string& getFrameId(const geometry_msgs::QuaternionStamped& t) {return t.header.frame_id;} -/** \brief Trivial "conversion" function for Quaternion message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A QuaternionStamped message. - * \return The input argument. - */ -inline -geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& in) -{ - return in; -} - -/** \brief Trivial "conversion" function for Quaternion message type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. - * \param msg A QuaternionStamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::QuaternionStamped& msg, geometry_msgs::QuaternionStamped& out) -{ - out = msg; -} - /** \brief Convert as stamped tf2 Quaternion type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in An instance of the tf2::Quaternion specialization of the tf2::Stamped template. - * \return The QuaternionStamped converted to a geometry_msgs QuaternionStamped message type. + * \param out The QuaternionStamped converted to a geometry_msgs QuaternionStamped message type. + * \return Reference to \c out parameter. */ +template <> inline -geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in) +geometry_msgs::QuaternionStamped& toMsg(const tf2::Stamped& in, geometry_msgs::QuaternionStamped& out) { - geometry_msgs::QuaternionStamped out; out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; - out.quaternion.w = in.getW(); - out.quaternion.x = in.getX(); - out.quaternion.y = in.getY(); - out.quaternion.z = in.getZ(); + toMsg(static_cast(in), out.quaternion); return out; } -template <> -inline -ROS_DEPRECATED geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in); - - -//Backwards compatibility remove when forked for Lunar or newer -template <> -inline -geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped& in) -{ - return toMsg(in); -} - /** \brief Convert a QuaternionStamped message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h. * \param in A QuaternionStamped message type. * \param out The QuaternionStamped converted to the equivalent tf2 type. */ +template <> inline void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out) { out.stamp_ = in.header.stamp; out.frame_id_ = in.header.frame_id; - tf2::Quaternion tmp; - fromMsg(in.quaternion, tmp); - out.setData(tmp); + fromMsg(in.quaternion, static_cast(out)); } -template<> -inline -ROS_DEPRECATED void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out); - -//Backwards compatibility remove when forked for Lunar or newer -template<> -inline -void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped& out) -{ - fromMsg(in, out); -} /**********/ /** Pose **/ @@ -428,12 +343,14 @@ void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped inline geometry_msgs::Pose& toMsg(const tf2::Transform& in, geometry_msgs::Pose& out) { toMsg(in.getOrigin(), out.position); - out.orientation = toMsg(in.getRotation()); + toMsg(in.getRotation(), out.orientation); return out; } @@ -441,6 +358,7 @@ geometry_msgs::Pose& toMsg(const tf2::Transform& in, geometry_msgs::Pose& out) * \param in A Pose message. * \param out The Pose converted to a tf2 Transform type. */ +template <> inline void fromMsg(const geometry_msgs::Pose& in, tf2::Transform& out) { @@ -456,7 +374,7 @@ void fromMsg(const geometry_msgs::Pose& in, tf2::Transform& out) /*****************/ /** \brief Extract a timestamp from the header of a Pose message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * This function is a specialization of the getTimestamp template defined in tf2/transform_functions.h. * \param t PoseStamped message to extract the timestamp from. * \return The timestamp of the message. */ @@ -465,7 +383,7 @@ inline const ros::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return t.header.stamp;} /** \brief Extract a frame ID from the header of a Pose message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * This function is a specialization of the getFrameId template defined in tf2/transform_functions.h. * \param t PoseStamped message to extract the frame ID from. * \return A string containing the frame ID of the message. */ @@ -473,56 +391,34 @@ template <> inline const std::string& getFrameId(const geometry_msgs::PoseStamped& t) {return t.header.frame_id;} -/** \brief Trivial "conversion" function for Pose message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A PoseStamped message. - * \return The input argument. - */ -inline -geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in) -{ - return in; -} - -/** \brief Trivial "conversion" function for Pose message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg A PoseStamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out) -{ - out = msg; -} - /** \brief Convert as stamped tf2 Pose type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in An instance of the tf2::Pose specialization of the tf2::Stamped template. - * \return The PoseStamped converted to a geometry_msgs PoseStamped message type. + * \param out The PoseStamped converted to a geometry_msgs PoseStamped message type. + * \return Reference to \c out parameter. */ +template <> inline -geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in, geometry_msgs::PoseStamped & out) +geometry_msgs::PoseStamped& toMsg(const tf2::Stamped& in, geometry_msgs::PoseStamped & out) { out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; - toMsg(in.getOrigin(), out.pose.position); - out.pose.orientation = toMsg(in.getRotation()); + toMsg(static_cast(in), out.pose); return out; } /** \brief Convert a PoseStamped message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h. * \param msg A PoseStamped message. * \param out The PoseStamped converted to the equivalent tf2 type. */ +template <> inline void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; - tf2::Transform tmp; - fromMsg(msg.pose, tmp); - out.setData(tmp); + fromMsg(msg.pose, static_cast(out)); } /*******************************/ @@ -530,7 +426,7 @@ void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped /*******************************/ /** \brief Extract a timestamp from the header of a PoseWithCovarianceStamped message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * This function is a specialization of the getTimestamp template defined in tf2/transform_functions.h. * \param t PoseWithCovarianceStamped message to extract the timestamp from. * \return The timestamp of the message. */ @@ -539,7 +435,7 @@ inline const ros::Time& getTimestamp(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.stamp;} /** \brief Extract a frame ID from the header of a PoseWithCovarianceStamped message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * This function is a specialization of the getFrameId template defined in tf2/transform_functions.h. * \param t PoseWithCovarianceStamped message to extract the frame ID from. * \return A string containing the frame ID of the message. */ @@ -547,56 +443,34 @@ template <> inline const std::string& getFrameId(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.frame_id;} -/** \brief Trivial "conversion" function for PoseWithCovarianceStamped message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A PoseWithCovarianceStamped message. - * \return The input argument. - */ -inline -geometry_msgs::PoseWithCovarianceStamped toMsg(const geometry_msgs::PoseWithCovarianceStamped& in) -{ - return in; -} - -/** \brief Trivial "conversion" function for PoseWithCovarianceStamped message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg A PoseWithCovarianceStamped message. - * \param out The input argument. - */ -inline -void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, geometry_msgs::PoseWithCovarianceStamped& out) -{ - out = msg; -} - /** \brief Convert as stamped tf2 PoseWithCovarianceStamped type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in An instance of the tf2::Pose specialization of the tf2::Stamped template. - * \return The PoseWithCovarianceStamped converted to a geometry_msgs PoseWithCovarianceStamped message type. + * \param out The PoseWithCovarianceStamped converted to a geometry_msgs PoseWithCovarianceStamped message type. + * \return Reference to \c out parameter. */ +template <> inline -geometry_msgs::PoseWithCovarianceStamped toMsg(const tf2::Stamped& in, geometry_msgs::PoseWithCovarianceStamped & out) +geometry_msgs::PoseWithCovarianceStamped& toMsg(const tf2::Stamped& in, geometry_msgs::PoseWithCovarianceStamped & out) { out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; - toMsg(in.getOrigin(), out.pose.pose.position); - out.pose.pose.orientation = toMsg(in.getRotation()); + toMsg(static_cast(in), out.pose.pose); return out; } /** \brief Convert a PoseWithCovarianceStamped message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h. * \param msg A PoseWithCovarianceStamped message. * \param out The PoseWithCovarianceStamped converted to the equivalent tf2 type. */ +template <> inline void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; - tf2::Transform tmp; - fromMsg(msg.pose, tmp); - out.setData(tmp); + fromMsg(msg.pose, static_cast(out)); } /***************/ @@ -604,24 +478,26 @@ void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, tf2::Stamped inline -geometry_msgs::Transform toMsg(const tf2::Transform& in) +geometry_msgs::Transform& toMsg(const tf2::Transform& in, geometry_msgs::Transform& out) { - geometry_msgs::Transform out; - out.translation = toMsg(in.getOrigin()); - out.rotation = toMsg(in.getRotation()); + toMsg(in.getOrigin(), out.translation); + toMsg(in.getRotation(), out.rotation); return out; } /** \brief Convert a Transform message to its equivalent tf2 representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in A Transform message type. * \param out The Transform converted to a tf2 type. */ +template <> inline void fromMsg(const geometry_msgs::Transform& in, tf2::Transform& out) { @@ -640,7 +516,7 @@ void fromMsg(const geometry_msgs::Transform& in, tf2::Transform& out) /**********************/ /** \brief Extract a timestamp from the header of a Transform message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * This function is a specialization of the getTimestamp template defined in tf2/transform_functions.h. * \param t TransformStamped message to extract the timestamp from. * \return The timestamp of the message. */ @@ -649,7 +525,7 @@ inline const ros::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return t.header.stamp;} /** \brief Extract a frame ID from the header of a Transform message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * This function is a specialization of the getFrameId template defined in tf2/transform_functions.h. * \param t TransformStamped message to extract the frame ID from. * \return A string containing the frame ID of the message. */ @@ -657,62 +533,39 @@ template <> inline const std::string& getFrameId(const geometry_msgs::TransformStamped& t) {return t.header.frame_id;} -/** \brief Trivial "conversion" function for Transform message type. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param in A TransformStamped message. - * \return The input argument. - */ -inline -geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in) -{ - return in; -} - -/** \brief Convert a TransformStamped message to its equivalent tf2 representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. - * \param msg A TransformStamped message type. - * \param out The TransformStamped converted to the equivalent tf2 type. - */ -inline -void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::TransformStamped& out) -{ - out = msg; -} - /** \brief Convert as stamped tf2 Transform type to its equivalent geometry_msgs representation. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in An instance of the tf2::Transform specialization of the tf2::Stamped template. - * \return The tf2::Stamped converted to a geometry_msgs TransformStamped message type. + * \param out The tf2::Stamped converted to a geometry_msgs TransformStamped message type. + * \return Reference to \c out parameter. */ +template <> inline -geometry_msgs::TransformStamped toMsg(const tf2::Stamped& in) +geometry_msgs::TransformStamped& toMsg(const tf2::Stamped& in, geometry_msgs::TransformStamped& out) { - geometry_msgs::TransformStamped out; out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; - out.transform.translation = toMsg(in.getOrigin()); - out.transform.rotation = toMsg(in.getRotation()); + toMsg(static_cast(in), out.transform); return out; } /** \brief Convert a TransformStamped message to its equivalent tf2 representation. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h. * \param msg A TransformStamped message. * \param out The TransformStamped converted to the equivalent tf2 type. */ +template <> inline void fromMsg(const geometry_msgs::TransformStamped& msg, tf2::Stamped& out) { out.stamp_ = msg.header.stamp; out.frame_id_ = msg.header.frame_id; - tf2::Transform tmp; - fromMsg(msg.transform, tmp); - out.setData(tmp); + fromMsg(msg.transform, static_cast(out)); } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Point type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The point to transform, as a Point3 message. * \param t_out The transformed point, as a Point3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -730,7 +583,7 @@ inline } /** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Point type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The point to transform, as a timestamped Point3 message. * \param t_out The transformed point, as a timestamped Point3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -745,7 +598,7 @@ inline } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Quaternion type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The quaternion to transform, as a Quaternion3 message. * \param t_out The transformed quaternion, as a Quaternion3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -759,11 +612,11 @@ void doTransform(const geometry_msgs::Quaternion& t_in, geometry_msgs::Quaternio fromMsg(t_in, q_in); tf2::Quaternion q_out = t * q_in; - t_out = toMsg(q_out); + toMsg(q_out, t_out); } /** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Quaternion type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The quaternion to transform, as a timestamped Quaternion3 message. * \param t_out The transformed quaternion, as a timestamped Quaternion3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -779,7 +632,7 @@ void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::Qu /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Pose type. -* This function is a specialization of the doTransform template defined in tf2/convert.h. +* This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The pose to transform, as a Pose3 message. * \param t_out The transformed pose, as a Pose3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -800,7 +653,7 @@ void doTransform(const geometry_msgs::Pose& t_in, geometry_msgs::Pose& t_out, co } /** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Pose type. -* This function is a specialization of the doTransform template defined in tf2/convert.h. +* This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The pose to transform, as a timestamped Pose3 message. * \param t_out The transformed pose, as a timestamped Pose3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -903,7 +756,7 @@ geometry_msgs::PoseWithCovariance::_covariance_type transformCovariance(const ge } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs PoseWithCovarianceStamped type. -* This function is a specialization of the doTransform template defined in tf2/convert.h. +* This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The pose to transform, as a timestamped PoseWithCovarianceStamped message. * \param t_out The transformed pose, as a timestamped PoseWithCovarianceStamped message. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -928,7 +781,7 @@ void doTransform(const geometry_msgs::PoseWithCovarianceStamped& t_in, geometry_ } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Transform type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The frame to transform, as a timestamped Transform3 message. * \param t_out The frame transform, as a timestamped Transform3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -944,13 +797,13 @@ void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::Tra fromMsg(transform.transform, t); tf2::Transform v_out = t * input; - t_out.transform = toMsg(v_out); + toMsg(v_out, t_out.transform); t_out.header.stamp = transform.header.stamp; t_out.header.frame_id = transform.header.frame_id; } /** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs Vector type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The vector to transform, as a Vector3 message. * \param t_out The transformed vector, as a Vector3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -968,7 +821,7 @@ inline } /** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Vector type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The vector to transform, as a timestamped Vector3 message. * \param t_out The transformed vector, as a timestamped Vector3 message. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -986,40 +839,48 @@ inline /**********************/ /*** WrenchStamped ****/ /**********************/ + +/** \brief Extract a timestamp from the header WrenchStamped message. + * This function is a specialization of the getTimestamp template defined in tf2/transform_functions.h. + * \param t WrenchStamped message to extract the timestamp from. + * \return The timestamp of the message. + */ template <> inline const ros::Time& getTimestamp(const geometry_msgs::WrenchStamped& t) {return t.header.stamp;} - +/** \brief Extract a frame ID from the header of a WrenchStamped message. + * This function is a specialization of the getFrameId template defined in tf2/transform_functions.h. + * \param t WrenchStamped message to extract the frame ID from. + * \return A string containing the frame ID of the message. + */ template <> inline const std::string& getFrameId(const geometry_msgs::WrenchStamped& t) {return t.header.frame_id;} - -inline -geometry_msgs::WrenchStamped toMsg(const geometry_msgs::WrenchStamped& in) -{ - return in; -} - -inline -void fromMsg(const geometry_msgs::WrenchStamped& msg, geometry_msgs::WrenchStamped& out) -{ - out = msg; -} - - +/** \brief Convert an stamped array of two tf2::Vector3 (force and torque) to its equivalent geometry_msgs representation. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. + * \param in A stamped std::array of two tf2::Vector3 instances. + * \param out WrenchStamped instance + * \return Reference to \c out parameter. + */ +template<> inline -geometry_msgs::WrenchStamped toMsg(const tf2::Stamped>& in, geometry_msgs::WrenchStamped & out) +geometry_msgs::WrenchStamped& toMsg(const tf2::Stamped>& in, geometry_msgs::WrenchStamped & out) { out.header.stamp = in.stamp_; out.header.frame_id = in.frame_id_; - out.wrench.force = toMsg(in[0]); - out.wrench.torque = toMsg(in[1]); + toMsg(in[0], out.wrench.force); + toMsg(in[1], out.wrench.torque); return out; } - +/** \brief Convert a WrenchStamped message to its equivalent tf2 representation. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h. + * \param msg A WrenchStamped message. + * \param out Stamped std::array of tf2::Vector3 instances (force and torque) + */ +template<> inline void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped>& out) { @@ -1035,6 +896,12 @@ void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped inline void doTransform(const geometry_msgs::Wrench& t_in, geometry_msgs::Wrench& t_out, const geometry_msgs::TransformStamped& transform) @@ -1043,7 +910,12 @@ void doTransform(const geometry_msgs::Wrench& t_in, geometry_msgs::Wrench& t_out doTransform(t_in.torque, t_out.torque, transform); } - +/** \brief Apply a geometry_msgs TransformStamped to an stamped geometry_msgs Wrench type. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. + * \param t_in The timestamped wrench to transform. + * \param t_out The transformed and timestamped wrench. + * \param transform The timestamped transform to apply, as a TransformStamped message. + */ template<> inline void doTransform(const geometry_msgs::WrenchStamped& t_in, geometry_msgs::WrenchStamped& t_out, const geometry_msgs::TransformStamped& transform) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 860db53f6..9fad3dc0c 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -131,7 +131,7 @@ TEST(TfGeometry, PoseWithCovarianceStamped) // rotate pi/2 radians about x-axis geometry_msgs::TransformStamped t_rot; - t_rot.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(1,0,0), M_PI/2)); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(1,0,0), M_PI/2), t_rot.transform.rotation); t_rot.header.stamp = ros::Time(2.0); t_rot.header.frame_id = "A"; t_rot.child_frame_id = "rotated"; @@ -251,7 +251,7 @@ TEST(TfGeometry, doTransformPoint) trafo.transform.translation.x = -1; trafo.transform.translation.y = 2; trafo.transform.translation.z = -3; - trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0), trafo.transform.rotation); tf2::doTransform(v1, res, trafo); @@ -269,7 +269,7 @@ TEST(TfGeometry, doTransformQuaterion) trafo.transform.translation.x = -1; trafo.transform.translation.y = 2; trafo.transform.translation.z = -3; - trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0), trafo.transform.rotation); tf2::doTransform(v1, res, trafo); @@ -291,7 +291,7 @@ TEST(TfGeometry, doTransformPose) trafo.transform.translation.x = -1; trafo.transform.translation.y = 2; trafo.transform.translation.z = -3; - trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0), trafo.transform.rotation); tf2::doTransform(v1, res, trafo); @@ -316,7 +316,7 @@ TEST(TfGeometry, doTransformVector3) trafo.transform.translation.x = -1; trafo.transform.translation.y = 2; trafo.transform.translation.z = -3; - trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0), trafo.transform.rotation); tf2::doTransform(v1, res, trafo); @@ -339,7 +339,7 @@ TEST(TfGeometry, doTransformWrench) trafo.transform.translation.x = -1; trafo.transform.translation.y = 2; trafo.transform.translation.z = -3; - trafo.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0)); + tf2::toMsg(tf2::Quaternion(tf2::Vector3(0,0,1), -M_PI / 2.0), trafo.transform.rotation); tf2::doTransform(v1, res, trafo); EXPECT_NEAR(res.force.x, 1, EPS); diff --git a/tf2_geometry_msgs/test/test_tomsg_frommsg.cpp b/tf2_geometry_msgs/test/test_tomsg_frommsg.cpp index 3139f234d..7285e1aa3 100644 --- a/tf2_geometry_msgs/test/test_tomsg_frommsg.cpp +++ b/tf2_geometry_msgs/test/test_tomsg_frommsg.cpp @@ -35,6 +35,13 @@ static const double EPS = 1e-6; +// check whether address of return value is identical to parameter b +template +void check_toMsg(const A& a, B& b) { + B& b_ret = tf2::toMsg(a, b); + EXPECT_EQ(&b_ret, &b); +} + tf2::Vector3 get_tf2_vector() { return tf2::Vector3(1.0, 2.0, 3.0); @@ -268,7 +275,8 @@ TEST(tf2_geometry_msgs, Vector3) fromMsg(m1, v1); SCOPED_TRACE("m1 v1"); expect_near(m1, v1); - geometry_msgs::Vector3 m2 = toMsg(v1); + geometry_msgs::Vector3 m2; + check_toMsg(v1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } @@ -281,7 +289,8 @@ TEST(tf2_geometry_msgs, Point) SCOPED_TRACE("m1 v1"); fromMsg(m1, v1); expect_near(m1, v1); - geometry_msgs::Point m2 = toMsg(v1, m2); + geometry_msgs::Point m2; + check_toMsg(v1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } @@ -294,7 +303,8 @@ TEST(tf2_geometry_msgs, Quaternion) SCOPED_TRACE("m1 q1"); fromMsg(m1, q1); expect_near(m1, q1); - geometry_msgs::Quaternion m2 = toMsg(q1); + geometry_msgs::Quaternion m2; + check_toMsg(q1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } @@ -307,7 +317,8 @@ TEST(tf2_geometry_msgs, Pose) fromMsg(m1, t1); SCOPED_TRACE("m1 t1"); expect_near(m1, t1); - geometry_msgs::Pose m2 = toMsg(t1, m2); + geometry_msgs::Pose m2; + check_toMsg(t1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } @@ -320,7 +331,8 @@ TEST(tf2_geometry_msgs, Transform) fromMsg(m1, t1); SCOPED_TRACE("m1 t1"); expect_near(m1, t1); - geometry_msgs::Transform m2 = toMsg(t1); + geometry_msgs::Transform m2; + check_toMsg(t1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } @@ -334,7 +346,7 @@ TEST(tf2_geometry_msgs, Vector3Stamped) SCOPED_TRACE("m1 v1"); // expect_near(m1, v1); geometry_msgs::Vector3Stamped m2; - m2 = toMsg(v1); + check_toMsg(v1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } @@ -348,7 +360,7 @@ TEST(tf2_geometry_msgs, PointStamped) SCOPED_TRACE("m1 v1"); // expect_near(m1, v1); //TODO implement cross verification explicityly geometry_msgs::PointStamped m2; - m2 = toMsg(v1, m2); + check_toMsg(v1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } @@ -362,7 +374,7 @@ TEST(tf2_geometry_msgs, QuaternionStamped) SCOPED_TRACE("m1 v1"); // expect_near(m1, v1); //TODO implement cross verification explicityly geometry_msgs::QuaternionStamped m2; - m2 = tf2::toMsg(v1); + check_toMsg(v1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } @@ -376,7 +388,7 @@ TEST(tf2_geometry_msgs, PoseStamped) fromMsg(m1, v1); // expect_near(m1, v1); //TODO implement cross verification explicityly geometry_msgs::PoseStamped m2; - m2 = tf2::toMsg(v1, m2); + check_toMsg(v1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } @@ -390,7 +402,7 @@ TEST(tf2_geometry_msgs, TransformStamped) SCOPED_TRACE("m1 v1"); // expect_near(m1, v1); geometry_msgs::TransformStamped m2; - m2 = tf2::toMsg(v1); + check_toMsg(v1, m2); SCOPED_TRACE("m1 m2"); expect_near(m1, m2); } diff --git a/tf2_kdl/include/tf2_kdl/tf2_kdl.h b/tf2_kdl/include/tf2_kdl/tf2_kdl.h index e1f28bfb1..75e7013db 100644 --- a/tf2_kdl/include/tf2_kdl/tf2_kdl.h +++ b/tf2_kdl/include/tf2_kdl/tf2_kdl.h @@ -32,7 +32,8 @@ #ifndef TF2_KDL_H #define TF2_KDL_H -#include +#include +#include #include #include #include @@ -74,7 +75,7 @@ geometry_msgs::TransformStamped kdlToTransform(const KDL::Frame& k) // Vector // --------------------- /** \brief Apply a geometry_msgs TransformStamped to an KDL-specific Vector type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The vector to transform, as a timestamped KDL Vector data type. * \param t_out The transformed vector, as a timestamped KDL Vector data type. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -87,14 +88,15 @@ inline } /** \brief Convert a stamped KDL Vector type to a PointStamped message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The timestamped KDL Vector to convert. - * \return The vector converted to a PointStamped message. + * \param msg The vector converted to a PointStamped message. + * \return Reference to \c msg parameter. */ +template <> inline -geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) +geometry_msgs::PointStamped& toMsg(const tf2::Stamped& in, geometry_msgs::PointStamped& msg) { - geometry_msgs::PointStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.point.x = in[0]; @@ -104,10 +106,11 @@ geometry_msgs::PointStamped toMsg(const tf2::Stamped& in) } /** \brief Convert a PointStamped message type to a stamped KDL-specific Vector type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h * \param msg The PointStamped message to convert. * \param out The point converted to a timestamped KDL Vector. */ +template <> inline void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& out) { @@ -122,7 +125,7 @@ void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped& // Twist // --------------------- /** \brief Apply a geometry_msgs TransformStamped to an KDL-specific Twist type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The twist to transform, as a timestamped KDL Twist data type. * \param t_out The transformed Twist, as a timestamped KDL Frame data type. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -135,14 +138,15 @@ inline } /** \brief Convert a stamped KDL Twist type to a TwistStamped message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The timestamped KDL Twist to convert. - * \return The twist converted to a TwistStamped message. + * \param msg The twist converted to a TwistStamped message. + * \return Reference to \c msg parameter. */ +template <> inline -geometry_msgs::TwistStamped toMsg(const tf2::Stamped& in) +geometry_msgs::TwistStamped& toMsg(const tf2::Stamped& in, geometry_msgs::TwistStamped& msg) { - geometry_msgs::TwistStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.twist.linear.x = in.vel[0]; @@ -155,10 +159,11 @@ geometry_msgs::TwistStamped toMsg(const tf2::Stamped& in) } /** \brief Convert a TwistStamped message type to a stamped KDL-specific Twist type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h * \param msg The TwistStamped message to convert. * \param out The twist converted to a timestamped KDL Twist. */ +template <> inline void fromMsg(const geometry_msgs::TwistStamped& msg, tf2::Stamped& out) { @@ -177,7 +182,7 @@ void fromMsg(const geometry_msgs::TwistStamped& msg, tf2::Stamped& o // Wrench // --------------------- /** \brief Apply a geometry_msgs TransformStamped to an KDL-specific Wrench type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The wrench to transform, as a timestamped KDL Wrench data type. * \param t_out The transformed Wrench, as a timestamped KDL Frame data type. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -190,14 +195,15 @@ inline } /** \brief Convert a stamped KDL Wrench type to a WrenchStamped message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The timestamped KDL Wrench to convert. - * \return The wrench converted to a WrenchStamped message. + * \param msg The wrench converted to a WrenchStamped message. + * \return Reference to \c msg parameter. */ +template <> inline -geometry_msgs::WrenchStamped toMsg(const tf2::Stamped& in) +geometry_msgs::WrenchStamped& toMsg(const tf2::Stamped& in, geometry_msgs::WrenchStamped& msg) { - geometry_msgs::WrenchStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; msg.wrench.force.x = in.force[0]; @@ -210,10 +216,11 @@ geometry_msgs::WrenchStamped toMsg(const tf2::Stamped& in) } /** \brief Convert a WrenchStamped message type to a stamped KDL-specific Wrench type. - * This function is a specialization of the fromMsg template defined in tf2/convert.h + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h * \param msg The WrenchStamped message to convert. * \param out The wrench converted to a timestamped KDL Wrench. */ +template <> inline void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped& out) { @@ -234,7 +241,7 @@ void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped& // Frame // --------------------- /** \brief Apply a geometry_msgs TransformStamped to a KDL-specific Frame data type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. + * This function is a specialization of the doTransform template defined in tf2/transform_functions.h. * \param t_in The frame to transform, as a timestamped KDL Frame. * \param t_out The transformed frame, as a timestamped KDL Frame. * \param transform The timestamped transform to apply, as a TransformStamped message. @@ -247,14 +254,15 @@ inline } /** \brief Convert a stamped KDL Frame type to a Pose message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The timestamped KDL Frame to convert. - * \return The frame converted to a Pose message. + * \param msg The frame converted to a Pose message. + * \return Reference to \c msg parameter. */ +template <> inline -geometry_msgs::Pose toMsg(const KDL::Frame& in) +geometry_msgs::Pose& toMsg(const KDL::Frame& in, geometry_msgs::Pose& msg) { - geometry_msgs::Pose msg; msg.position.x = in.p[0]; msg.position.y = in.p[1]; msg.position.z = in.p[2]; @@ -263,10 +271,11 @@ geometry_msgs::Pose toMsg(const KDL::Frame& in) } /** \brief Convert a Pose message type to a KDL Frame. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h. * \param msg The Pose message to convert. * \param out The pose converted to a KDL Frame. */ +template <> inline void fromMsg(const geometry_msgs::Pose& msg, KDL::Frame& out) { @@ -277,25 +286,27 @@ void fromMsg(const geometry_msgs::Pose& msg, KDL::Frame& out) } /** \brief Convert a stamped KDL Frame type to a Pose message. - * This function is a specialization of the toMsg template defined in tf2/convert.h. + * This function is a specialization of the toMsg template defined in tf2/transform_functions.h. * \param in The timestamped KDL Frame to convert. - * \return The frame converted to a PoseStamped message. + * \param msg The frame converted to a PoseStamped message. + * \return Reference to \c msg parameter. */ +template <> inline -geometry_msgs::PoseStamped toMsg(const tf2::Stamped& in) +geometry_msgs::PoseStamped& toMsg(const tf2::Stamped& in, geometry_msgs::PoseStamped& msg) { - geometry_msgs::PoseStamped msg; msg.header.stamp = in.stamp_; msg.header.frame_id = in.frame_id_; - msg.pose = toMsg(static_cast(in)); + toMsg(static_cast(in), msg.pose); return msg; } /** \brief Convert a Pose message transform type to a stamped KDL Frame. - * This function is a specialization of the fromMsg template defined in tf2/convert.h. + * This function is a specialization of the fromMsg template defined in tf2/transform_functions.h. * \param msg The PoseStamped message to convert. * \param out The pose converted to a timestamped KDL Frame. */ +template <> inline void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped& out) { diff --git a/tf2_kdl/test/test_tf2_kdl.cpp b/tf2_kdl/test/test_tf2_kdl.cpp index a0fdfd160..5d806e3d7 100644 --- a/tf2_kdl/test/test_tf2_kdl.cpp +++ b/tf2_kdl/test/test_tf2_kdl.cpp @@ -30,6 +30,7 @@ /** \author Wim Meeussen */ +#include #include #include #include diff --git a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h index 9e16e0d2e..bb5f05b6b 100644 --- a/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h +++ b/tf2_sensor_msgs/include/tf2_sensor_msgs/tf2_sensor_msgs.h @@ -30,7 +30,7 @@ #ifndef TF2_SENSOR_MSGS_H #define TF2_SENSOR_MSGS_H -#include +#include #include #include #include @@ -44,7 +44,7 @@ namespace tf2 /********************/ /** \brief Extract a timestamp from the header of a PointCloud2 message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. + * This function is a specialization of the getTimestamp template defined in tf2/transform_functions.h. * \param t PointCloud2 message to extract the timestamp from. * \return The timestamp of the message. The lifetime of the returned reference * is bound to the lifetime of the argument. @@ -54,7 +54,7 @@ inline const ros::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return p.header.stamp;} /** \brief Extract a frame ID from the header of a PointCloud2 message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. + * This function is a specialization of the getFrameId template defined in tf2/transform_functions.h. * \param t PointCloud2 message to extract the frame ID from. * \return A string containing the frame ID of the message. The lifetime of the * returned reference is bound to the lifetime of the argument. @@ -91,11 +91,16 @@ void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 *z_out = point.z(); } } + +template <> inline -sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in) +sensor_msgs::PointCloud2& toMsg(const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2& out) { - return in; + out = in; + return out; } + +template <> inline void fromMsg(const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out) {