Robot-localization porting to ROS2

Next Generation ROS
1

Hello @automatom, as communicated over the email, I would like to thank you for your response. From your update, I understood that you managed to get robot_localization to build for ROS2, but have been running into issues with the ROS1 bridge, so you haven’t really been able to test it. Do you have any further update for that? As I mentioned, Can you also please respond back with your input to following two question for my team?

  1. Where are the ROS2 specific src code /build system changes maintained? Don’t see them in the git source repos.
  2. To verify ROS2 functionality, will it suffice if we just run existing ROS1 test suite (kept in test folder in ROS1 src repos)? But these are all simulation tests ( not running on real h/w, sensors ).

We are looking forward to hearing back from you.

Thanks,
Dan

2

So we had hoped to cross this bridge at ROSCon, but luck wasn’t with us on the Lightning Talk random selection.

Here’s the state of…state estimation on ROS2, at least from my (and Locus Robotics’) view.

r_l is a decent generic EKF implementation, but it has some serious drawbacks, namely

  • It doesn’t handle bias estimation for IMU data
  • It uses a generic 3D omnidirectional kinematic model that isn’t appropriate for all robots
  • It’s hard to extend
  • It needs some serious refactoring

So my colleague at Locus, Stephen Williams, decided to write a new state estimation package for ROS1 and ROS2, which we are calling fuse. Fuse addresses literally all of the shortcomings of r_l (or will). The core fuse package is available now, but the core package lacks the ability to actually take in ROS sensor data. To that end, I am working on a fuse_rl package that will replicate all of the behavior of r_l, but in the fuse framework. I may even put together a configuration file converter. fuse_rl will contain, among other things, an explicit 2D kinematic model that is appropriate for differential drive robots, as that appears to be the most common use case for the current package. All of the other functionality will remain intact.

But this post is about ROS2. We don’t yet support ROS2 on fuse, but we are actively working on the problem, as time permits.

As for r_l, I did put some time in to get it to build on ROS2, but I had some trouble with the ROS1 bridge and r_l's custom service definitions. We’re not actively using ROS2 yet, and my free time these days is extremely limited, so I was forced to shelve my efforts for now (I admittedly didn’t put a lot of time into this).

Anyway, this is a very long-winded way of saying that I welcome community involvement in a ROS2 port of r_l, but I expect support to be limited, especially once fuse is released for ROS2. It’s a much more flexible and far better written package, and I think it’ll serve the community well. Having said that, I don’t have a firm timeline on when it will be available, so getting r_l properly working in ROS2 is a worthwhile exercise.

In any case, the answers to your questions are

  1. The ROS2 source for r_l lives in the ros2 branch of the r_l repo:

    https://github.com/cra-ros-pkg/robot_localization/tree/ros2

  2. The r_l test suite comprises bagged data from real and simulated robots. It should be sufficient to verify functionality.

Finally, if you look at the ROS2 migration ticket for r_l, you’ll see that @mkhansen has created a ticket to verify the ROS2 functionality of r_l. I only point this out to ensure that we don’t duplicate efforts unnecessarily.

2 Likes
3

@automatom, thanks for your detail reply. While you are focusing on Fuse core and fuse_rl that will eventually limit the usage of current r_l, as you mentioned, “getting r_l properly working in ROS2 is a worthwhile exercise”, we can continue to put effort on that line for now.

@mkhansen, As Tom mentioned that you might be verifying the ROS2 functionality of r_l, please let me know if you want us to assist you in that exercise.

4

@prasenjitdan - thank you for the offer. We haven’t had the cycles to get to this yet, but are hoping to test it with our ROS2 navigation sometime before the Crystal release. However, if you have some time to do some initial testing of it please go ahead and do so. @mhpanah on our team is working on AMCL and could work with you from our side to get it working at the system level with navigation in a week or two. We’re having weekly Navigation WG meetings and we could make this a topic next week if needed.

5

@mkhansen, sure and thank you for accepting our offer. We will start initial testing on this as soon as we are done with the current package that team is working. We will keep you posted on outcome once we do testing on our side.

6

