Nav2 Integration Guide

This page guides users through the example implementations provided in the slamcore-ros2-examples repository which integrate the Slamcore SLAM algorithms into Nav2 (the ROS 2 Navigation Stack), using Slamcore as the core component to map the environment as well as to provide accurate positioning of the robotic platform, in ROS 2 Foxy or Galactic or Humble.

The tutorial details how to run the examples natively on Ubuntu 20.04 or using a Docker container for systems which do not support Ubuntu 20.04. Instructions are provided for the iRobot Create 3, Clearpath Robotics TurtleBot 4 and Yujin Robot Kobuki robots, however, using the example launch and configuration files along with these instructions should allow you to integrate Slamcore with Nav2 easily on any robotic platform.

Note

This page only covers the instructions on how to run the examples, to learn more about the Slamcore ↔︎ Nav2 Integration and supported hardware, first see Nav2 Integration Overview.

Outline

Following is the list of steps we will cover in this tutorial.

Fig. 72 Outline of the demo

We’ll delve into each one of these steps in more detail in the next sections.

Robot-specific Features and Changes When Using Slamcore
Initial Robot Set Up and Configuration
slamcore-ros2-examples Metapackage Set Up
Visualization Machine Set Up
Teleoperation
[OPTIONAL] Visual-Inertial-Kinematic Calibration, to improve the overall performance.
The slamcore/base_link ➞ base_link Transformation
Mapping and Live Navigation while teleoperating your robot. For detailed steps on map creation and navigation see, Navigation in Single Session SLAM Mode or Navigation in Localisation Mode Using a Pre-recorded Map.
Interact with the Navigation Demo, set waypoints and navigation goals using our dedicated RViz2 launch file.

Robot-specific Features and Changes When Using Slamcore

This section outlines which features from each robot we will take advantage of and which changes are required when running these demos, compared to the default setup that is provided when first using one of the selected robot platforms.

Create 3

We currently only support the Create 3 robot using ROS 2 Galactic. Minimal changes are required to use Slamcore and Nav2 with the Create 3 robot. The robot must be using firmware G4.1 or newer. We will use the wheel odometry from the robot by subscribing to the Create’s /odom topic to improve our localisation robustness, however, we will disable the odom TF publishing of the robot as our SLAM will take care of publishing the base_link ↔︎ odom transform used for navigation.

The Create 3 implements a number of safety features and reflexes, for example, the robot will reverse slightly when it bumps into something or detects a cliff. These settings can be configured through the ROS 2 parameter server. We will keep all the default setting enabled, the only setting we will change for these examples is the safety_override parameter which is exposed in the Create 3 webserver. We will change this setting from none to backup_only to allow the robot to drive backwards without triggering the cliff safety backup limit. You may keep this parameter in the default setting if you prefer.

We will also change the default RMW_IMPLEMENTATION being used from rmw_cyclonedds_cpp to rmw_fastrtps_cpp, as we have found this Data Distribution Service (DDS) to be more reliable when using corporate Wi-Fi networks. Again, you may keep this parameter in the default setting if you prefer.

Lastly, instead of publishing velocity commands directly to the Create’s /cmd_vel topic, we will implement a cmd_vel_mux node which listens to incoming Twist messages from different topics and chooses the highest priority one to republish to the /cmd_vel topic, to avoid clashes between e.g., teleoperation commands and the navigation stack. The cmd_vel_mux priorities are set via a YAML configuration file.

Instructions on how to configure these changes are provided in the following sections.

TurtleBot 4

We currently only support the TurtleBot 4 robot using ROS 2 Galactic. The TurtleBot 4 robot comes with a number of turtlebot4 ROS 2 packages pre-installed on the Raspberry Pi as mentioned on the TurtleBot 4 Documentation. These packages mainly add functionality to bring up the components that are included with the robot such as the 2D lidar and OAK-D camera, allow control of the buttons and display on the TurtleBot 4 Standard (these are not present on the Lite version), and provide example launch and configuration files for using SLAM and navigation on the TurtleBot 4 with the aforementioned 2D lidar and OAK-D camera.

When using Slamcore on the TurtleBot 4, our goal was to try and maintain all the original functionality, making it easy to revert any new changes that might be necessary for our integration. As a result, all the changes have been documented below and instructions on how to apply them and revert them are provided further down the page.

Note

The TurtleBot 4 uses a Create 3 robot as a base, therefore, the same changes that are described in our Create 3-only example will be applied for this example. These have been included below as well for completeness.

Documented changes:

