Migration to cartographer 2.0 library and proper ros2 implementation by doisyg · Pull Request #59 · ros2/cartographer_ros

doisyg · 2022-03-04T10:52:26Z

As discussed in length here: cartographer-project#1622

…r-project#893) * remove architecture specific definitions exported by PCL This is an issue on PCL 1.8.X causing SIGILL, Illegal instruction crashes: ros-gbp/cartographer_ros-release#10 Should be fixed in future PCL version with PointCloudLibrary/pcl#2100

…-project#891) Fixes the problem of ever-growing memory after `rosbag play --clock` finishes, as discussed in cartographer-project/cartographer#1182 The wall timers caused the timer callback that publishes TF data to be called even if no simulated `/clock` was published anymore. As the TF buffer cache time of the TF listener seems to be based on the ROS time instead of wall clock, it could grow out of bounds. Now, `ros::Timer` plays nicely with both normal (wall) and simulated time and no callbacks are executed if `/clock` stops in simulation.

…ect#896)

In cartographer we check for strict ordering, i.e. do not allow subsequent timestamps to be exactly equal. This fixes the rosbag validation tool to do the same.

…mers. (cartographer-project#898)

Prevents confusion with TrajectoryState and GetTrajectoryStates() of the pose graph interface. The affected data is only local.

…apher-project#910) cartographer-project/rfcs#35 - makes use of the trajectory state in `map_builder` and `node` - adds a service to query the trajectory states - follow-up to cartographer-project/cartographer#1214 that takes the deleted state into account in the `/finish_trajectory` service (could crash otherwise)

…project#912) We don't need it after the occupancy grid is drawn. Reduces the memory consumption especially for large maps.

…ographer-project#899)

- minimal counter, gauge and histogram implementations - metric family interfaces as defined in libcartographer - serializable to ROS messages RFC: cartographer-project/rfcs#26

…er-project#917) [RFC 24](https://github.com/googlecartographer/rfcs/blob/master/text/0024-monitoring-ros.md) Public API: - adds `cartographer_ros::metrics::FamilyFactory` - compatible with `::cartographer::metrics::RegisterAllMetrics` Public RPC interface: - adds the ROS service `/read_metrics` - response contains the latest values of all available metric families

This PR adds additional bookkeeping for trajectories that we scheduled for finishing. In Node::RunFinalOptimization(...), we were calling FinishTrajectory for every active trajectory (state == ACTIVE). Since the state only gets updated once the corresponding worker for the FinishTrajectory task is scheduled, we were potentially calling FinishTrajectory twice for a single trajectory id. Reproducible on master e.g. with ``` roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=b2-2016-02-02-14-01-56.bag no_rviz:=true ```

To compare different SLAM software online, it is necessary to disable tf broadcast. Because we already have a parameter "pose_publish_period_sec", we use a zero value here to turn off tf broadcast.

…rapher-project#927) `DIRECTORY` by itself doesn't work as reliable as expected.

Use mutex locker instead of atomic operations in Gauge. Remove unnecessary constructor overload from Histogram.

FIXES=cartographer-project#929

This adds a .clang-format file, so that git clang-format uses Google style without the need to remember the commandline flag. Similar to cartographer-project/cartographer#1249.

)

`std::bind` is bug prone and should be avoided. Lambdas are a more general and safer replacement. Similar to cartographer-project/cartographer#1261.

This is useful for tuning/debugging to rule out (simulated) time issues (because published pose will then only depend on header times). Another use case is when Cartographer runs on a separate machine that has a different system clock than the sensors.

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

clalancette · 2022-03-28T21:03:53Z

This compiled for me after installing all dependencies. I do have a few pieces of feedback:

I'm going to suggest we put COLCON_IGNORE on cartographer_ros_docs. It doesn't have an install target, so what will be built and installed won't really be useful at the moment. Besides that, I'm not at all convinced that the ROS 2 buildfarm will be able to do the right thing. If we want to mess around with it later, we can, but I think we should start without it for now.
I'm getting the following warning when building cartographer_ros on Ubuntu 22.04:

CMake Warning (dev) at /usr/share/cmake-3.22/Modules/FindPackageHandleStandardArgs.cmake:438 (message):
  The package name passed to `find_package_handle_standard_args` (Lua) does
  not match the name of the calling package (LuaGoogle).  This can lead to
  problems in calling code that expects `find_package` result variables
  (e.g., `_FOUND`) to follow a certain pattern.
Call Stack (most recent call first):
  /home/ubuntu/cart_ws/install/cartographer/share/cartographer/cmake/modules/FindLuaGoogle.cmake:211 (FIND_PACKAGE_HANDLE_STANDARD_ARGS)
  CMakeLists.txt:48 (find_package)
This warning is for project developers.  Use -Wno-dev to suppress it.

Can you look into cleaning that up, and/or suppressing it?

