Mapping an environment in ROS 2#

Many robots operate in pre-mapped environments. In Nav2 the map of the environment is used both for localization and for generating a costmap for motion planning. This document demonstrates how to create a map of the environment using SLAM toolbox.

Table of Contents

Prerequisites top#

  • SVL Simulator release 2021.2.2 or later
  • Linux operating system (preferably Ubuntu 18.04 or later)

For a guide to setting up and using the simulator see the Getting Started guide. For other robotics guides see Running a basic Robotics Simulation with ROS 2 and Running a ROS2 Robotics Simulation with Nav2.

Installing ROS 2 top#

ROS2 can be installed by following the steps in the official installation guide.

SVL Robot Startup top#

Robots often have many 3D coordinate frames that change in position and orientation over time. tf2 is a transform library used to keep track of the relative transformation between these coordinate frames in ROS/ROS 2. The simulator does not directly broadcast tf transforms. Therefore, an external ROS2 node is needed for this. svl_robot_startup is a ROS2 package that publishes static transforms for the robot sensors and subscribes to the /odom topic published by the simulator to figure out and broadcast the transform between the odom and basefootprint frames.

The SVL Simulator's ROS 2 bridge lgsvl_bridge is included as a dependency and added to the launch file to enable communication with the simulator.

The navigation2 and nav2_bringup packages are also added as dependencies as they will be needed for running Nav2.

Follow these steps to setup the package and install dependencies:

source /opt/ros/foxy/setup.bash
mkdir -p robot_ws/src
cd robot_ws/src
git clone https://github.com/lgsvl/svl_robot_bringup.git
cd ..
rosdep update
rosdep install --from-path src -iy --rosdistro foxy
colcon build --symlink-install

Installing Nav2#

Nav2 be installed by rosdep in the previous step. If, for whatever reason, the installation was skipped and can be manually installed using the following commands.

sudo apt update
sudo apt install ros-foxy-navigation2 ros-foxy-nav2-bringup

Installing SLAM toolbox#

SLAM toolbox provides a set of open-source tools for 2D SLAM which will be used in this tutorial for mapping the environment. It can be built from source (follow instructions on GitHub) or installed using the following command:

sudo apt install ros-foxy-slam-toolbox

Setting up a Simulation top#

Click on the Simulations tab and click Add New to create a new simulation.

  • Enter a Simulation Name and select the Cluster. Click Next.
    • Optional - Enable Interactive Mode to be able to start the simulation at the desired time by pressing a play button in the simulator screen.
  • Select the Random Traffic Runtime Template. Select LGSeocho under Map (if it is not present you will need to go back to the store and add the map to your library). Select the LGCloi ego vehicle and the Nav2 Sensor Configuration. Click Next.
  • Select Other ROS2 Autopilot under Autopilot and enter the Bridge Connection address (by default localhost:9090).
  • Click Publish

Mapping the environment#

  • In a new terminal start the robot_tf_launch:
source /opt/ros/foxy/setup.bash
cd robot_ws
source install/setup.bash
ros2 launch svl_robot_bringup robot_tf_launch.py

  • Start the simulation in the SVL Simulator

  • Run the Simulator binary and click OPEN BROWSER

  • In the web user interface, open the Simulations page, select the recently built simulation and click Run Simulation

robot_at_spawn_point

  • Launch navigation_launch fron Nav2:
source /opt/ros/foxy/setup.bash
ros2 launch nav2_bringup navigation_launch.py
  • Launch slam toolbox:
souce /opt/ros/foxy/setup.bash
ros2 launch slam_toolbox online_async_launch.py
  • Launch rviz:
source /opt/ros/foxy/setup.bash
ros2 launch nav2_bringup rviz_launch.py
  • You can start mapping by either giving destinations using the Navigation2 Goal button in rviz or by manually driving the robot around using the keyboard.

mapping

  • Once satisfied with the map it can be saved using:
ros2 run nav2_map_server map_saver_cli -f ~/map

Note on saving maps: Map saving tends to fail due to timeout. Attempting to save multiple times can result in a successful map save. See this issue on GitHub for more information.

Setting NavOrigin for your generated costmap#

In order to be able to use the python API functionalities for setting an initial pose or a destination of a robot along with your generated costmap, a NavOrigin game object should be placed in a scene such that its transform matches with the origin of the costmap you generated. Also, it should have the same offsets as the ones in your costmap yaml file.

To find transform and offset values of your costmap origin:

  • Transform of the origin: Coordinates of the lower-left pixel in the generated map image file.

LGSeocho costmap image

  • Offsets of the origin: The generated YAML file contains an offset for your origin. The offsets are given in 2D position with a third value indicating a rotation.

LGSeocho costmap YAML

How to place NavOrigin using the VSE tool#

You can use the Visual Scenario Editor (VSE) tool of SVL Simulator for a placement of NavOrigin in a scene following the instructions below:

  1. Launch SVL Simulator

  2. Click on the Visual Editor button to start the VSE tool

  3. Select your map in the Map section (e.g., LGSeocho)

  4. Place NavOrigin on a map by clicking the ScenarioNavOrigin button in the Add section

  5. Move and rotate the NavOrigin so that its placement matches with your costmap origin

  6. Write down the x/y/z position and rotation values of the NavOrigin (e.g., in the attached image below, position is (53.4, 0, -57.6) and rotation is ((0, 0, 0)))

NavOrigin placement

Example usage in Python API script:#

import lgsvl
from lgsvl import Vector, Quaternion, Transform

# Establish a connection to SVL Simulator
sim = lgsvl.Simulator(lgsvl.wise.SimulatorSettings.simulator_host, lgsvl.wise.SimulatorSettings.simulator_port)

# Load LGSeocho map
scene = lgsvl.wise.DefaultAssets.map_lgseocho
if sim.current_scene == scene:
    sim.reset()
else:
    sim.load(scene)

# Set up LGCloi robot in a scene
state = lgsvl.AgentState()
state.transform = sim.get_spawn()[0]
robot = sim.add_agent(lgsvl.wise.DefaultAssets.ego_lgcloi_navigation2, lgsvl.AgentType.EGO, state)
robot.connect_bridge(lgsvl.wise.SimulatorSettings.bridge_host, lgsvl.wise.SimulatorSettings.bridge_port)

# Use x/y/z position and rotation values of NavOrigin
transform = Transform(Vector(53.4, 0, -57.6), Vector(0, 0, 0))
# Use offsets from a costmap YAML file
offset = Vector(-14.7, -24.6, 0)
# Set NavOrigin in a scene
sim.set_nav_origin(transform, offset)

# Get NavOrigin from a scene and verify its values
nav_origin = sim.get_nav_origin()
print('NavOrigin transform:', nav_origin['transform'])
print('NavOrigin offset:', nav_origin['offset'])

# Map a destination point in ROS coordinates to Simulator coordinates
dst_ros = ((7.118, -0.016, 0), (0, 0, 0.0004, 0.999))
dst_sim = sim.map_from_nav(Vector(*dst_ros[0]), Quaternion(*dst_ros[1]))

# Set initial pose of robot
robot.set_initial_pose()
sim.run(5)

# Set destination for Navigation2 stack
robot.set_destination(dst_sim)
sim.run()