We will not be using the lidar and camera on board the TurtleBot 4. We will replace the camera with an Intel RealSense D435i.
We will use Slamcore Visual-Inertial-Kinematic SLAM using the RealSense D435i camera and the wheel odometry from the Create 3 instead of the 2D lidar SLAM with slam_toolbox. Similar to the Create 3-only example, we will subscribe to the Create’s /odom topic to integrate the wheel odometry data and disable the odom TF publishing from the Create 3 robot base as Slamcore will take care of publishing both the odom ↔︎ base_link and map ↔︎ odom frame transforms. Additionally, note that map generation, saving and loading is all managed through the Slamcore ROS 2 Wrapper.
We will not use the turtlebot4_navigation package or its launch and configuration files. Instead, we will use our slamcore_ros2_turtlebot4_example package from slamcore-ros2-examples which provides similar navigation launch and configuration files but using our SLAM.
We will use the turtlebot4_description package for the TurtleBot4 urdf model, but we will replace the OAK-D with a RealSense Camera. Our full URDF models for the TurtleBot 4 Standard and Lite can be found in the slamcore_ros2_turtlebot_example/descriptions folder.
We will edit the turtlebot4-bringup launch files to not launch the lidar and OAK-D camera. With these new launch files, we will also bring up our modified robot_description which includes a RealSense camera, joy_teleop with slightly different controls and a cmd_vel_mux so that we can set priorities for the different incoming commands i.e. teleoperation, autonomous navigation. You can find the new launch files in the slamcore_ros2_turtlebot_example/launch folder.
We will disable the default bringup service turtlebot.service that is installed on the Raspberry Pi using robot_upstart and runs on boot and will replace it with a new slamcore_turtlebot4.service based on our new bringup launch files. Instructions on how to set this up are provided further below.
We will still retain the button and screen functionality on the TurtleBot 4 Standard.
We will use the turtlebot4_diagnostics package, however, diagnostics for the OAK-D camera and lidar will be broken, as we will be using a RealSense D435i instead, for which we have not implemented diagnostics. Our SLAM, however, does publish diagnostics info on the /diagnostics topic.
We will not use the turtlebot4_viz package. We have our own RViz2 config to visualize the robot during navigation as well as our own launch file to visualize the static robot model. These are essentially the same as the ones provided by turtlebot but they load our own robot model.
We will not support the turtlebot4_simulator package.
The Create 3 implements a number of safety features and reflexes, for example, the robot will reverse slightly when it bumps into something or detects a cliff. These settings can be configured through the ROS 2 parameter server. We will keep all the default setting enabled, the only setting we will change for these examples is the safety_override parameter which is exposed in the Create 3 webserver. We will change this setting from none to backup_only to allow the robot to drive backwards without triggering the cliff safety backup limit. You may keep this parameter in the default setting if you prefer.
We will change the default RMW_IMPLEMENTATION being used from rmw_cyclonedds_cpp to rmw_fastrtps_cpp, as we have found this DDS to be more reliable when using corporate Wi-Fi networks. Again, you may keep this parameter in the default setting if you prefer.
Lastly, instead of publishing velocity commands directly to the Create’s /cmd_vel topic, we will implement a cmd_vel_mux node which listens to incoming Twist messages from different topics and chooses the highest priority one to republish to the /cmd_vel topic, to avoid clashes between e.g., teleoperation commands and the navigation stack. The cmd_vel_mux priorities are set via a YAML configuration file.

Instructions on how to configure these changes are provided in the following sections.

Kobuki

We currently only support the Kobuki robot using ROS 2 Foxy. Note that the Kobuki robot does not run ROS 2 onboard, so the motor control nodes will run on your compute board of choice, which must be connected via USB to the robot.

We will use the kobuki_node, kobuki_description, kobuki_safety_controller, kobuki_bumper2pc and kobuki_keyop packages from the kobuki_ros repository built from source, launching the nodes from these packages with our slamcore_ros2_kobuki_example launch files. In addition, similar to the other examples, we will implement a cmd_vel_mux package which selects among a number of incoming Twist messages, choosing the highest priority one to republish to the output /cmd_vel topic. The cmd_vel_mux priorities are set via a YAML configuration file.

In our slamcore_ros2_kobuki_example package, the kobuki_setup_comms_launch.py launch file will bring up the kobuki_node motor controller, the cmd_vel_mux and the kobuki_safety_controller, which will cause the robot to reverse if it hits something with a bumper or detects a cliff. The safety controller will also stop the wheels if the robot is picked up.

Our kobuki_live_navigation_launch.py launch file launches our SLAM, Nav2, the robot_state_publisher which loads our robot model URDF and the kobuki_bumper2pc node which publishes any bumper presses as point cloud points in front of the robot, so that any bumper events can be taken into account and added to the costmaps during navigation, allowing the robot to replan its path.

Initial Robot Set Up and Configuration

This section details how to apply the changes described in the previous section and configure your robot base and compute board to work with our slamcore-ros2-examples metapackage. If using a different robot platform, you may skip this section.

Create 3

The Create 3 runs ROS 2 directly on board. Make sure you follow the Create 3 Initial Setup Guide and are using the Create 3 Galactic firmware version G4.1 or newer with our examples. Note, our examples do not currently support ROS 2 Humble.

Once you have connected the Create 3 to Wi-Fi and can access the webserver, you will need to do the following changes:

Set a ROS 2 Domain ID if desired. Note this Domain ID will have to be the same on the Create 3, your compute board and your visualization laptop for them to communicate correctly. You can do this on your compute board and laptop by running, e.g., export ROS_DOMAIN_ID=20 in your terminal or adding that command to the end of your ~/.bashrc file.

Do not set a ROS 2 Namespace as our launch files are meant to be used with the Create 3 with no namespace. You may add a namespace and update our launch files manually if desired.

We recommend changing the RMW_IMPLEMENTATION to rmw_fastrtps_cpp as we have found this to work better with corporate networks. Note the RMW_IMPLEMENTATION will have to be the same on the Create 3, your compute board and your visualization laptop for them to communicate correctly. You can do this on your compute board and laptop by running, e.g., export RMW_IMPLEMENTATION=rmw_fastrtps_cpp in your terminal or adding it to your ~/.bashrc file.
Replace the yaml text under Application ROS 2 Parameters File with the following:
motion_control:
  ros__parameters:
    # safety_override options are
    # "none" - standard safety profile, robot cannot backup more than an inch because of lack of cliff protection in rear, max speed 0.306m/s
    # "backup_only" - allow backup without cliff safety, but keep cliff safety forward and max speed at 0.306m/s
    # "full" - no cliff safety, robot will ignore cliffs and set max speed to 0.46m/s
    safety_override: "backup_only"

