Skip to content
This repository was archived by the owner on May 31, 2025. It is now read-only.

Conversation

@gleichdick
Copy link

@gleichdick gleichdick commented Dec 8, 2019

Overview

At the moment, when you use tf2::convert() with two types which are not from geometry_msgs package (i.e. tf2::Vector3 and Eigen::Vector3d) linking errors can occur. See #430 for details.

This PR changes all overloads of fromMsg() and toMsg() to be template specialisations so that the correct overload is found by tf2::convert() independ of the header include order.

A Python script creates mappings between non-msgs types with forward declarations, an example of its output can be found here. The script runs with Python 2 and 3.
These mappings are picked up by tf2::convert() with some SFINAE magic, if no mapping is found a nice error message is shown:

In file included from /ws_moveit/src/geometry2/tf2/include/tf2/convert.h:36:0,
                 from /ws_moveit/src/geometry2/tf2/test/three_way_convert.cpp:31:
/ws_moveit/src/geometry2/tf2/include/tf2/impl/convert.h: In instantiation of ‘void tf2::impl::convertViaMessage(const A&, B&, typename std::enable_if<tf2::impl::has_no_common_msgs<A, B>::value, void*>::type) [with A = Bar::Vec3; B = Baz::Vec3; typename std::enable_if<tf2::impl::has_no_common_msgs<A, B>::value, void*>::type = void*]’:
/ws_moveit/src/geometry2/tf2/include/tf2/impl/convert.h:151:20:   required from ‘static void tf2::impl::Converter<IS_MESSAGE_A, IS_MESSAGE_B>::convert(const A&, B&) [with A = Bar::Vec3; B = Baz::Vec3; bool IS_MESSAGE_A = false; bool IS_MESSAGE_B = false]’
/ws_moveit/src/geometry2/tf2/include/tf2/convert.h:51:113:   required from ‘void tf2::convert(const A&, B&) [with A = Bar::Vec3; B = Baz::Vec3]’
/ws_moveit/src/geometry2/tf2/test/three_way_convert.cpp:213:22:   required from here
/ws_moveit/src/geometry2/tf2/include/tf2/impl/convert.h:110:3: error: static assertion failed: Please add a tf2::BidirectionalTypeMap or tf2::UnidirectionalTypeMap specialisation for types A and B.
   static_assert(! has_no_common_msgs<A, B>::value,

A typemap looks like

template<>
struct BidirectionalTypeMap<Foo::Vec3, Bar::Vec3> {
  using type = geometry_msgs::Vector3;
};

template<>
struct UnidirectionalTypeMap<Foo::Vec3, Baz::Vec3> {
  using type = geometry_msgs::Vector3;
};

Caveats

The signature of toMsg is changed, this will break user code.
The generated TypeMaps will be included in <tf2/convert.h>, so tf2::Vector3, KDL::Frame and friends will always be forwad defined when including <tf2/convert.h>. So maybe the TypeMaps should be split into separate headers.

Things to be done

  • Doxygen
  • Further tests for three way conversions
  • Commit history cleanup
  • Adapt wiki (e.g. new fromMsg/toMsg signature, how to extend three way converting)

Related PRs/Issues

#430
#431
moveit/moveit#1785
moveit/moveit#1794

@gleichdick gleichdick changed the title WIP: Fix tf2::convert for non-msgs types Fix tf2::convert for non-msgs types Jan 31, 2020
Bjar Ne added 5 commits February 3, 2020 23:30
Also added a Vector3Stamped to Eigen::Vector3d conversion
All fromMsg/toMsg overloads are adapted to the new signatures.
Further simplifications (e.g. reusing functions for stamped equivalents)
are made and identity conversations are removed.
tf2::convert() now uses the new signature of toMsg(),
two non-Message types can be converted via a common message type.
If no common message type was defined, a compiler error will show up.
Bjar Ne added 2 commits February 5, 2020 00:05
A python script (run by CMake) will create a header file at build time
which contains known mappings of message types to non-message types to
make conversions between two non-message (like KDL::Vector and
Eigen::Vector3d) possible.
@gleichdick
Copy link
Author

Replaced by #491

@gleichdick gleichdick closed this Dec 11, 2020
Sign up for free to subscribe to this conversation on GitHub. Already have an account? Sign in.

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant