Fixes to make tf2_eigen work with ROS2. by clalancette · Pull Request #1 · ros2/geometry2

clalancette · 2017-03-22T18:11:48Z

This isn't necessarily the prettiest commit, and things might be done better. In particular, I'm not at all sure that the time conversions I did in tf2_eigen.h are correct, so feedback for that is welcome.

Signed-off-by: Chris Lalnacette <clalancette@osrfoundation.org>

Karsten1987

Please verify the Eigen include on Windows. I do believe that you may have to adjust the include dirs.
Also you may want to use rclcpp::Time::now() to get rid of this verbose chrono code.

Karsten1987 · 2017-03-22T18:33:41Z

tf2_eigen/CMakeLists.txt

+
+find_package(Eigen3 REQUIRED)
 include_directories(${EIGEN_INCLUDE_DIRS})
 add_definitions(${EIGEN_DEFINITIONS})


Does this compile on Windows?
@nuclearsandwich might be able to adjust the choco package in case.

I have no clue. I didn't even consider Windows right now. One thing I could do would be to do a "soft" lookup of Eigen3, and then fall back to finding Eigen if that fails (I do this in some of my ROS1 packages). Do you think that would make it work better on Windows?

you can look at what we did for orocos_kdl. I am not saying that this is the way to go, but it works:
https://github.com/ros2/orocos_kinematics_dynamics/blob/ros2/orocos_kdl/CMakeLists.txt

The cmake variables for Eigen are just a bit special :)

Right, this is similar in spirit to what I do in my ROS1 packages. However, it seems to require that we carry around a "FindEigen3.cmake" module in every project. Do we want to find a common place for that? In ROS1, that seems to be cmake_modules.

Karsten1987 · 2017-03-22T18:34:57Z

tf2_eigen/include/tf2_eigen/tf2_eigen.h

+  geometry_msgs::msg::PointStamped msg;
+  auto nanosecs = std::chrono::duration_cast<std::chrono::nanoseconds>(in.stamp_.time_since_epoch());
+  msg.header.stamp.sec = nanosecs.count() / 1000000000LL;
+  msg.header.stamp.nanosec = (nanosecs.count() % 1000000000LL) * 1000000000LL;


you can use rclcpp::Time::now() for this block

Actually, I don't think I can. I don't want the time now; I actually want the time from the tf2::Stamped object, but converted into a form that the msg.header.stamp understands. Can I still use rclcpp::Time for this?

see general comment.

Yeah, this isn't "now". @dhood had a cleaned up simplified toMsg for TimePoint at least locally that would make this cleaner.

Karsten1987 · 2017-03-22T18:36:22Z

tf2_eigen/include/tf2_eigen/tf2_eigen.h

+  std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> converted{std::chrono::nanoseconds{msg.header.stamp.sec*1000000000LL+msg.header.stamp.nanosec}};
+  std::chrono::system_clock::time_point recovered = std::chrono::time_point_cast<std::chrono::system_clock::duration>(converted);
+
+  out.stamp_ = recovered;


same as above. have a look at rclcpp::Time::now(). That may ease up the code.

mikaelarguedas

Looks good to me except a few comments that I think need to be addressed:

It would be great if you can port the test rather than disable it.
Should this leverage timestamps and rclcpp::Time rather than using system_time arbitrarily?
Do you have any CI jobs showing that it builds on all platforms?

mikaelarguedas · 2017-03-22T18:22:58Z

tf2/include/tf2/convert.h

+/*   { */
+/*     //printf("In double type convert\n"); */
+/*     impl::Converter<ros::message_traits::IsMessage<A>::value, ros::message_traits::IsMessage<B>::value>::convert(a, b); */
+/*   } */


Nit: please use either block comments for the entire block or // for single line comments

mikaelarguedas · 2017-03-22T18:26:59Z

tf2_eigen/include/tf2_eigen/tf2_eigen.h