robot_state:
  ros__parameters:
    publish_odom_tfs: false
This will change the default safety_override from none to backup_only, to allow driving the robot backwards without cliff safety kicking in, and will disable the odom TF publishing from the Create 3, as Slamcore will take care of publishing that transform (Note this feature is only available from G.4.1 onward).
Save all the changes and restart the application.

Note

If you’d like to use wheel odometry, make sure you set up Chrony on your compute board following the Network Time Protocol page from the Create 3 docs, to ensure the Create 3 and compute board are on the same clock.

TurtleBot 4

Note

The TurtleBot 4 uses a Create 3 robot as a base, therefore, the same changes that are described in our Create 3-only example will be applied for this example. These have been included below as well for completeness.

Follow the TurtleBot 4 Basic Setup tutorial to bring up your robot and connect it to Wi-Fi. We currently only support the TurtleBot 4 with ROS 2 Galactic, make sure the Raspberry Pi on the robot is running Ubuntu 20.04 and ROS 2 Galactic. Similarly, make sure you update the Create 3 base through its webserver to Create 3 Galactic firmware version G.4.1 or newer. Our examples do not currently support ROS 2 Humble.

If you haven’t done so already, the first step is to swap the TurtleBot’s OAK-D camera with a RealSense D435i. The RealSense D435i can be mounted on the existing camera mount, you will need 2x M3 5mm screws. If you have longer screws you can use some nuts or washers to ensure the camera is secured correctly.

Once you have connected the TurtleBot 4 to Wi-Fi and can access the Create 3 webserver (Accessing the Create 3 webserver) , you will need to do the following changes to the Create 3 configuration:

Set a ROS 2 Domain ID if desired. Note this Domain ID will have to be the same on the Create 3, your compute board and your visualization laptop for them to communicate correctly. You can do this on your compute board and laptop by running, e.g., export ROS_DOMAIN_ID=20 in your terminal or adding that command to the end of your ~/.bashrc file.

Do not set a ROS 2 Namespace as our launch files are meant to be used with the Create 3 with no namespace. You may add a namespace and update our launch files manually if desired.

We recommend changing the RMW_IMPLEMENTATION to rmw_fastrtps_cpp as we have found this to work better with corporate networks. Note the RMW_IMPLEMENTATION will have to be the same on the Create 3, your compute board and your visualization laptop for them to communicate correctly. You can do this on your compute board and laptop by running, e.g., export RMW_IMPLEMENTATION=rmw_fastrtps_cpp in your terminal or adding it to your ~/.bashrc file.
Replace the yaml text under Application ROS 2 Parameters File with the following:
motion_control:
  ros__parameters:
    # safety_override options are
    # "none" - standard safety profile, robot cannot backup more than an inch because of lack of cliff protection in rear, max speed 0.306m/s
    # "backup_only" - allow backup without cliff safety, but keep cliff safety forward and max speed at 0.306m/s
    # "full" - no cliff safety, robot will ignore cliffs and set max speed to 0.46m/s
    safety_override: "backup_only"

robot_state:
  ros__parameters:
    publish_odom_tfs: false
This will change the default safety_override from none to backup_only, to allow driving the robot backwards without cliff safety kicking in, and will disable the odom TF publishing from the Create 3, as Slamcore will take care of publishing that transform (Note this feature is only available from G.4.1 onward).
Save all the changes and restart the application.

If you have set a ROS_DOMAIN_ID or changed the RMW_IMPLEMENTATION on the Create 3, you will need to do this on the Raspberry Pi (and your laptop) as well, to ensure the Pi, the robot and your laptop can communicate correctly.

ssh and open the .bashrc file on the Pi with e.g. vim:
$ vim ~/.bashrc
Change the RMW_IMPLEMENTATION from rmw_cyclonedds_cpp to rmw_fastrtps_cpp. Comment out the CYCLONEDDS_URI line as we are no longer using CYCLONE_DDS and, finally, add a line to set your chosen ROS_DOMAIN_ID.
export RMW_IMPLEMENTATION=rmw_fastrtps_dynamic_cpp
#export CYCLONEDDS_URI=/etc/cyclonedds_rpi.xml
export ROS_DOMAIN_ID=20
Save and exit. Then source your updated .bashrc file and restart the ROS 2 daemon.
$ source ~/.bashrc
$ ros2 daemon stop
$ ros2 daemon start

Lastly, we will disable the pre-installed TurleBot 4 bringup service:

The Raspberry Pi has a systemd bringup service that will launch the standard.launch or lite.launch file from the turtlebot4_bringup package on boot. We will set up our own systemd service that will launch our custom bringup launch file, so we want to disable the existing one so that they don’t clash.
$ sudo systemctl disable turtlebot4.service --now
You can check the status of the service with:
$ sudo systemctl status turtlebot4.service
We will look into how to set up our new service in the next section.

Kobuki

As the Kobuki robot does not have a configurable system on board, no changes are necessary before setting up our slamcore-ros2-examples metapackage.

Visualization Machine Set Up

You may repeat the same steps outlined above on a second machine for visualization. We will then be able to take advantage of ROS 2’s network capabilities to visualize the topics being published by the robot on this second machine with minimal effort - as long as both machines are on the same network.

Note

You may use the Dockerfile provided even if your system is Ubuntu 20.04 for an easier, isolated setup.

Teleoperation

There are two ways we can teleoperate a robot, keyboard teleoperation or joystick teleoperation, e.g. using a Bluetooth PS4 controller. Instructions on how to teleoperate each robot are provided in the tabs below.

