SLAMcore ROS2 Wrapper

Introduction

The SLAMcore ROS2 Wrapper allows interaction with SLAMcore algorithms via ROS2 Foxy. There are two different options to use the ROS2 wrapper:

  1. Install ROS2 Foxy on your host system (only Ubuntu 20.04 is supported)

    In this case, a working ROS2 installation is required before installing the SLAMcore ROS2 Wrapper. More specifically ros-foxy-ros-base should be installed while the ros-foxy-desktop package is optional.

    To get started with ROS2, follow the instructions provided here: https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html

  2. Use SLAMcore’s ROS2 Docker image (this supports Ubuntu 18.04, 20.04 and NVIDIA Jetson)

    In this case, no ROS2 installation is required before installing the SLAMcore ROS2 wrapper.

    1. Download the SLAMcore ROS2 Wrapper for NVIDIA Jetson from the Download SLAMcore Software link at the SLAMcore Portal.

    2. Clone the slamcore-ros2-docker repository and follow the instructions on GitHub to build SLAMcore’s ROS2 Docker image.

    $ git clone https://github.com/slamcore/slamcore-ros2-docker.git
    

Below we describe how to use SLAMcore’s ROS2 wrapper on Ubuntu 20.04 with natively installed ROS2 Foxy. Details for running with the Docker image are provided at slamcore-ros2-docker.

Installation on Host System (Ubuntu 20.04)

Warning

This is required when installing ROS2 locally and supported on Ubuntu 20.04.

Obtain the ROS2 Foxy Wrapper package from the Download SLAMcore Software link at the SLAMcore Portal.

Install the packages using apt or an apt-based tool to automatically resolve dependencies.

# Update the list of available packages
$ sudo apt update

# To understand what will be installed, use a command like:
$ apt install --dry-run /path/to/ros-foxy-slamcore-ros*.deb
#                       ^~~~~ Specify the .deb file.

# To Install the slamcore_ros Debian package, use a command like:
$ sudo apt install /path/to/ros-foxy-slamcore-ros*.b
#                  ^~~~~~~~~~ Specify the .deb file.

Usage

Our ROS2 node streams poses only when the tracking status is OK.

Note

Make sure you have sourced the setup.bash (adjust suffix according to your shell) before running any of the RO2 nodes or launchfiles provided.

SLAM Mode

Here we provide an example use case of running SLAM. Before running it, make sure you have connected a registered, supported camera.

$ source /opt/ros/foxy/setup.bash
# connect to the camera and run SLAM
$ ros2 launch slamcore_slam slam_publisher.launch.py
# Verify that it's streaming data
$ ros2 topic echo /slamcore/pose

Expect output in the form:

# header:
#   stamp:
#     sec: 1627314066
#     nanosec: 961842688
#   frame_id: world
# pose:
#   position:
#     x: -0.00044667969888344613
#     y: -0.00023092165271676365
#     z: 0.0003275341432887679
#   orientation:
#     x: 0.059299501965805336
#     y: 0.003310471496864338
#     z: -0.0005325452114257774
#     w: 0.998234604810249

To save a SLAM session in ROS2, call the /slamcore/save_session service:

$ ros2 service call /slamcore/save_session std_srvs/srv/Trigger

The session file are saved by default in the working directory.

SLAM using a recorded dataset

To run SLAM using a prerecorded dataset instead of a live camera you have to set the path to the dataset using either the dataset_path roslaunch argument or using the /slamcore/dataset_path ROS2 parameter:

# set parameter and run
$ ros2 launch slamcore_slam slam_publisher.launch.py dataset_path:=</path/to/dataset>

Localisation Mode

In localisation mode, our system uses a previously created session map. To load a session map, provide the full path to the session file when launching the SLAM wrapper ROS2 node, e.g.:

$ ros2 launch slamcore_slam slam_publisher.launch.py session_file:=<path/to/session/file>

Height Mapping Mode

In mapping mode, our system runs in SLAM mode but also generates a height map and an occupancy map which can be used in autonomous navigation.

To generate a height map, set the generate_map2d parameter to true and ensure the depth stream is enabled (true by default):

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

Dataset Recorder

To record a dataset in our ROS2 wrapper run:

$ ros2 launch slamcore_slam run_dataset_recorder.launch.py output_dir:=/home/<user>/<output-directory>

By default, the dataset will be saved in the current working directory, unless the output_dir argument is specified. When recording a dataset, you can also provide a config file, for example to adjust the camera/depth settings config_file:=$HOME/depth_preset.json.

See the Troubleshooting page to help diagnose common errors.

ROS2 API

This section outlines the topics, services, and parameters that the SLAMcore ROS2 Wrapper uses as either inputs or outputs.

Advertised Topics