I'm also getting the following warning when compiling cartographer_ros on Ubuntu 22.04:

/home/ubuntu/cart_ws/src/cartographer_ros/cartographer_ros/src/node.cpp: In member function ‘void cartographer_ros::Node::PublishLocalTrajectoryData()’:
/home/ubuntu/cart_ws/src/cartographer_ros/cartographer_ros/src/node.cpp:265:59: warning: loop variable ‘point’ creates a copy from type ‘const cartographer::sensor::RangefinderPoint’ [-Wrange-loop-construct]
  265 |         for (const cartographer::sensor::RangefinderPoint point :
      |                                                           ^~~~~
/home/ubuntu/cart_ws/src/cartographer_ros/cartographer_ros/src/node.cpp:265:59: note: use reference type to prevent copying
  265 |         for (const cartographer::sensor::RangefinderPoint point :
      |                                                           ^~~~~
      |

Can you take a look and fix it?

Once we have those cleaned up, I think we can start thinking about merging this.

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

…UIRED)` Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

doisyg · 2022-03-29T10:31:13Z

This compiled for me after installing all dependencies. I do have a few pieces of feedback:

I'm going to suggest we put COLCON_IGNORE on cartographer_ros_docs. It doesn't have an install target, so what will be built and installed won't really be useful at the moment. Besides that, I'm not at all convinced that the ROS 2 buildfarm will be able to do the right thing. If we want to mess around with it later, we can, but I think we should start without it for now.

COLCON_IGNORE added.

I'm getting the following warning when building cartographer_ros on Ubuntu 22.04:

CMake Warning (dev) at /usr/share/cmake-3.22/Modules/FindPackageHandleStandardArgs.cmake:438 (message):
  The package name passed to `find_package_handle_standard_args` (Lua) does
  not match the name of the calling package (LuaGoogle).  This can lead to
  problems in calling code that expects `find_package` result variables
  (e.g., `_FOUND`) to follow a certain pattern.
Call Stack (most recent call first):
  /home/ubuntu/cart_ws/install/cartographer/share/cartographer/cmake/modules/FindLuaGoogle.cmake:211 (FIND_PACKAGE_HANDLE_STANDARD_ARGS)
  CMakeLists.txt:48 (find_package)
This warning is for project developers.  Use -Wno-dev to suppress it.

Fixed by changing find_package(LuaGoogle REQUIRED) for find_package(Lua REQUIRED). I guess there is the same issue/warning for cartographer compilation

Can you look into cleaning that up, and/or suppressing it?

I'm also getting the following warning when compiling cartographer_ros on Ubuntu 22.04:

/home/ubuntu/cart_ws/src/cartographer_ros/cartographer_ros/src/node.cpp: In member function ‘void cartographer_ros::Node::PublishLocalTrajectoryData()’:
/home/ubuntu/cart_ws/src/cartographer_ros/cartographer_ros/src/node.cpp:265:59: warning: loop variable ‘point’ creates a copy from type ‘const cartographer::sensor::RangefinderPoint’ [-Wrange-loop-construct]
  265 |         for (const cartographer::sensor::RangefinderPoint point :
      |                                                           ^~~~~
/home/ubuntu/cart_ws/src/cartographer_ros/cartographer_ros/src/node.cpp:265:59: note: use reference type to prevent copying
  265 |         for (const cartographer::sensor::RangefinderPoint point :
      |                                                           ^~~~~
      |

Fixed

clalancette · 2022-03-29T12:47:46Z

OK, so this now compiles for me and runs the tests, so I think this is almost ready to go. Thanks for the fixes so far.

That said, I can't actually merge this right now, because there are conflicts with the ros2 branch. I don't want to hard-reset the ros2 branch, since that has all of the history of cartographer_ros in ROS 2 up to this point. So I think the thing to do here is to make a merge commit merging this code into the ros2 branch. There are definitely conflicts that will need to be resolved, but I think it should be relatively easy to resolve them; just always take the "new" code. Once that is all done, I can go ahead and merge this, and then go through the release process.

doisyg · 2022-03-29T13:46:33Z

That said, I can't actually merge this right now, because there are conflicts with the ros2 branch. I don't want to hard-reset the ros2 branch, since that has all of the history of cartographer_ros in ROS 2 up to this point. So I think the thing to do here is to make a merge commit merging this code into the ros2 branch. There are definitely conflicts that will need to be resolved, but I think it should be relatively easy to resolve them; just always take the "new" code. Once that is all done, I can go ahead and merge this, and then go through the release process.

I understand the need for keeping this branch ros2 history, but I believe it should be the other way around: hard resetting this repo ros2 branch and cherry picking few commits if needed (I don't believe there is any actually); OR creating a new branch ros2_2.0 (indicating that it is a wrapper for the v2.0 of the cartographer library).
The reason is that this PR contains the history of all the developments and improvements that were done in ros1 when the cartographer lib improved and that were never ported/synced to this repo ros2 branch (more than 100 commits!). My PR work begins after these 100+ commits.
In addition to that, this PR contains the proper porting to ros2 of most of the executable whereas this repo ros2 branch is just one executable + some urgent fixes.

clalancette · 2022-03-29T13:51:26Z

I understand the need for keeping this branch ros2 history, but I believe it should be the other way around: hard resetting this repo ros2 branch and cherry picking few commits if needed (I don't believe there is any actually);

But I don't want to lose the history, so this isn't an option.

OR creating a new branch ros2_2.0 (indicating that it is a wrapper for the v2.0 of the cartographer library).

We could do that, but I really don't see the need to. There are already enough branches in these repositories that people get confused as it is, I don't really want to add to that if we don't need to.

The reason is that this PR contains the history of all the developments and improvements that were done in ros1 when the cartographer lib improved and that were never ported/synced to this repo ros2 branch (more than 100 commits!). My PR work begins after these 100+ commits.

That's the beautiful thing about merge commits; all of that history will be preserved on one "side" of the merge. The other side of the merge will contain all of the development that happened here in this fork, so we don't need to lose any of that.

doisyg · 2022-03-29T13:59:05Z

Ok, I am trying to do a merge (43+ conflicts for now), but I am a bit reluctant as it won't be as rigorously tested as the non merged version (but yes, always taking the newest version should do the trick)

clalancette · 2022-03-29T14:02:19Z

Ok, I am trying to do a merge (43+ conflicts for now), but I am a bit reluctant as it won't be as rigorously tested as the non merged version (but yes, always taking the newest version should do the trick)

I found a good trick to greatly increase confidence in it. Checkout your current, well-tested code to another branch (I'll just call it rolling-working). Do the merge (git checkout rolling ; git merge ros2). Fix conflicts, commit the result. Then do a diff between your original branch and the one you've just merged together: git diff rolling-working..rolling. If there are no differences, then you are done. If there are differences, then somehow the merge wasn't right, rinse and repeat.

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

doisyg · 2022-03-31T10:23:24Z

It should be all right since my last push ed85808

clalancette · 2022-03-31T13:07:22Z

It should be all right since my last push ed85808

Yep, it looks good to me! Thanks for the hard work here. I'm going to go ahead and merge this, and then do a separate PR to setup release-related things (like update the package.xml versions and such).

clalancette · 2022-03-31T13:20:27Z

@doisyg Hm, while looking at what version number to call this, I realized that upstream https://github.com/cartographer-project/cartographer_ros hasn't done a tag since 1.0 in 2018 . So I'm not quite sure what I should tag this as. My initial thought is to tag it as something like 2.0.9000 (to go along with the version scheme in https://github.com/ros2/cartographer), but since there is no upstream 2.0.0 tag the same rules don't apply. What are your thoughts on versioning this?

doisyg · 2022-03-31T13:38:21Z

Hm I don't really, the whole situation is super confusing. We want to point out that this wrapper is for ros2 (as opposite to ros1) AND that it is targeting the version 2.0 of the cartographer library (as opposite to the current released ros2 foxy/galactic wrapper that is targeting the version 1.0 of the cartographer library AND in a similar way as the released ros1 noetic wrapper that is targeting the version 2.0 of the cartographer library).

clalancette · 2022-03-31T14:02:32Z

Yeah, it's a tough call.

Since we don't really have any movement upstream, I'm going to go ahead and make the assumption that the 1.x series of tags upstream is for ROS 1, and the 2.x series of tags will be for ROS 2. That leads me to use 2.0.9000 for these changes. If upstream comes alive again and chooses something different, we can always re-evaluate the situation.

I'll add a copious comment in the package.xml explaining this decision.

doisyg · 2022-03-31T14:04:36Z

That sounds like a good compromise, thanks for facilitating this !

ros-discourse · 2022-04-16T13:22:30Z

This pull request has been mentioned on ROS Discourse. There might be relevant details there:

https://discourse.ros.org/t/rolling-and-soon-humble-release-of-both-cartographer-and-cartographer-ros-v2-and-call-for-testing/25137/1

mikaelarguedas and others added 30 commits May 30, 2018 19:17

Add maintainers and authors to package.xml (cartographer-project#886)

c4ac76a

Release 1.0. (cartographer-project#889)

42b2763

set required version of dependencies (cartographer-project#892)

6b473e3

Use timing channel from PointCloud2, if available. (cartographer-proj…

82b5eb9

…ect#896)

Ensure we validate what we CHECK(...) (cartographer-project#897)

4f32f88

In cartographer we check for strict ordering, i.e. do not allow subsequent timestamps to be exactly equal. This fixes the rosbag validation tool to do the same.

Revert timers other than PublishTrajectoryStates back to being WallTi…

424e702

…mers. (cartographer-project#898)

Improve internal naming of local SLAM data. (cartographer-project#908)

c560d05

Prevents confusion with TrajectoryState and GetTrajectoryStates() of the pose graph interface. The affected data is only local.

Discard proto data in pbstream_map_publisher via RAII. (cartographer-…

0e65aa5

…project#912) We don't need it after the occupancy grid is drawn. Reduces the memory consumption especially for large maps.

Allow to ignore (un-)frozen submaps in the occupancy grid node. (cart…

f5b583f

…ographer-project#899)

Add internal metric families. (cartographer-project#914)

237dd83

- minimal counter, gauge and histogram implementations - metric family interfaces as defined in libcartographer - serializable to ROS messages RFC: cartographer-project/rfcs#26

Use new pure localization trimmer options. (cartographer-project#918)

bb4aec3

Minor grammar fix. (cartographer-project#900)

0beb972

Update msg_conversion.cc (cartographer-project#925)

bfe0d01

Use 'landmarks' instead of 'landmark'. (cartographer-project#931)

944b225

Allow zero pose_publish_period (cartographer-project#933)

427fbe6

To compare different SLAM software online, it is necessary to disable tf broadcast. Because we already have a parameter "pose_publish_period_sec", we use a zero value here to turn off tf broadcast.

List files explicitly in cartographer_ros_msgs/CMakeLists.txt (cartog…

168f4cc

…rapher-project#927) `DIRECTORY` by itself doesn't work as reliable as expected.

Follow cartographer-project/cartographer#1241 (cartographer-project#923)

e07c226

remove unused declaration (cartographer-project#934)

c157164

Simplify gauge and histogram implementation. (cartographer-project#922)

fb5b16d

Use mutex locker instead of atomic operations in Gauge. Remove unnecessary constructor overload from Histogram.

Warn for possible topic mismatch (cartographer-project#935)

54e8aa4

FIXES=cartographer-project#929

Introduce value converter tables. (cartographer-project#937)

6831292

Add .clang-format file. (cartographer-project#938)

0572ebb

This adds a .clang-format file, so that git clang-format uses Google style without the need to remember the commandline flag. Similar to cartographer-project/cartographer#1249.

Move conversion table to LoadOccupancyGridMap. (cartographer-project#941

a90e995

)

Get rid of std::bind. (cartographer-project#939)

b9af636

`std::bind` is bug prone and should be avoided. Lambdas are a more general and safer replacement. Similar to cartographer-project/cartographer#1261.

Guillaume Doisy added 7 commits January 13, 2022 16:13

all bags launch files

cb2e96b

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

fix assets_writer for MultiEchoLaserScan and PointCloud2

5e04306

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

assets writer launch conversion

d9daae8

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

args for asserts writer backpack 3d

446f5a3

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

change folder structure to standard ros2

c91cac5

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

fix obsolete eigen header

9681e02

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

comment buggy orientation conversion log

d014100

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

doisyg mentioned this pull request Mar 4, 2022

ROS2 rolling/galactic conversion cartographer-project/cartographer_ros#1622

Open

visualize pbstream launch

f235c55

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

Guillaume Doisy added 3 commits March 29, 2022 10:50

colcon ignore

53997c3

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

fix warning "265:59: note: use reference type to prevent copying"

48eab66

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

changing find_package(LuaGoogle REQUIRED) for `find_package(Lua REQ…

d2af956

…UIRED)` Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

Merge remote-tracking branch 'ros2/ros2' into rolling

ed85808

Signed-off-by: Guillaume Doisy <guillaume.doisy@wyca.fr>

clalancette approved these changes Mar 31, 2022

View reviewed changes

clalancette merged commit 4220d07 into ros2:ros2 Mar 31, 2022

clalancette mentioned this pull request Mar 31, 2022

[WIP] Sync dashing branch with upstream master changes #44

Closed

98 tasks

Conversation

doisyg commented Mar 4, 2022

Uh oh!

clalancette commented Mar 28, 2022

Uh oh!

doisyg commented Mar 29, 2022

Uh oh!

clalancette commented Mar 29, 2022

Uh oh!

doisyg commented Mar 29, 2022

Uh oh!

clalancette commented Mar 29, 2022

Uh oh!

doisyg commented Mar 29, 2022

Uh oh!

clalancette commented Mar 29, 2022

Uh oh!

doisyg commented Mar 31, 2022

Uh oh!

clalancette commented Mar 31, 2022

Uh oh!

clalancette commented Mar 31, 2022

Uh oh!

doisyg commented Mar 31, 2022

Uh oh!

clalancette commented Mar 31, 2022

Uh oh!

doisyg commented Mar 31, 2022

Uh oh!

ros-discourse commented Apr 16, 2022

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

20 participants