Create 3

To teleoperate the Create 3, first make sure the cmd_vel_mux node is running. On your compute board run:

$ ros2 launch slamcore_ros2_create3_example create3_cmd_vel_mux_launch.py

Then you can launch keyboard teleoperation or joystick teleoperation.

Keyboard Teleoperation

After setting up the slamcore-ros2-examples workspace on your laptop and installing the dependencies-create3.txt, you can run keyboard teleoperation with:

$ ros2 run slamcore_ros2_create3_example create3_teleop_key

This script starts the teleop_twist_keyboard node, allowing you to drive the robot with the i j l , keys via the terminal. The node will publish the commands on the ROS 2 network which the cmd_vel_mux on the compute board will subscribe to.

Joystick Teleoperation

For joystick teleoperation, you will need a Bluetooth controller, e.g., a PS4 controller. First, make sure you pair the controller to your compute board and can connect it automatically reliably. Then launch joy_teleop on your compute board with:

$ ros2 launch slamcore_ros2_create3_example create3_teleop_joy_launch.py

To drive the robot, press and hold the L1 button (dead man switch) and use the left joystick for Fwd/Back and the right joystick for steering (See PS4 Button Mapping). You can also hold the R1 button to drive the robot at a higher speed.

The current configuration is meant to be used with a PS4 controller, you can change the joystick configuration or create a new one using the following example: ps4-joy.yaml.

TurtleBot 4

On the TurtleBot 4 platforms, if you set up the slamcore_turtlebot4.service bringup service as explained in the previous section, the cmd_vel_mux node will be launched on boot. otherwise, you will have to launch turtlebot4_cmd_vel_mux_launch.py manually for the robot to receive the teleoperation commands.

Follow the instructions below depending on the version of TurtleBot you are using.

TurtleBot 4 Standard

If you have not enabled the slamcore_turtlebot4.service on boot, bring up the essential TurtleBot 4 nodes, which includes the cmd_vel_mux and joy_teleop nodes with:

$ ros2 run slamcore_ros2_turtlebot4_example turtlebot4_bringup_standard.launch.py

Keyboard Teleoperation

After setting up the slamcore-ros2-examples workspace on your laptop and installing the dependencies-turtlebot4.txt, you can run Keyboard Teleoperation with:

$ ros2 run slamcore_ros2_turtlebot4_example turtlebot4_teleop_key

This script starts the teleop_twist_keyboard node, allowing you to drive the robot with the i j l , keys via the terminal. The node will publish the commands on the ROS 2 network which the cmd_vel_mux on the compute board will subscribe to.

Joystick Teleoperation

The TurtleBot 4 Standard, includes a Bluetooth joystick controller which should already be paired to the Raspberry Pi. You can turn it on by pressing the central home button. The controller will show it has paired successfully when the light on it turns blue. If the controller does not pair or you’d like to pair a new controller, see the TurtleBot 4 Controller Setup Guide.

The turtlebot4_bringup_standard.launch.py launch file which is brought up with the slamcore_turtlebot4.service on boot will launch the cmd_vel_mux and joy_teleop nodes, which means after you turn on the robot and connect the controller, you should be able to teleoperate the robot straight away.

You can check that the service is running correctly with:

$ sudo systemctl status slamcore_turtlebot4.service

Alternatively, if it is not running, you can launch joystick teleoperation manually with:

$ ros2 launch slamcore_ros2_turtlebot4_example turtlebot4_teleop_joy_launch.py

Warning

Make sure the cmd_vel_mux is also running if you launch joy_teleop manually.

To drive the robot, press and hold the L1 button (dead man switch) and use the left joystick for Fwd/Back and the right joystick for steering (See PS4 Button Mapping). You can also hold the R1 button to drive the robot at a higher speed.

The current configuration is meant to be used with a PS4 controller, you can change the joystick configuration or create a new one using the following example: ps4-joy.yaml.

The TurtleBot 4 Standard also adds the following options to the controller, allowing you to call a number of the Create’s services and to control the menu on the on board display:

controller:
    a: ["Select"]
    b: ["EStop"]
    x: ["Back"]
    y: ["RPLIDAR Motor"]
    up: ["Scroll Up"]
    down: ["Scroll Down"]
    l2: ["Wall Follow Left"]
    r2: ["Wall Follow Right"]
    home: ["Dock", "Undock", "3000"]

TurtleBot 4 Lite

If you have not enabled the slamcore_turtlebot4.service on boot, bring up the essential TurtleBot 4 nodes, which includes the cmd_vel_mux:

Warning

On the TurtleBot 4 Lite, joy_teleop is not included in the bringup launch file and needs to be launched manually. Further details below.

$ ros2 run slamcore_ros2_turtlebot4_example turtlebot4_bringup_lite.launch.py

Keyboard Teleoperation

After setting up the slamcore-ros2-examples workspace on your laptop and installing the dependencies-turtlebot4.txt, you can run Keyboard Teleoperation with:

$ ros2 run slamcore_ros2_turtlebot4_example turtlebot4_teleop_key

This script starts the teleop_twist_keyboard node, allowing you to drive the robot with the i j l , keys via the terminal. The node will publish the commands on the ROS 2 network which the cmd_vel_mux on the compute board will subscribe to.

Joystick Teleoperation

The TurtleBot 4 Lite, does not include a Bluetooth joystick controller so you will have to pair a new one. The Bluetooth packages are not installed on the Lite’s Raspberry Pi by default so, if you haven’t done so already, you will need to install them with sudo bluetooth.sh (a TurtleBot 4 script that should be present on the Pi that comes with the robot) and then reboot the Pi, as explained in the TurtleBot 4 Joystick Teleoperation Tutorial. You can then pair a new controller following the TurtleBot 4 Controller Setup Guide.