@automatom, For r_l initial testing, we have pulled r_l source from ros2 branch of the r_l repo and also pulled test folder from kinetic-devel. We are currently workin on migraiting test cases and we have ported/executed only “FilterBaseTest” testcase successfully but for remaining testcases, we have observed few things below.

  1. Testcases are dependent on .test (launch) file that needs rostest/rosparam/rosbag etc pkgs that used in ros1 and not avaiable in ros2.
  2. For “filter_base_diagnostics_timestamps” testcase, diagnostics code is commented in r_l source.
  3. For “robot_localization_estimator” and “ros_robot_localization_listener” testcase, estimator and listener related header/class are not available in r_l source.
  4. “node_bag_pose_tester” testcase is also depend on tag (under .test file) for fatching parameter values from multiple .test file.

We are heading towards migration using below steps.
As per migration guide, .test(launch) files should be converted into .py (launch.py) files using launch pkg in ros2.
But we are facing issues in loading yaml file with this launch file as there is no ros2 load param feature available.

Also, in CMakelists.txt, .test files are added and those are getting executed with ros1 gtest.
But in ros2 when we convert these .test into .py, we are not able to invoke those files from CMakelist.txt.

If anyne have any information/suggestion on the same, it will help us to move faster.

7

Thanks very much for looking into all of this! Let’s move this discussion to the r_l repo. I say we keep it under this issue:

Would you mind re-posting this there? Thanks!

8

Hi Tom and Everyone,

Here is our proposal which we followed to test robot localization in ros2 and passed the tests successfully.

  1. As there are few test cases which depends on the rosbag and rosbag is not available in ros2, ros1 bridge should be used to complete those test scnerios.
    https://index.ros.org/doc/ros2/Rosbag-with-ROS1-Bridge/

  2. As per the migration guide, .test files should be converted into launch.py. But it will not work with “colcon test” as rostest package is not available in ros2. Also it is not sure that how any test which depends on roscore, ros1 bridge, rosbag and then launch.py should work with colcon test as we can not add all these combinations in CMakeLists.txt.

  3. Test cases have been checked by launching specific launch files with other nodes running simultaneously on terminals.Automated scripts have been developed to test these scenrios which opens multiple terminals and run the specific commands related to that test case.

  4. As the launch.py files are launching multiple nodes, it is not exiting on its own.The exit is working with ctrlC. We are not sure if it is possible to exit test which depends on rosbag and ros1 bridge.The test cases which are independent of rosbag and ros1 bridge, can exit from launch.py wihtout ctrlC.

Please provide your suggestions/feedback so that we can confirm on this test environment.
Appreciate your response.
Do let us know in case if you want try these test scenarios and need any help.

Thanks.

9

Let’s keep this discussion over on the source repo, thanks. I’ll respond when I have some time (travelling at the moment).

10

Thanks Tom . We will be waiting for your response.

11

The upcoming ROS 2 Crystal release does contain rosbag2.

While not complete or fully features the launch_testing package provides some ways to write tests based on launch files.

You should structure you launch-based tests in a way that e.g. one of the processed (usually the one using gtest) terminates after running all tests. You can use event handlers in the launch file to then shutdown all the other processes cleanly. While using the legacy API the tests in the demo_nodes_cpp package can serve as an example.

12

@dirk-thomas, Thanks for your response. definitely it will help us and will follow the same.

13

I am not sure if I should ask this here or at the repo.
I would like to ask if the aforementioned drawbacks, like “bias estimation for IMU data” and “generic 3D omnidirectional kinematic model that isn’t appropriate for all robots”, have been solved in the robot_localization dashing-devel branch.

14

I don’t think they’re “drawbacks”, those are examples of feature requests that no one has stood up to take ownership over to implement. They are not implemented in ROS2 (or ROS1) nor do I nor @automatom (I believe) plan on implementing them.

More than welcome to review PRs implementing though :slight_smile:

15

I personally don’t have nearly enough cycles to implement those features, as they represent major changes that will require serious refactoring. I don’t get time to work on r_l for my day job, so those changes will have to come from user contributions.

We haven’t started the ROS2 port of fuse yet, but it is far easier to extend with new kinematic models, bias estimation, etc.