ROS2 rolling/galactic conversion

[doisyg]

I needed an updated ros2 wrapper compiling against rolling/galactic, so here it is. I started from the master branch but took inspiration from https://github.com/ros2/cartographer_ros when needed.
Still a wip but the main nodes are working. Would be nice to target a galactic release.

Converted to this point (28/01/2022):

• cartographer_node, tested with a real robot for 2D mapping with a 2D lidar and wheel odometry, no IMU
• cartographer_occupancy_grid_node, working
• cartographer_offline_node, tested with a bag and same config as cartographer_node. Don't forget to start with use_sim_time to true. Tested with converted legacy museam bags
• cartographer_assets_writer, same as above
• cartographer_pbstream_map_publisher, working
• cartographer_pbstream_to_ros_map, working
• cartographer_rosbag_validate, working
• cartographer_ros_msgs package
• cartographer_rviz package
• deboostify
• review and replace RCL_SYSTEM_TIME by RCL_ROS_TIME in asset_writer
• all launch files for running demo bags

Still to do but less critical:

• update CI
• few TODOs in the code
• dev and grpc nodes
• test nodes
• Clean the CMakelists
• Unify code style (and decide for one)
• Changelogs and authors update
• fix performance issue when subscribing to /trajectory_node_list (slows down everything when enabled, specially visible in offline mode)
• remaining launch files (grpc_demo and visualize_pbstream)

Converted launch files and demo bags:

# Offline offline_backpack_2d
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
cd  ~/Downloads 
rosbags-convert cartographer_paper_deutsches_museum.bag
ros2 launch cartographer_ros offline_backpack_2d.launch.py bag_filenames:=${HOME}/Downloads/cartographer_paper_deutsches_museum

# Offline offline_backpack_3d
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/with_intensities/b3-2016-04-05-14-14-00.bag
cd ~/Downloads
rosbags-convert b3-2016-04-05-14-14-00.bag
ros2 launch cartographer_ros offline_backpack_3d.launch.py bag_filenames:=${HOME}/Downloads/b3-2016-04-05-14-14-00

# Live replay demo_backpack_2d
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
cd  ~/Downloads 
rosbags-convert cartographer_paper_deutsches_museum.bag
ros2 launch cartographer_ros demo_backpack_2d.launch.py bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum

# Live replay demo_backpack_3d
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/with_intensities/b3-2016-04-05-14-14-00.bag
cd  ~/Downloads 
rosbags-convert b3-2016-04-05-14-14-00.bag
ros2 launch cartographer_ros demo_backpack_3d.launch.py bag_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00

# Live demo_backpack_2d_localization
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-27-12-31-41.bag
cd ~/Downloads 
rosbags-convert b2-2016-04-05-14-44-52.bag
rosbags-convert b2-2016-04-27-12-31-41.bag
ros2 launch cartographer_ros offline_backpack_2d.launch.py bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52
ros2 launch cartographer_ros demo_backpack_2d_localization.launch.py load_state_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.pbstream bag_filename:=${HOME}/Downloads/b2-2016-04-27-12-31-41

# Live demo_backpack_3d_localization
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-13-54-42.bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-15-52-20.bag
cd ~/Downloads 
rosbags-convert b3-2016-04-05-13-54-42.bag
rosbags-convert b3-2016-04-05-15-52-20.bag
ros2 launch cartographer_ros offline_backpack_3d.launch.py bag_filenames:=${HOME}/Downloads/b3-2016-04-05-13-54-42
ros2 launch cartographer_ros demo_backpack_3d_localization.launch.py load_state_filename:=${HOME}/Downloads/b3-2016-04-05-13-54-42.pbstream bag_filename:=${HOME}/Downloads/b3-2016-04-05-15-52-20

# Static landmarks
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/mir/landmarks_demo_uncalibrated.bag
cd ~/Downloads 
rosbags-convert landmarks_demo_uncalibrated.bag
ros2 launch cartographer_ros offline_mir_100_rviz.launch.py bag_filename:=${HOME}/Downloads/landmarks_demo_uncalibrated

# Revo LDS
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/revo_lds/cartographer_paper_revo_lds.bag
cd ~/Downloads 
rosbags-convert cartographer_paper_revo_lds.bag
ros2 launch cartographer_ros demo_revo_lds.launch.py bag_filename:=${HOME}/Downloads/cartographer_paper_revo_lds

# Taurob Tracker
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/taurob_tracker/taurob_tracker_simulation.bag
cd ~/Downloads 
rosbags-convert taurob_tracker_simulation.bag
ros2 launch cartographer_ros demo_taurob_tracker.launch.py bag_filename:=${HOME}/Downloads/taurob_tracker_simulation