The turtlebot4_bringup_lite.launch.py launch file which is brought up with the slamcore_turtlebot4.service on boot will launch the cmd_vel_mux node but not the joy_teleop node. Therefore, you will need to launch this manually to start joystick teleoperation:

$ ros2 launch slamcore_ros2_turtlebot4_example turtlebot4_teleop_joy_launch.py

Warning

Make sure the cmd_vel_mux is also running when you launch joy_teleop manually.

To drive the robot, press and hold the L1 button (dead man switch) and use the left joystick for Fwd/Back and the right joystick for steering (See PS4 Button Mapping). You can also hold the R1 button to drive the robot at a higher speed.

The current configuration is meant to be used with a PS4 controller, you can change the joystick configuration or create a new one using the following example: ps4-joy.yaml.

Kobuki

To teleoperate the Kobuki, first make sure the kobuki_node and cmd_vel_mux nodes are running. On your compute board run:

$ ros2 launch slamcore_ros2_kobuki_example kobuki_setup_comms_launch.py

Then you can launch keyboard teleoperation or joystick teleoperation.

Keyboard Teleoperation

After setting up the slamcore-ros2-examples workspace on your laptop and installing the dependencies-kobuki.txt, you can run keyboard teleoperation with:

$ ros2 run slamcore_ros2_kobuki_example kobuki_teleop_key

This script starts the kobuki_keyop node, allowing you to drive the robot with the arrow keys via the terminal. The node will publish the commands on the ROS 2 network which the cmd_vel_mux on the compute board will subscribe to.

Joystick Teleoperation

For joystick teleoperation, you will need a Bluetooth controller, e.g., a PS4 controller. First, make sure you pair the controller to your compute board and can connect it automatically reliably. Then launch joy_teleop on your compute board with:

$ ros2 run slamcore_ros2_kobuki_example kobuki_teleop_joy

To drive the robot, press and hold the L1 button (dead man switch) and use the left joystick for Fwd/Back and the right joystick for steering (See PS4 Button Mapping).

Note

If the above mapping does not work with your joystick/driver, you may try the following alternative using the joystick_mode:=old argument:

$ # Press L1 and use the left joystick for Fwd/Back, right joystick for Left/Right
$ ros2 run slamcore_ros2_kobuki_example kobuki_teleop_joy --ros-args -p joystick_mode:=old

See the PS4 Button Mapping section in the Appendix for an illustration of the PS4 controller buttons to be used.

Visual-Inertial-Kinematic Calibration

To increase the overall accuracy of the pose estimation we will fuse the wheel-odometry measurements of the robot encoders into our SLAM processing pipeline. This also makes our positioning robust to kidnapping issues (objects partially or totally blocking the camera field of view) since the algorithm can now depend on the odometry to maintain tracking.

To enable the wheel-odometry integration, follow the corresponding tutorial: Wheel Odometry Integration. After the aforementioned calibration step, you will receive a VIK configuration file similar to the one shown below:

Warning

The configuration file format for running SLAM on customized parameters has changed in v23.01. This affects all VIK calibration files previously provided to you. Please see JSON configuration file migration for more information.

The `slamcore/base_link` ➞ `base_link` Transformation

To integrate the Slamcore coordinate frame (located at the camera’s IMU) with the robot’s base coordinate frame, usually base_link or base_footprint at the base of the robot between the wheels, we must set a slamcore/base_link $\rightarrow$ base_link transformation. This will allow us to connect the map $\rightarrow$ odom $\rightarrow$ slamcore/base_link transform tree with the robot’s static transform tree described in its URDF file. With this we will be able to display the robot model correctly.

If you are using one of the robots supported in these examples, with the camera mounted in the same position, you can use the existing slamcore_camera_to_robot_base_transform defined in the <robot>-nav2-demo-params.yaml file of each example. Our static_transform_pub_launch script then fetches and loads this static transform when included in a launch file, like seen, for example, in turtlebot4_slam_launch.py.

If you are using your own robot or have mounted the camera elsewhere, you will need to measure and define this transformation, as detailed in the drop down below.

How to set the slamcore/base_link ➞ base_link transformation

Before you can start navigating in the environment, you need to provide the transformation between the frame of reference of the robot base, base_link in our example and the frame of reference of the Slamcore pose estimation algorithm, i.e., slamcore/base_link.

The translation and rotation parts of this transform should be specified in the xyz and rpy fields of the slamcore_camera_to_robot_base_transform parameter in the <robot>-nav2-demo-params.yaml config file. Examples can be found in our slamcore-ros2-examples repo. You may copy this file and modify the parameters to suit your setup. You can then load this file by passing in the path to the file using the params_file argument when launching navigation.

The more accurate this transform the better, but for now a rough estimation will do. In our case, since the camera is placed 23cm above and 8.7cm in front of the base_link we used the following values.

slamcore_camera_to_robot_base_transform:
 ros__parameters:
   parent_frame: slamcore/base_link
   child_frame: base_link
   xyz: [0.015, 0.236, -0.087]
   rpy: [0.000, -1.571, 1.571]

Note