-  msg.header.stamp = in.stamp_;
+  geometry_msgs::msg::PointStamped msg;
+  auto nanosecs = std::chrono::duration_cast<std::chrono::nanoseconds>(in.stamp_.time_since_epoch());
+  msg.header.stamp.sec = nanosecs.count() / 1000000000LL;


please use rcl NS_TO_S ans S_TO_NS for time conversion (https://github.com/ros2/rcl/blob/master/rcl/include/rcl/time.h#L28), same below

So, there is a downside to doing that, which is that geometry2 then has a dependency on rcl (which it doesn't currently). I don't really care if we make that dependency, but it is new. Thoughts?

I think that geometry2 will need a notion of ros::Time one way or another. I don't know is this should rely on the rclcpp or the rcl implementation, but I think this will end up depending on one of those. Maybe people who know more how all the Time part of ROS2 will fit together can give a more educated answer

Yeah, looking at some of the history, you are right. The initial tf2::Stamped from ROS1 used a ros::Time; when that was converted for ROS2, that's when it was changed to a std::chrono time_point. However, I see no reason not to switch it to a ROS2 builtin_interfaces::msg::Time, so I'll do that.

We may want some input from @tfoote regarding this.
I think it's been changed to chrono in order to make the library completely ROS agnostic. So maybe re-adding a notion of ros time here is not the way to go.

Yeah, it does look as though TimePoint is used in a number of places in geometry2. I'll wait for @tfoote to comment on this, as that was his invention.

tf[2] is something that there has been a lot of requests to make completely independent. At the core library level I'd rather keep it independent of the headers and use the std::chrono datatypes at the core.

mikaelarguedas · 2017-03-22T18:27:52Z

tf2_eigen/include/tf2_eigen/tf2_eigen.h

+		 const geometry_msgs::msg::TransformStamped& transform) {
+  std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> converted{std::chrono::nanoseconds{transform.header.stamp.sec*1000000000LL+transform.header.stamp.nanosec}};
+  std::chrono::system_clock::time_point recovered = std::chrono::time_point_cast<std::chrono::system_clock::duration>(converted);
  t_out = tf2::Stamped<Eigen::Vector3d>(transformToEigen(transform) * t_in,


Nit: indent after opening bracket (same everywhere in this PR)

I'm not quite sure I understand; there are 2 spaces after the opening function bracket everywhere in this file, so I just followed that convention. What specifically do you mean?

Sorry for being unclear I meant wrapping.
In this particular case I was referring to wrapping after opening parenthesis. This way if you decide to change t_out to t_out_new you dont need to reindent the all block.

t_out = tf2::Stamped<Eigen::Vector3d>( transformToEigen(transform) * t_in, recovered, transform.header.frame_id);

My point being that if you decide to do style changes in this PR, I'd prefer them to span the entire file / package and matching the style described in the developer guide.

mikaelarguedas · 2017-03-22T18:28:31Z

tf2_eigen/package.xml

  <build_depend>eigen</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>tf2</build_depend>
  <build_depend>cmake_modules</build_depend>


I don't think you need cmake_modules anymore

Good call, probably not. I'll remove it.

mikaelarguedas · 2017-03-22T18:29:32Z

tf2_eigen/package.xml

-  <run_depend>cmake_modules</run_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>tf2</exec_depend>



please add linter tests to the package and fix all linter issues.
Or if style is not targetted by this PR please revert all style changes.
Tip: in general style changes are made in a different commit to make review easier

Yeah, I should have split the style out, sorry. The changes were small enough that I didn't, but I should have done it anyway.

For this package please don't refactor to ROS2 style. We're still maintaining this in parallel with ROS1 and if you style refactor cherry picking will be effectively stopped.

To be sure we agree on the meaning, here is what I interpret from this:

Changing the package format to format 2 (being required for ament and being supported by catkin/ROS1) should be submitted upstream and not on this fork.

Style changes should either be avoided or submitted upstream directly to avoid divergence.

mikaelarguedas · 2017-03-22T18:30:15Z

tf2_eigen/CMakeLists.txt

-
-if(CATKIN_ENABLE_TESTING)
-
-  catkin_add_gtest(tf2_eigen-test test/tf2_eigen-test.cpp)


I think we should also port the tests rather than removing them from the cmake while keeping the files

Hm, yeah, good point. I'll look at re-enabling them.

mikaelarguedas · 2017-03-22T18:48:07Z

tf2_eigen/include/tf2_eigen/tf2_eigen.h

-		 const geometry_msgs::TransformStamped& transform) {
-  t_out = tf2::Stamped<Eigen::Affine3d>(transformToEigen(transform) * t_in, transform.header.stamp, transform.header.frame_id);
+		 const geometry_msgs::msg::TransformStamped& transform) {
+  std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds> converted{std::chrono::nanoseconds{transform.header.stamp.sec*1000000000LL+transform.header.stamp.nanosec}};


any reason for not using the time from the message header as it was done before ?

No, actually, I was just re-reviewing the code, and I realized that this is a bug. I'll go back through this and make it work like it did before.

Looking back at this, yes. The problem is that all of the geometry_msgs have a header, and that header has a time. The time is a builtin_interfaces::msg::Time_ object, and tf2::Stamped wants a std::chrono::time_point object. I could also add a new constructor to tf2::Stamped that takes in a builtin_interfaces::msg::Time_ object instead; do you think that would be better?

see general comment.

Karsten1987 · 2017-03-22T20:09:00Z

I generally think that tf2::Stamped should not transfer a std::chrono object. We could better have modifications to rclcpp::Time[0] if the builtin_msg time is not sufficient. Is there a reason why not overall convey the builtin type?
https://github.com/ros2/rclcpp/blob/master/rclcpp/include/rclcpp/time.hpp

clalancette · 2017-03-22T20:13:51Z

Oh, do you mean change tf2::Stamped to use builtin_interfaces::msg::Time for its stamp_, rather than std::chrono? If so, I have no particular objection to it, I was just concerned that something else might depend on the current situation. If that's the way we want to go, though, I'm happy to change it.

mikaelarguedas · 2017-03-22T20:31:33Z

I think that tf2::Stamped uses builtin_interfaces/Time as well as every *Stamped message from geometry_msgs uses builtin_interfaces/Time for the stamp field of their headers. So I was suggesting using this everywhere.

clalancette · 2017-03-22T20:34:15Z

OK, yeah, I'm good with that. It gets rid of some fiddly code, and makes this whole thing nicer. I'm going to change that and do a full build to make sure nothing breaks.

tfoote

In general I'm not worried about geometry2 bringing in ROS dependencies but the core tf2 package is slowly removing external dependencies. It still has geometry_msgs in it's API for legacy reasons, but once that is removed or ifdefed it will be completely ROS independent.

tfoote · 2017-03-22T21:55:38Z

tf2_eigen/include/tf2_eigen/tf2_eigen.h

-  msg.header.stamp = in.stamp_;
+  geometry_msgs::msg::PointStamped msg;
+  auto nanosecs = std::chrono::duration_cast<std::chrono::nanoseconds>(in.stamp_.time_since_epoch());
+  msg.header.stamp.sec = nanosecs.count() / 1000000000LL;


tf[2] is something that there has been a lot of requests to make completely independent. At the core library level I'd rather keep it independent of the headers and use the std::chrono datatypes at the core.

tfoote · 2017-03-22T21:56:24Z

tf2_eigen/include/tf2_eigen/tf2_eigen.h

+  geometry_msgs::msg::PointStamped msg;
+  auto nanosecs = std::chrono::duration_cast<std::chrono::nanoseconds>(in.stamp_.time_since_epoch());
+  msg.header.stamp.sec = nanosecs.count() / 1000000000LL;
+  msg.header.stamp.nanosec = (nanosecs.count() % 1000000000LL) * 1000000000LL;


Yeah, this isn't "now". @dhood had a cleaned up simplified toMsg for TimePoint at least locally that would make this cleaner.

tfoote · 2017-03-22T21:58:13Z

tf2_eigen/package.xml

-  <run_depend>cmake_modules</run_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>tf2</exec_depend>



For this package please don't refactor to ROS2 style. We're still maintaining this in parallel with ROS1 and if you style refactor cherry picking will be effectively stopped.

clalancette · 2017-03-22T22:24:37Z

tf[2] is something that there has been a lot of requests to make completely independent. At the core library level I'd rather keep it independent of the headers and use the std::chrono datatypes at the core.

@tfoote The whole point of this file seems to be to convert tf2 to/from ROS2 messages, so we'll necessarily have to pull in ROS2 dependencies. Are you suggesting moving this code out from tf2, or are you just saying that you want to keep tf2::Stamped as a std::chrono, and do the conversion in tf2_eigen?

dhood · 2017-03-22T22:41:33Z

@clalancette I just opened ros/geometry2#213 which you should be able to use to convert ROS headers to and from tf2::TimePoints for stamped objects

tfoote · 2017-03-22T23:14:04Z

@tfoote The whole point of this file seems to be to convert tf2 to/from ROS2 messages, so we'll necessarily have to pull in ROS2 dependencies. Are you suggesting moving this code out from tf2, or are you just saying that you want to keep tf2::Stamped as a std::chrono, and do the conversion in tf2_eigen?

My point is not to change the core representation in tf2 as was mentioned in some of the comments. tf2_eigen can use the message and rcl dependencies if needed. But from @dhood 's ros#213 should simplify.

clalancette · 2017-03-24T18:58:38Z

So, this PR got a bit messy, and I ended up rebasing the code anyway. I'm going to close this one out, and open a new one. I'll make sure to state how I fixed all of the problems that were pointed out in this PR. Thanks!

mikaelarguedas · 2017-04-04T15:31:02Z

@clalancette can this branch be deleted?

clalancette · 2017-04-04T15:33:13Z

Yes, good call. Done now.

bringing up to date

Add vendor package files for orocos_kdl and python_orocos_kdl

Fixes to make tf2_eigen work with ROS2.

36e3575

Signed-off-by: Chris Lalnacette <clalancette@osrfoundation.org>

Karsten1987 added in progress Actively being worked on (Kanban column) in review Waiting for review (Kanban column) and removed in progress Actively being worked on (Kanban column) labels Mar 22, 2017

Karsten1987 reviewed Mar 22, 2017

View reviewed changes

mikaelarguedas requested changes Mar 22, 2017

View reviewed changes

tfoote reviewed Mar 22, 2017

View reviewed changes

clalancette closed this Mar 24, 2017

clalancette removed the in review Waiting for review (Kanban column) label Mar 24, 2017

clalancette mentioned this pull request Mar 24, 2017

ROS2 tf2-eigen Reloaded #4

Merged

clalancette deleted the ros2-tf2-eigen branch April 4, 2017 15:33

JacobJBeck pushed a commit to JacobJBeck/geometry2 that referenced this pull request Dec 8, 2018

Merge pull request ros2#1 from ros2/ros2

e5c2c1f

bringing up to date

relffok mentioned this pull request Feb 21, 2020

echo / monitor causes delay with warning messages #240

Closed

BriceRenaudeau mentioned this pull request Sep 28, 2021

TF2 Buffer crash due to timer not destructed #460

Closed

srmainwaring pushed a commit to srmainwaring/geometry2 that referenced this pull request Sep 18, 2023

Add vendor packages for orocos_kdl (ros2#1)

d444958

Add vendor package files for orocos_kdl and python_orocos_kdl


		if(CATKIN_ENABLE_TESTING)

		catkin_add_gtest(tf2_eigen-test test/tf2_eigen-test.cpp)

Conversation

clalancette commented Mar 22, 2017

Uh oh!

Karsten1987 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mikaelarguedas left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Karsten1987 commented Mar 22, 2017

Uh oh!

clalancette commented Mar 22, 2017

Uh oh!

mikaelarguedas left a comment •

edited

Loading

clalancette commented Mar 22, 2017 •

edited

Loading