List of topics advertised by slam_publisher:

  • /diagnostics
  • /slamcore/accel
    • Type: sensor_msgs/Imu

    • Description: Accelerometer data, interpolated to align with the gyroscope measurements

  • /slamcore/gyro
  • /slamcore/{infrared, visual, depth}{, _0, _1, …}/camera_info
  • /slamcore/{infrared, visual}{_0, _1, …}/image_raw
  • /slamcore/depth/image_raw
    • Type: sensor_msgs/Image

    • Description: Depth data. Data is provided with float encoding, where each pixel value is the depth in metres

  • /slamcore/metadata/{num_features, tracked_features, tracking_status}
    • Type: std_msgs/Int64

    • Description: Metadata published periodically by the SLAM algorithm.

  • /slamcore/metadata/{distance_travelled}
    • Type: std_msgs/Float64

    • Description: Distance travelled (metres) published periodically by the SLAM algorithm.

  • /slamcore/pose
  • /slamcore/odom
    • Type: nav_msgs/Odometry

    • Description: Pose, angular velocity and twist of the sensor

  • /slamcore/sparse_map
  • /slamcore/map
  • /tf
  • /tf_static

Run the following to view the full list of topics:

$ ros2 topic list

Advertised Services

Below is the list of services advertised by slam_publisher. These are used mainly for interaction of the client/user with the SLAM machine. Run the following to call the service:

$ ros2 service call /slamcore/<service-name> <service_type>

List of advertised services:

  • /slamcore/save_session
    • Type: std_srvs/Trigger

    • Description: Save session map file (saved in the current working directory by default).

  • /slamcore/static_map
    • Type: nav_msgs/GetMap

    • Description: Retrieve the static map (only advertised if loading from a session file).

Run the following to view the full list of services:

$ ros2 service list -t

Advertised Parameters

Below is the list of parameters that slam_publisher fetches during initialisation. To set the parameters:

# set the parameter via the corresponding roslaunch argument
$ ros2 launch slamcore_slam slam_publisher.launch.py <param-name>:=<desired-value>   # for slam_publisher.launch.py OR
$ ros2 launch slamcore_slam dataset_recorder.launch.py <param-name>:=<desired-value> # dataset_recorder.launch.py

Run the following to view the full list of advertised parameters:

$ ros2 launch slamcore_slam slam_publisher.launch.py --show-args
$ ros2 launch slamcore_slam dataset_recorder.launch.py --show-args

Note

The full path(s) must be provided for the relevant parameter values e.g. /home/<user>/<path/to/file>.

  • /slamcore/slam_node/advertise_images
    • Type: Boolean

    • Default: True

    • Description: Advertise (publish) images

  • /slamcore/slam_node/config_file
    • Type: String or a List of strings

    • Description: Path to the configuration/preset file(s). One can provide a single config file, e.g. config_file:=/usr/share/slamcore/presets/outdoor.json, or multiple config files as a list config_file:=[/usr/share/slamcore/presets/outdoor.json,$HOME/depth_preset.json]

  • /slamcore/slam_node/dataset_path
    • Type: String

    • Description: Path to the recorded dataset to run SLAM with (details).

  • /slamcore/slam_node/dataset_timescale
    • Type: Float

    • Default: -1 (runs as fast as possible).

    • Description: Control the playback speed by selecting a playback timescale. Setting it to values greater than zero sets a corresponding playback speed, and setting the timescale to 1.0 will play back in emulated real time. If no timescale is specified the playback will run as fast as possible to process every frame.

  • /slamcore/slam_node/override_realsense_depth
    • Type: Boolean

    • Default: False

    • Description: Overwrite the depth stream, and therefore the depth topic for a RealSense, with the value provided with /slamcore/realsense_depth_override_value camera. See FAQs for more information.

  • /slamcore/slam_node/realsense_depth_override_value
    • Type: Boolean

    • Default: False

    • Description: Overwrite the depth stream, and therefore the depth topic for a RealSense camera. See FAQs for more information.

  • /slamcore/slam_node/enable_color
    • Type: Boolean

    • Default: False

    • Description: Publish RGB image. Please contact support@slamcore.com to enable this feature on your SLAMcore account.

  • /slamcore/slam_node/session_file
    • Type: String

    • Description: Path to the session map to load to run in localisation mode (details).

  • /slamcore/slam_node/session_save_dir
    • Type: String

    • Default: current working directory

    • Description: Specify directory to save session map at. If none is provided, maps will be saved to the current working directory.

  • /slamcore/slam_node/generate_map2d
    • Type: Boolean

    • Default: False

    • Description: Generate a new 2D map.

  • /slamcore/slam_node/odom_reading_topic
    • Type: String

    • Description: Topic to read input wheel odometry from.

  • /slamcore/slam_node/world_frame
    • Type: String

    • Default: world

    • Description: Name given to the frame_id the pose is published relative to.

  • /slamcore/slam_node/odom_frame
    • Type: String

    • Default: odom

    • Description: Name given to the frame_id of the smooth world frame published on /tf (see REP-105).

  • /slamcore/slam_node/base_frame
    • Type: String

    • Description: Name given to the frame_id the pose refers to.

  • /dataset_recorder/output_dir
    • Type: String

    • Default: ~/.ros/output

    • Description: Specify directory to save the recorded dataset at. If none is provided, maps will be saved to the default directory.

  • /dataset_recorder/odom_reading_topic
    • Type: String

    • Default: /odom

    • Description: Odometry input topic to subscribe to. The slam_publisher and dataset_recorder will listen on this topic for nav_msgs/Odometry messages.