Note that, if you are also integrating wheel odometry measurements, the slamcore/base_link $\rightarrow$ base_link transform will be specified in two places, once in the slamcore_camera_to_robot_base_transform variables in <robot>-nav2-demo-params.yaml and a second time in the kinematic parameters (see Visual-Inertial-Kinematic Calibration section) of the slam-config.json file. You can save some time and copy the T_SO.T section given by the VIK config file to the slamcore_camera_to_robot_base_transform xyz values in <robot>-nav2-demo-params.yaml. Note however that calibration procedure cannot compute the height difference between the two frames since it’s not observable in planar motion. Therefore, you will have to measure and edit the (Y) value manually of slamcore_camera_to_robot_base_transform’s xyz variable.

Mapping and Live Navigation

Now that the robot has been set up with our software, there are two ways of running the examples:

Navigation in Single Session SLAM Mode, this involves creating a map in real-time and navigating inside it as the robot explores a space.
Navigation in Localisation Mode Using a Pre-recorded Map, this involves creating a map first and then loading the map to run in localisation mode.

Navigation in Single Session SLAM Mode

In single session SLAM mode, the robot will generate a map as it moves around a space. When SLAM is stopped, the map will be discarded unless the slamcore/save_session service is called before.

You can bring up the robot, SLAM and Nav2 in single session SLAM mode with the following commands on your robot.

Note

If you have created new SLAM and Nav2 configuration files, don’t forget to pass these in with the config_file and params_file arguments respectively, to override the default ones. The config_file should contain the VIK calibration parameters which will enable the robot to use Visual-Inertial-Kinematic SLAM (if you have completed a VIK calibration) and any additional Slamcore configuration parameters, detailed in SLAM Configuration.

Create 3

First, launch SLAM with:

$ ros2 launch slamcore_ros2_create3_example create3_slam_launch.py

Then, launch Nav2 with:

$ ros2 launch slamcore_ros2_create3_example create3_navigation_launch.py

Note

SLAM and Nav2 are launched in separate files, one after the other, to avoid overloading the DDS network when launching everything at the same time, which can cause the Create 3 robot to run out of memory. We have experienced this issue when using Fast DDS and have not tested Cyclone DDS. Also note that when running in Single Session SLAM mode, the order in which you launch SLAM and Nav2 does not matter.

TurtleBot 4

First, launch SLAM with:

$ ros2 launch slamcore_ros2_turtlebot4_example turtlebot4_slam_launch.py

Then, launch Nav2 with:

$ ros2 launch slamcore_ros2_turtlebot4_example turtlebot4_navigation_launch.py

Note

SLAM and Nav2 are launched in separate files, one after the other, to avoid overloading the DDS network when launching everything at the same time, which can cause the Create 3 robot to run out of memory. We have experienced this issue when using Fast DDS and have not tested Cyclone DDS. Also note that when running in Single Session SLAM mode, the order in which you launch SLAM and Nav2 does not matter.

Kobuki

On the Kobuki, you can launch SLAM and Nav2 with a single launch file:

$ ros2 launch slamcore_ros2_examples kobuki_live_navigation_launch.py \
>   config_file:=</path/to/slam/config/json> \
>   params_file:=</path/to/params/yaml/>

Note

If you have already brought up the Kobuki with our kobuki_setup_comms_launch.py launch script, to for example teleoperate the robot, you can launch the above file with the comms argument set to false, to only launch the navigation and SLAM components.

$ ros2 launch slamcore_ros2_examples kobuki_live_navigation_launch.py \
>   session_file:=<path/to/session/file> \
>   config_file:=</path/to/slam/config/json> \
>   params_file:=</path/to/params/yaml/> \
>   comms:=false

Visualization

On your visualization machine, run the following to visualize the map being created in RViz2:

Create 3

$ ros2 launch slamcore_ros2_create3_example create3_navigation_monitoring_launch.py

TurtleBot 4

$ ros2 launch slamcore_ros2_turtlebot4_example turtlebot4_navigation_monitoring_launch.py

Kobuki

$ ros2 launch slamcore_ros2_kobuki_example kobuki_navigation_monitoring_launch.py

Once Nav2 is up and running, see Interact with the Navigation Demo to learn how to set single goals or multiple waypoints for navigation.

Save the map

If you would like to save the map to reuse it in the future, you can call the slamcore/save_session service from another terminal:

$ ros2 service call /slamcore/save_session std_srvs/Trigger

By default, the session file will be saved to the working directory from which <robot>_slam_launch.py was called and SLAM will be paused while the file is being generated. If you would like the service to save the session file to a specific directory, you must set the session_save_dir parameter when launching <robot>_slam_launch.py.

Warning

Note that when using our Docker container, files should be saved inside the slamcore-ros2-examples directory of the container, as otherwise they will not be saved to your machine after exiting the container.

The next time you launch navigation you can provide the path to the session file using the session_file argument as explained in the section below.

Navigation in Localisation Mode Using a Pre-recorded Map

In localisation mode, we can navigate in a map that has been recorded previously. This map can either be generated from a recorded dataset or at the end of a previous live run by using the slamcore/save_session service as mentioned above. If running on a limited compute platform, it might be preferable to record a dataset and then generate the map on a more powerful machine. Check the drop-down below to see the benefits and disadvantages of creating a map live VS from a recorded dataset.

The two options for map creation are detailed below.

Generate a map from a dataset

We can generate a map by first recording a dataset and then processing it using slamcore_visualiser (GUI) or slamcore_dataset_processor (command line tool).

Record Dataset to Map the Environment

Assuming the main robot nodes are up and we can teleoperate the robot, we will be using the dataset_recorder.launch.py launch file to capture a dataset that contains visual-inertial, depth as well as kinematic information (if you have a VIK calibration).

To launch the Slamcore dataset recorder run:

Create 3

