SLAMcore ROS1 Wrapper

Software Prerequisites

A working ROS1 installation is required before installing the SLAMcore ROS1 Wrapper. More specifically ros-melodic-ros-base should be installed while the desktop packages are optional. To get started with ROS1 you can follow the instructions provided here:

ROS1 Wrapper Layout

The ROS1 Wrapper allows you interaction with SLAMcore software via ROS Melodic. To allow for maximum flexibility it is split into two separate Debian packages:

  • slamcore-ros

    Consists of the core functionality for communicating with the SLAMcore SLAM software over ROS1 (publish estimated pose and map via ROS1 topics, reset SLAM via ROS services etc.) and has the minimum amount of package dependencies, namely the ros-base.

  • slamcore-viz

    Provides a set of nodes and GUIs to better visualise the overall SLAM process. More specifically, it offers a customised rviz view of the camera images, as well as the estimated pose and sparse map of the environment. It also allows interaction with the SLAM process via rqt (Reset SLAM, toggle streams, save session etc.).

This distinction makes slamcore-ros ideal if you want to use SLAMcore software over ROS1 but don’t want to bring the whole ROS1 visualisation stack as a dependency. On top of that you can optionally also install slamcore-viz to facilitate the SLAM visualisation.

Finally, here is a more detailed view of the components of each Debian package and how these translate into ROS packages and corresponding components:

slamcore_ros                           <-- Debian Package / ROS Metapackage

├── slamcore_bridge                    <-- ROS Package - handles the SLAMcore <-> ROS conversions

└── slamcore_slam                      <-- ROS Package - handles Interaction with the SLAM engine

    └── slam_publisher                 <-- Main ROS node handling the publishing of SLAM data
    └── dataset_recorder               <-- ROS node handling the recording of datasets
    └── run_slam.launch                <-- Launchfile - wraps slam_publisher
    └── run_dataset_recorder.launch    <-- Launchfile - wraps dataset recorder

slamcore_viz                           <-- Debian Package / ROS Package

├── setup_monitoring.launch            <-- Main launchfile - Delegates to rqt, rviz accordingly
├── rqt/slam.perspective
├── rviz/slamcore.rviz
└── scripts
    ├── local_publisher.py             <-- Accumulates individual pose messages, dumps nav_msgs::Path
    │                                      messages to visualise in rviz.

    └── republish_viz_node.py          <-- Republishes /slamcore/{Visible,Infrared}_N messages
                                           on /slamcore/imageN

Installation

Obtain the ROS1 Wrapper package(s) 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-melodic-slamcore-ros*.deb
#                       ^~~~~ Specify the .deb file.

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

Repeat the installation step if you also want to install slamcore-viz.

Usage

Our ROS1 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 ROS1 nodes or launchfiles provided.

SLAM Mode

Following is an example use case of running SLAM. Before running it, make sure you have plugged in a registered camera.

$ source /opt/ros/melodic/setup.bash

# connect to the camera and run SLAM
$ roslaunch slamcore_slam run_slam.launch

# Verify that it's streaming data
$ rostopic echo /slamcore/pose

# Expect output in the form:
#   header:
#     stamp:
#       sec: 377
#       nanosec: 598881764
#     frame_id: slamcore_map
#   pose:
#     position:
#       x: -0.02378664211846218
#       y: 0.041697142300108944
#       z: -0.10841154367750798
#     orientation:
#       x: 0.12618178364483054
#       y: -0.6937589654026391
#       z: 0.12688238138290922
#       w: 0.6976227610207906

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

$ rosservice call /slamcore/save_session

The session maps are saved in the ~/.ros/ directory by default.

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 ROS1 parameter:

# set parameter and run
$ roslaunch slamcore_slam run_slam.launch 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 ROS1 node, e.g.:

$ roslaunch slamcore_slam run_slam.launch session_file:=/home/<user>/<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):

$ roslaunch slamcore_slam run_slam.launch generate_map2d:=true

Dataset Recorder

To record a dataset in our ROS1 wrapper run:

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

See the Troubleshooting page to help diagnose common errors.

ROS1 Visualisation

Note

Please install the slamcore-viz Debian package to use the tools described in this section.

You can visualise the advertised topics as well as interact with SLAM via the slamcore_viz ROS1 package and its setup_monitoring.launch launchfile.

First, launch the SLAMcore ROS1 node to record a dataset or start SLAM as mentioned above. Then, to enable the visualisation in rviz, run on another terminal:

$ roslaunch slamcore_viz setup_monitoring.launch

OR to enable visualisation in rviz and control SLAM execution in rqt:

$ roslaunch slamcore_viz setup_monitoring.launch launch_rqt:=true