# Assets writer backpack 2d
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
cd  ~/Downloads 
rosbags-convert cartographer_paper_deutsches_museum.bag
ros2 launch cartographer_ros offline_backpack_2d.launch.py bag_filenames:=${HOME}/Downloads/cartographer_paper_deutsches_museum
ros2 launch cartographer_ros assets_writer_backpack_2d.launch.py bag_filenames:=${HOME}/Downloads/cartographer_paper_deutsches_museum pose_graph_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.pbstream

# Assets writer backpack 3d
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-14-14-00.bag
cd ~/Downloads 
rosbags-convert b3-2016-04-05-14-14-00.bag
ros2 launch cartographer_ros offline_backpack_3d.launch.py bag_filenames:=${HOME}/Downloads/b3-2016-04-05-14-14-00
ros2 launch cartographer_ros assets_writer_backpack_3d.launch.py bag_filenames:=${HOME}/Downloads/b3-2016-04-05-14-14-00 pose_graph_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.pbstream

# Assets writer ros map
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
cd  ~/Downloads 
rosbags-convert cartographer_paper_deutsches_museum.bag
ros2 launch cartographer_ros offline_backpack_2d.launch.py bag_filenames:=${HOME}/Downloads/cartographer_paper_deutsches_museum
ros2 launch cartographer_ros assets_writer_ros_map.launch.py bag_filenames:=${HOME}/Downloads/cartographer_paper_deutsches_museum pose_graph_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.pbstream

[doisyg]

Friendly ping @MichaelGrupp, can you have a look? We are ready to take care of the rviz part next.
Is there anybody maintaining the project ? We would be okay to help maintaining a ros2 branch

[hengjiUSTC]

Sure, I'v uploaded all relevant file to google drive. We'v also tested other bags and they all crash at same place with save error message.
https://drive.google.com/drive/folders/1jpA0vDHzF4_SSMyhk2NGrNSFcei9bDyX?usp=sharing

was it a setup that was working with ros1?

Yes, the pipeline works fine in ros1, we use it extensively.

[doisyg]

@hengjiUSTC , thanks for the feedback and the bag, I actually only basically tested the asset writer node before with the LaserScan message type. There was a pointer error with the MultiEchoLaserScan and PointCloud2 message types. I pushed the fix along with converted assets writer launch files.
You can now make use of the converted launch file, try:

ros2 launch cartographer_ros assets_writer_backpack_2d.launch.py bag_filenames:=${HOME}/Downloads/rosbag2_2022_01_25-14_31_37_0.db3 pose_graph_filename:=${HOME}/Downloads/map.pbstream urdf_filename:=${HOME}/Downloads/mn.urdf configuration_directory:=${HOME}/Downloads

Assuming you have all your files in ${HOME}/Downloads or adapt accordingly. You need also transform.lua in what you choose to be your configuration_directory.
Can you tell me if you notice a performance degradation compared to ROS1?

[hengjiUSTC]

Thanks @doisyg for replying so quickly, it works now. We didn't see any degradation.
Ros1

Ros2

[doisyg]

Great! Did you notice significant differences in CPU or completion time ?

[hengjiUSTC]

@doisyg We do facing new crash and issue in ROS2 galgactic and still investigating. Sharing our status as reference.
Bag link: https://drive.google.com/file/d/1Z_KtI_yVdEWq7-h0z4VFB0jh-wDdJ7sw/view?usp=sharing

The bag is recorded when we are doing on vehicle test
We have more modules related with control, planning and detection running at same time. If another topic for detection also subscribes to /rslidar_points, cartographer crashes consistently.
log: https://drive.google.com/file/d/1F69lTXQ0wKdW3jMKv8uQR9v9jA3cjsrY/view?usp=sharing
Crash process:

• At first, cartographer works fine and send out TFs
• around 1643274514.401085265 we enable all control, planning and detection modules, cartographer starts to complain Queue waiting for data: (1, points2) not sure why.
• around 1643274579.927902419, crashes with Check failed: last_dispatched_time_ <= next_data->GetTime() (637788713369306715 vs. 637788713367306890) Non-sorted data added to queue: '(1, points2)' found /rslidar_points timestamp just backward. We feel it maybe related with QoS due to another subscription on same PointCloud2 topic.

When playing back
cartographer keeps complaining about [cartographer logger]: W0129 00:57:52.000000 8812 ordered_multi_queue.cc:155] Queue waiting for data: (1, points2).