$ ros2 launch slamcore_slam dataset_recorder.launch.py \
>   override_realsense_depth:=true \
>   realsense_depth_override_value:=true \
>   odom_reading_topic:=/odom \
>   ros_extra_params:=<path>/<to>/slamcore-ros2-examples/src/slamcore_ros2_examples_common/config/dataset-recorder-odom-override.yaml

TurtleBot 4

$ ros2 launch slamcore_slam dataset_recorder.launch.py \
>   override_realsense_depth:=true \
>   realsense_depth_override_value:=true \
>   odom_reading_topic:=/odom \
>   ros_extra_params:=<path>/<to>/slamcore-ros2-examples/src/slamcore_ros2_examples_common/config/dataset-recorder-odom-override.yaml

Kobuki

$ ros2 launch slamcore_slam dataset_recorder.launch.py \
>   override_realsense_depth:=true \
>   realsense_depth_override_value:=true \
>   odom_reading_topic:=/odom

Note that recording the kinematic measurements (by subscribing to a wheel odometry topic) is not necessary, since we can generate a map using purely the visual-inertial information from the camera. Kinematics will however increase the overall accuracy if recorded and used.

When you have covered all the space that you want to map, send a Ctrl-c signal to the application to stop. By default, the dataset will be saved in the current working directory, unless the output_dir argument is specified.

Warning

Note that when using our Docker container, files should be saved inside the slamcore-ros2-examples directory of the container, as otherwise they will not be saved to your machine after exiting the container.

You now have to process this dataset and generate the .session file. In our case, we compressed and copied the dataset to an x86_64 machine in order to accelerate the overall procedure.

$ tar -cvfz mydataset.tgz mydataset/
$ rsync --progress -avt mydataset.tgz <ip-addr-of-x86_64-machine>:

Create Session and Map for Navigation

Once you have the (uncompressed) dataset on the machine that you want to do the processing at, use the slamcore_visualiser to process the whole dataset and at the end of it, save the resulting session.

$ # Launch slamcore_visualiser, enable mapping features - `-m`
$ slamcore_visualiser dataset \
>   -u mydataset/ \
>   -c /usr/share/slamcore/presets/mapping/default.json /<path>/<to>/<vik-calibration.json> \
>   -m

Alternatively, you can use the slamcore_dataset_processor command line tool, however, you won’t be able to visualize the map being built.

$ # Launch slamcore_dataset_processor, enable mapping features `-m` and session saving `-s`
$ slamcore_dataset_processor dataset \
>   -u mydataset/ \
>   -c /usr/share/slamcore/presets/mapping/default.json /<path>/<to>/<vik-calibration.json> \
>   -m \
>   -s

Note

Refer to Step 2 - Prepare the mapping configuration file in case you want to tune the mapping configuration file in use.

Fig. 73 2.5D Height Map being generated

Fig. 74 2D Occupancy Grid being generated

Edit the Generated Session/Map

You can optionally use slamcore_session_explorer and the editing tool of your choice, e.g. Gimp to create the final session and corresponding embedded map. See Slamcore Session Explorer for more. When done, copy the session file over to the machine that will be running SLAM, if not already there.

Generate a map live

Instead of recording a dataset, we can save a session map directly when running SLAM in Height Mapping/Single Session mode. If you are already running Nav2 in single session SLAM mode with the commands shown in Navigation in Single Session SLAM Mode or you simply launch our SLAM with the following:

$ ros2 launch slamcore_slam slam_publisher.launch.py generate_map2d:=true

you should be able to visualize the map being built in RViz2 by subscribing to the /slamcore/map topic. You can then call the slamcore/save_session service from another terminal to save the current map that is being generated.

$ ros2 service call /slamcore/save_session std_srvs/Trigger

The session file will be saved to the current working directory by default.

Warning

Note that when using our Docker container, files should be saved inside the slamcore-ros2-examples directory of the container, as otherwise they will not be saved to your machine after exiting the container.

Once you have the map, you can pass the path to the session file as an argument when launching navigation.

Note

If you have created new SLAM and Nav2 configuration files, don’t forget to pass these in with the config_file and params_file arguments respectively, to override the default ones. The config_file should contain the VIK calibration parameters which will enable the robot to use Visual-Inertial-Kinematic SLAM (if you have completed a VIK calibration) and any additional Slamcore configuration parameters, detailed in SLAM Configuration.

Create 3

First, load the navigation stack:

$ ros2 launch slamcore_ros2_create3_example create3_navigation_launch.py

Then, launch SLAM in localisation mode:

$ ros2 launch slamcore_ros2_create3_example create3_slam_launch.py \
>   session_file:=</path/to/session/file> \
>   config_file:=</path/to/slam/config/json> \
>   params_file:=</path/to/params/yaml>

Warning

In localisation mode, the occupancy map is published by our software in Quality of Service Durability Transient Local, therefore, make sure you launch Nav2 first before launching our SLAM. Launching Nav2 after our SLAM might lead to Nav2 throwing an error saying the map cannot be found.

TurtleBot 4

First, load the navigation stack:

$ ros2 launch slamcore_ros2_turtlebot4_example turtlebot4_navigation_launch.py

Then, launch SLAM in localisation mode:

$ ros2 launch slamcore_ros2_turtlebot4_example turtlebot4_slam_launch.py \
>   session_file:=</path/to/session/file> \
>   config_file:=</path/to/slam/config/json> \
>   params_file:=</path/to/params/yaml>

Warning

In localisation mode, the occupancy map is published by our software in Quality of Service Durability Transient Local, therefore, make sure you launch Nav2 first before launching our SLAM. Launching Nav2 after our SLAM might lead to Nav2 throwing an error saying the map cannot be found.