When executed, rviz and rqt are launched for visualising and controlling SLAM execution respectively. Following are sample rviz and rqt views when launching:

_images/rviz_view.png

Fig. 14 RVIZ - User Interface

_images/rqt_view.png

Fig. 15 RQT - User Interface

ROS1 API

This section outlines the topics, services, and parameters that the SLAMcore ROS1 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/local_point_cloud
  • /slamcore/metadata/{num_features, tracked_features, tracking_status}
    • Type: std_msgs/Int64

    • Description: Metadata published periodically by the SLAM algorithm. (see FAQs for

      more information on what the numbers refer to)

  • /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:

$ rostopic list

Advertised Services

Following 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:

$ rosservice call /slamcore/<service-name>

List of advertised services:

  • /slamcore/reset_signal
    • Type: std_srvs/Trigger

    • Description: Reset SLAM execution. After this call, SLAM automatically restarts.

      If you are running from a dataset this call will reset the dataset to the beginning.

  • /slamcore/save_session
    • Type: std_srvs/Trigger

    • Description: Save session map file (saved in the ~/.ros/ 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:

$ rosservice list

Advertised Parameters

Following is the list of parameters that slam_publisher fetches during initialisation. For each one of the mentioned parameters there is a corresponding roslaunch argument when using run_slam.launch or run_dataset_recorder.launch. Thus there are two ways of setting them:

# set the parameter manually before the run
$ rosparam set /slamcore/<param-name> <desired-value>       # for run_slam.launch OR
$ rosparam set /dataset_recorder/<param-name> <desired-value> # for run_dataset_recorder.launch

$ rosrun slamcore_slam slam_publisher

# Alternatively, set it via the corresponding roslaunch argument
$ roslaunch slamcore_slam run_slam.launch <param-name>:=<desired-value>             # for run_slam.launch OR
$ roslaunch slamcore_slam run_dataset_recorder.launch <param-name>:=<desired-value> # for run_dataset_recorder.launch

Note

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

  • /slamcore/advertise_images
    • Type: Boolean

    • Default: True

    • Description: Advertise images

  • /slamcore/config_file
  • /slamcore/dataset_path
    • Type: String

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

  • /slamcore/dataset_timescale
    • Type: Float

    • 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/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/realsense_depth_override_value
    • Type: Boolean

    • Default: True

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

  • /slamcore/enable_color
    • Type: Boolean

    • Default: False

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

  • /slamcore/publish_hw_timestamps
    • Type: Boolean

    • Default: False

    • Description: Specify whether to publish measurements with hardware or system-clock based timestamps (details)

  • /slamcore/session_file
    • Type: String

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

  • /slamcore/session_save_dir
    • Type: String

    • Default: ~/.ros

    • Description: Specify directory to save session map at. If none is provided, maps will be saved to the node’s current working directory, usually ~/.ros.

  • /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.

  • /slamcore/generate_map2d
    • Type: Boolean

    • Default: False

    • Description: Generate a new map

  • /slamcore/base_frame
  • /slamcore/map_frame
  • /slamcore/odom_frame

Host and device timestamps

To abide by the ROS1 common practices, and to facilitate using the SLAM output in other ROS1 nodes, the slam_publisher node publishes messages timestamped in the host computer timeline (using the synchronised camera timestamp if the hardware supports it; this is currently only supported for the D435i and D455). Alternatively, you can switch to the original camera timestamps by using the /slamcore/publish_hw_timestamps ROS1 parameter:

# set parameter and run
$ roslaunch slamcore_slam run_slam.launch publish_hw_timestamps:=true

Note

The slam_publisher adjusts the hardware timestamps in this mode to ensure the clock is monotonic. This differs from the internal timestamps of the D435i.

See Fig. 16 below for more details on the differences between the two types of timestamps:

_images/camera_host_timelines.png

Fig. 16 Camera and Host Timelines

To make a clear separation and avoid mixing data from different timelines, if you instruct the slam_publisher to use the hardware timestamps, then the names of the advertised topics will be suffixed with _hw.

$ roslaunch slamcore_slam run_slam.launch publish_hw_timestamps:=true

$ rostopic list
/rosout
/rosout_agg
/slamcore/accel_hw
/slamcore/depth_hw/camera_info
/slamcore/depth_hw/image_raw
/slamcore/gyro_hw
/slamcore/infrared_0_hw/camera_info
/slamcore/infrared_0_hw/image_raw
/slamcore/infrared_1_hw/camera_info
/slamcore/infrared_1_hw/image_raw
/slamcore/map_hw
/slamcore/odom_hw
/slamcore/pose_hw

Note

ROS1 Visualisation is not compatible with hardware timestamps.