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. Fuse Introduction
Fuse is built as part of MoveIt Pro, meaning you have access to advanced state estimation capabilities out of the box.
Fuse is a state estimation framework utilizing graph optimization techniques to create an extensible, robust framework for many robotics state estimation needs.
To get an overview of Fuse's capabilities, principles, and usage, explore the repository. In particular, the tutorials will provide a way to see a few common use cases in action, including utilizing apriltags to detect a robot, using range sensors, and more.
Once you're comfortable with Fuse's capabilities, you can keep reading to find out how to see an example and learn how to use Fuse in your MoveIt Pro application.
2. Example
First, you may want to take a look at how we use Fuse in our satellite simulation example here. If you launch the example workspace with the space_satellite_sim configuration package, you can run the Grapple Moving Satellite with Fuse example to see a powerful example of Fuse in action!
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 YOUR_CONFIG/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 the Fuse configuration file from our example workspace is a good place to start.
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.
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
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 configuration from the example workspace, 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 the example workspace, you would publish a tf from servicer_camera_optical_frame or wrist_mounted_camera_optical_frame to satellite_tag_* (where * is 0-2).
Then, Fuse will use this measurement and produce a state estimate based off of its value!
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.