Kobuki

$ ros2 launch slamcore_ros2_kobuki_example kobuki_live_navigation_launch.py \
>   session_file:=</path/to/session/file> \
>   config_file:=</path/to/slam/config/json> \
>   params_file:=</path/to/params/yaml>

Note

If you have already brought up the Kobuki with our kobuki_setup_comms_launch.py launch script, to for example teleoperate the robot, you can launch the above command with the comms argument set to false, to only launch the navigation and SLAM components.

$ ros2 launch slamcore_ros2_examples kobuki_live_navigation_launch.py \
>   session_file:=</path/to/session/file> \
>   config_file:=</path/to/slam/config/json> \
>   params_file:=</path/to/params/yaml> \
>   comms:=false

Visualization

You can visualize the robot navigating in the map on a separate machine by running:

Create 3

$ ros2 launch slamcore_ros2_create3_example create3_navigation_monitoring_launch.py

TurtleBot 4

$ ros2 launch slamcore_ros2_turtlebot4_example turtlebot4_navigation_monitoring_launch.py

Kobuki

$ ros2 launch slamcore_ros2_kobuki_example kobuki_navigation_monitoring_launch.py

Warning

In localisation mode, the occupancy map is published by our software in Quality of Service Durability Transient Local, therefore, if you launch our SLAM but cannot see the map in RViz2 open the Map item in the left sidebar of RViz2,then open the Topic drop down and change the Durability Policy to Transient Local.

Note

If the map is rendered in RViz2 but the robot model does not appear, you must manually drive the robot around until it relocalises in the loaded map.

We can see that the relocalisation took place by looking at the RViz2 view where the local and global costmaps start getting rendered, or by subscribing to the /slamcore/pose where we start seeing incoming Pose messages.

This is how the RViz2 view should look like after the robot has relocalised.

Fig. 75 RViz2 view during navigation

Once Nav2 is up and running, see Interact with the Navigation Demo below to learn how to set single goals or multiple waypoints for navigation.

Interact with the Navigation Demo

In RViz2 we can set individual navigation goals with the Nav2 Goal button or by publishing a Pose message to the /goal_pose topic. The robot will then try to plan a path and start navigating towards the goal if the path is feasible.

Fig. 76 Robot navigating towards single goal

It is also possible to issue multiple waypoints using Nav2’s Waypoint Mode button. When Waypoint Mode is selected, we can set multiple waypoints on the map with the Nav2 Goal button. When all waypoints have been set, press the Start Navigation button and the robot will attempt to navigate through all the waypoints.

Fig. 77 Waypoint mode

Nav2 Configuration

The <robot>-nav2-demo-params.yaml file (Create 3 file, TurtleBot 4 Standard file, Kobuki file) contains parameters, such as the planner and costmap parameters, that can be tuned to obtain the best navigation performance. The Nav2 docs include a Configuration Guide with information on the available parameters and how to use them.

In this file, we set the obstacle_layer and voxel_layer that are used in the global and local costmap for obstacle avoidance. The observation source for these is the /slamcore/local_point_cloud published by our software. This point cloud can be trimmed using a Slamcore JSON configuration file to, for example, remove points below a certain height that should not be marked as obstacles. More details on point cloud trimming can be found on the Point Cloud Configuration page.

Note

In addition to trimming the point cloud using a Slamcore JSON configuration file, we set the ROS obstacle layer’s min_obstacle_height parameter in nav2-demo-params.yaml to e.g. -0.18 for the Kobuki.

This parameter lets you set a height (measured from the map frame) from which all points above are considered valid and can be marked as obstacles. All points below are simply ignored (they are not removed, as is the case with point cloud trimming). As in this example the map frame is at the camera height, we want to make sure that points in the cloud that are below the camera (between camera height (23cm) and the floor) are included. If the map frame was on the ground level, min_obstacle_height could be kept at e.g. 0.05- only points 5cm above the map Z coordinate would be considered when marking obstacles.

This parameter can be used together with point cloud trimming, as is the case in this demo, or without point cloud trimming, if you would like to keep the raw local point cloud.

You may copy these files, found in the config folder, and adjust them to your setup. Remember to provide the path to the modified files when launching navigation to override the default ones.

Appendix

Troubleshooting

PS4 Button Mapping

Known Issues

In ROS 2 Foxy, RViz2 may crash when setting a new goal if the Controller visualization checkbox is enabled. (See https://github.com/ros2/rviz/issues/703 for more details)
When loading the Create 3 model in RViz2 you might see the following error Invalid frame ID "left_wheel" passed to canTransform argument source_frame or the bumper might appear offset. These issues have been reported in https://github.com/iRobotEducation/create3_sim/issues/125 and https://github.com/iRobotEducation/create3_sim/issues/170, and are being addressed with https://github.com/iRobotEducation/create3_sim/pull/200.

Nav2 Integration Guide

Outline

Robot-specific Features and Changes When Using Slamcore

Initial Robot Set Up and Configuration

slamcore-ros2-examples Metapackage Set Up

Visualization Machine Set Up

Teleoperation

Visual-Inertial-Kinematic Calibration

The slamcore/base_link ➞ base_link Transformation

Mapping and Live Navigation

Navigation in Single Session SLAM Mode

Navigation in Localisation Mode Using a Pre-recorded Map

Interact with the Navigation Demo

Nav2 Configuration

Appendix

Troubleshooting

PS4 Button Mapping

Known Issues

`slamcore-ros2-examples` Metapackage Set Up

The `slamcore/base_link` ➞ `base_link` Transformation