State Estimation Setup Guide

MoveIt Pro has built-in support for Fuse, a state estimation framework. Fuse runs alongside MoveIt Pro and publishes configurable estimated topics for use inside MoveIt Pro, similarly to other external frameworks such as Nav2.

This how-to guide demonstrates how to update your MoveIt Pro config to utilize state estimation with Fuse.

1. Add and install Fuse

First, add PickNik's fork of the Fuse library to your MoveIt Pro configuration. To do this, we recommend adding it as a submodule. You can create a folder in your MoveIt Pro configuration under src/dependencies. Then, add Fuse with git submodule add https://github.com/PickNikRobotics/fuse/ src/dependencies/fuse. Next, cd src/dependencies/fuse and git checkout humble. Now that you have checked out the humble branch, you also need to add the submodules fuse relies on. To do this, run git submodule update --init --recursive in the fuse folder you are currently in. MoveIt Pro is built on ROS Humble, so you must check out that branch of fuse.

2. Build Fuse

Now that Fuse exists as a submodule, you can simply build your codebase and fuse will be built alongside it (because fuse packages include package.xml files).

Optional: Try Fuse

If you moveit_pro dev your config, you should notice fuse packages building, too. The first time you do this (and any time you perform a clean build), it will add a significant amount of time to the build. Once you're in the dev container, you can try ros2 launch fuse_tutorials fuse_3d_tutorial.launch.py to see that it is working. If you like, you can explore the tutorials to learn more about Fuse.

3. Integrate Fuse

Now, you're ready to integrate Fuse with your MoveIt Pro config. To do so, add a launch file to your config's launch folder. For our purposes, this will be in our_config_package/launch/fuse.launch.py. This launch file will bring up everything you need to use Fuse in a standalone sense. As such, it will be useful to look over the fuse tutorials and base your launch file off of one of them. Using fuse_apriltag_tutorial.launch.py is a good place to start. Note that you cannot use this launch file unmodified, as it relies on simulators that will only be useful in the tutorial. If you'd like to use those to ensure running fuse from your config works, you can copy the launch file as-is, modifying only the fuse config file path if you would like to use your own.

Notice that fuse requires a config file of its own to run. This config file is relatively complex, but the fuse tutorials and examples provide several common use-cases. For this how-to guide, simply copy over the fuse_apriltag_tutorial.yaml file from the apriltag tutorial.

Next, we want to make it so that MoveIt Pro will launch this file when your config is started. To do this, we will add it to our agent_bridge.launch.xml file, similarly to how you would when adding a camera or other sensor.

Here is an example agent_bridge.launch.xml:

<?xml version="1.0" encoding="UTF-8"?>
<launch>
    <include file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"/>
    <include file="$(find-pkg-share our_config_package)/launch/fuse.launch.py"/>
</launch>

The last file included launches the launch file we created at the beginning of this step. Since your agent_bridge.launch.xml now includes fuse, it will start when you moveit_pro run. If you want to use it in moveit_pro dev, you will have to source your setup.bash and run it manually with ros2 launch our_config_package fuse.launch.py.

4. Use Fuse

Now when fuse is running, it will publish on whatever configured topic you set up in its config file! For example, for the fuse_apriltag_tutorial, it will publish an Odometry message on odom_filtered. Now, in MoveIt Pro you can simply subscribe to this topic to get your estimate out! You can also publish from MoveIt Pro to a topic to provide input to the state estimator. For fuse_apriltag_tutorial you would publish a tf from base_link to april_* (where * is 1-8). Then, fuse will use this measurement and produce a state estimate based off of its value!

Optional: Customize Fuse

Now that you have set up fuse, you may want to add your own sensors, publishers, motion models, or even optimizers. Luckily, fuse is a modular framework built to use each of these as plugins. You should read the documentation and docstrings of whichever plugin type you would like to implement. Additionally, there are many examples of each type of plugin, which should help in implementing your own.