Analyze:
We check this bag with good bags（the one in previous comment: https://drive.google.com/drive/folders/1jpA0vDHzF4_SSMyhk2NGrNSFcei9bDyX?usp=sharing ), and don't see any difference on topic /rsliar_point. Except one timestamp jump back at 1643274579.927902419 that causes on vehicle test crash, but error message occurs from start.

vehicle_model: https://drive.google.com/file/d/1o-_M3eGknuyZQTUhtHy45vU-aQXcb0ne/view?usp=sharing
pbstream: https://drive.google.com/file/d/1AaApjTgvjxPRdYAWkNA0Bi8Ml08AnXS_/view?usp=sharing
lua: https://drive.google.com/file/d/1v4tIcYW2VsKr-t8jw_xxcvAm1fYhXIa8/view?usp=sharing
Launch:

<node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
    <param name="robot_description" value="$(command 'xacro $(var vehicle_model_file)')"/>
</node>

<node pkg="cartographer_ros" exec="cartographer_node" name="cartographer_node" 
      args="
          -configuration_directory $(var lua_path)
          -configuration_basename tracking.lua
          -load_state_filename $(var pbstream_path)"
    >
      <remap from="/points2" to="/rslidar_points" />
      <remap from="/imu" to="/driver/imu" />
      <param name="use_sim_time" value="$(var use_sim_time)"/>
</node>

Not sure if it's appropriate to discuss here, hope our use case could help make cartographer in galactic more mature.

[hengjiUSTC]

FYI, issue is fixed. The problem is caused by rslidar. After we change lidar to velodyne, cartographer in galactic works perfectly.

[clalancette]

@doisyg I think we've waited long enough here, so I'm going to suggest that we move on with a fork. I can spend some time getting things together, though as this is a side project for me I can't commit to any particular timeline.

The very first question I have is: which version of cartographer should I release into Rolling? I know you originally said 2.0, but is there a newer version (or more particular one) that I should release?

Once that is done (and building in Rolling), I'll come back here so that we can discuss exactly where this code should live, the tagged version it should be, etc.

[doisyg]

Hi Chris,
Thanks for the support !

The very first question I have is: which version of cartographer should I release into Rolling? I know you originally said 2.0, but is there a newer version (or more particular one) that I should release?

The very last commit of the master branch (which is basically the tag 2.0.0 + one commit for CI changes): https://github.com/cartographer-project/cartographer/tree/b8228ee6564f5a7ad0d6d0b9a30516521cff2ee9

Once that is done (and building in Rolling), I'll come back here so that we can discuss exactly where this code should live, the tagged version it should be, etc.

Great, once you have a rolling bin, I will be happy to make the changes for it to compile against it (currently it is galactic).
I also have a commit ready that cleans the CMakeLists.txt files and make it follow the ros package conventional structure (ie separate folders src for .cpp and include for .hpp)

[clalancette]

The very last commit of the master branch (which is basically the tag 2.0.0 + one commit for CI changes): https://github.com/cartographer-project/cartographer/tree/b8228ee6564f5a7ad0d6d0b9a30516521cff2ee9

It's actually easier for us to release tags, so if there are no important changes beyond the 2.0.0 tag, then I'll just release that.

[doisyg]

Great, releasing to tag 2.0.0 will work just fine

[clalancette]

I took a brief look at this today, and we need a couple of fixes to release cartographer.

First, we need to get this rosdep key in: ros/rosdistro#32226
Then we need to get the cartographer package.xml to properly depend on it: https://github.com/cartographer-project/cartographer

The second one we can patch in at release time if need be, but we definitely can't release without the first one.

[doisyg]

First, we need to get this rosdep key in: ros/rosdistro#32226

I did not know that abseil would exist in Jammy, that would simplify things! Though, cartographer asks for a specific commit, see https://github.com/cartographer-project/cartographer/blob/105c034577220268cd28a304a185adbec46b729f/scripts/install_abseil.sh
I would try to run a VM with Jammy and compile cartographer with libabsl-dev deb to check

Then we need to get the cartographer package.xml to properly depend on it: https://github.com/cartographer-project/cartographer

Yes, I can make a PR once the rosdep key is added and that it is confirmed that it works with that version of abseil. Tough it would require, same as for the ros wrapper, a responsive maintainer.

[doisyg]

Also, I just pushed a change in the folder structure of the wrapper to conform more to the classic ros folder organization (and with *.cpp extension instead of *.cc) + cleaner CMakeLists

[doisyg]

I took a bit of time to spin a Jammy VM + Ros2 rolling (testing bin).
Results:

• Cartographer compiles fine with libabsl-dev deb
• Cartographer compiles fine and works (basic offline 2d sanity check) with just one log line adaptation (tf2::getYaw crashes for some reason). Rolling branch: https://github.com/wyca-robotics/cartographer_ros/tree/rolling

[clalancette]

I took a bit of time to spin a Jammy VM + Ros2 rolling (testing bin). Results:

* Cartographer compiles fine with `libabsl-dev` deb

* Cartographer compiles fine and works (basic offline 2d sanity check) with just one log line adaptation (tf2::getYaw crashes for some reason). Rolling branch: https://github.com/wyca-robotics/cartographer_ros/tree/rolling

Thanks for checking in on this. Because of the issues with getting things into upstream, what I'm going to do for now is to continue to use the fork of cartographer at https://github.com/ros2/cartographer. I'll update that to the latest 2.0.0 tag of cartographer, then apply the patch to the package.xml on top. Once I get that done, I'll see if I can do a successful release into Rolling.

[clalancette]

Thanks for checking in on this. Because of the issues with getting things into upstream, what I'm going to do for now is to continue to use the fork of cartographer at https://github.com/ros2/cartographer. I'll update that to the latest 2.0.0 tag of cartographer, then apply the patch to the package.xml on top. Once I get that done, I'll see if I can do a successful release into Rolling.

All right, I successfully released cartographer 2.0.0 into Rolling: https://build.ros2.org/job/Rbin_uJ64__cartographer__ubuntu_jammy_amd64__binary/18/ . I've run out of time to work on this today, but the next step would be to take the patches in here, apply them to https://github.com/ros2/cartographer_ros, and do a release there. @doisyg if I'm going to do that, what tagged version of cartographer_ros should I start from?

[doisyg]

Great news !
Then if you intend to build from https://github.com/ros2/cartographer_ros, I guess I will need to do a PR there. Can you create a branch rolling and I will pr on it ?

[clalancette]

Then if you intend to build from https://github.com/ros2/cartographer_ros, I guess I will need to do a PR there. Can you create a branch rolling and I will pr on it ?

There is already the ros2 branch on https://github.com/ros2/cartographer_ros, so feel free to make a PR to that one.

Before doing that, it would be a good idea to do a build-n-test of your branch of cartographer_ros against the version of cartographer 2.0.0 that I built into Rolling. You should be able to accomplish that by installing an Ubuntu 22.04 container, installing ROS 2 Rolling on it, and then enabling the testing repository (by changing /etc/apt/sources.list.d/ros2.list to contain deb http://packages.ros.org/ros2-testing/ubuntu jammy main.

Once we have confirmation that the cartographer package is good, and you've opened the PR against the ros2 branch, we can proceed with review and merge of your code into cartographer_ros. Thanks!

[doisyg]

Before doing that, it would be a good idea to do a build-n-test of your branch of cartographer_ros against the version of cartographer 2.0.0 that I built into Rolling. You should be able to accomplish that by installing an Ubuntu 22.04 container, installing ROS 2 Rolling on it, and then enabling the testing repository (by changing /etc/apt/sources.list.d/ros2.list to contain deb http://packages.ros.org/ros2-testing/ubuntu jammy main.

I actually did it yesterday after you published it in testing, I get the following error at compilation:

CMake Error in CMakeLists.txt:
Imported target "cartographer" includes non-existent path

"/tmp/binarydeb/ros-rolling-cartographer-2.0.9001/"

in its INTERFACE_INCLUDE_DIRECTORIES. Possible reasons include:

• The path was deleted, renamed, or moved to another location.

• An install or uninstall procedure did not complete successfully.

• The installation package was faulty and references files it does not
  provide.

[clalancette]

I actually did it yesterday after you published it in testing, I get the following error at compilation:

I think there is some kind of packaging bug with Ceres on Jammy. Regardless, cartographer was adding the include directories for Ceres in a non-supported way, so I've now removed that. ros-rolling-cartographer-2.0.9002-1 should fix it, so give that a whirl and see if it is any better.

[doisyg]

Yes, compiling and working fine with the last update

[doisyg]

There is already the ros2 branch on https://github.com/ros2/cartographer_ros, so feel free to make a PR to that one.

I naively made a PR : ros2#59, but the ros2/cartographer_ros ros2 branch should maybe be completely re-based on mine as I started from the original repo cartographer-project/cartographer_ros master branch

[doisyg]

@clalancette any update on your side ?
Confirming it is still building with the last Ubuntu and rolling deb on Ubuntu 22.04.
If you manage to release it, I can take care of an announcement on ros Discourse and a general call for testing in order to gather some feedback before Humble