Running a ROS2 Robotics Simulation with Nav2#

The SVL Simulator robotics release adds support for robots as ego vehicles. Robots differ from ego vehicles in the way their motion is controlled. The robotics release of the simulator makes use of Unity's Articulation Body physics components to realistically model joints for robotic movements. This robotics release of the SVL Simulator also provides an ego vehicle, the LG CLOi robot, and an accompanying control plugin that defines the robot motion.

Autonomous navigation of the robot in simulated environments is possible using the simulator with the ROS 2 Navigation stack (Nav2). This document guides users through setting up and running a simulation alongside Nav2.

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.

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.

In addition, the launch file in the package can optionally use pointcloud_to_laserscan to convert the single beam LiDAR's PointCloud2 message to a LaserScan message type using by passing an command line argument. This node is not launched by default.

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

Setting up a Simulation top#

To run a simulation, we must have an ego vehicle (CLOi robot) and an environment available on the web user interface as well as a valid sensor configuration. We will go through the steps of setting these up here. To access the web user interface, run the Simulator executable and click OPEN BROWSER. Note that you may need to link to cloud and build an account/login if you have not already done so.

CLOi robot ego vehicle top#

The CLOi robot is available by default in the store and is located under Store > Vehicles in the web user interface. The ego vehicle includes the implementation of the vehicle dynamics for the robot. See dynamics documentation to read more about this.

The steps here are only provided to give insight into how the sensor configuration is setup and how changes can be made. You may skip this section if you wish to quickly run a simulation.

vehicle_store

If it is not already added to your library click the red button with a + sign on it to add it. Click on the model to expand it and under Sensor Configurations choose the Navigation2 configuration. The Navigation2 sensor configuration is a basic sensor configuration for running the simulator with Nav2 that has the following sensors:

sensor_config

We will go through the noteworthy details of each sensor below.

Clock Sensor top#

Click on the Clock Sensor to view its parameters.

clock

The only notable parameter here is the topic which must be set to /clock. ROS2 will use the time provided in this topic as reference time when the use_sim_time parameter is set to true.

LiDAR2D Sensor top#

Click on the LiDAR Sensor to view its details.

The position of the sensor is set to {0.27, 0, 0.1125}. These are the coordinates of the sensor relative to base_link.

The CenterAngle is set to 0. The RotationFrequency is set to 10 and the Compensated parameter was marked as No. The Topic name is set to /scan and the Frame name is set to base_scan.

lidar2d

LiDAR Sensor top#

The LiDAR sensor published pointcloud messages which are not the type of message Nav2 looks for by default. It is, however, possible to convert the message to laserscan using the ROS 2 package pointcloud_to_laserscan. The Nav2_PointCloud sensor configuration uses this lidar instead of the LiDAR2D sensor and is configured to be used with the pointcloud_to_laserscan node.

lidar

The position of the sensor is set to {0.27, 0, 0.1125}. These are the coordinates of the sensor relative to base_link.

The LaserCount parameter is set to 1 which is the number of beams the LiDAR will have. To simulate a planar LiDAR we set this value to 1 and also set the CenterAngle parameter to 0. The RotationFrequency was set to 10 (Hz) and the Compensated parameter was marked as No. The Topic name is set to /cloud_in which is the topic name the pointcloud_to_laserscan node expects. Frame name is set to base_scan which is the same frame that will be passed on to the LaserScan message which Nav2 expects.

Destination Sensor top#

The destination sensor will not be used in this tutorial. The sensor enables users to programmatically set the initial pose and desired destination of the robot through Python API.

destination_sensor

Differential Drive Control Sensor top#

The Differential Drive Control Sensor is responsible for subscribing to control commands from Nav2 and driving the robot as well as publishing the Odometry message. Click on the sensor to view its details.

diffdrive

This sensor translates a commanded velocity pose into motion for each of the two drive wheels on the robot. To do this the sensor requires the path to the links for the left and right wheels of the robot model. For the CLOi robot model these paths are LeftWheelLinkPath: link_MainBody/SuspensionLeft/link/wheel_left/link and RightWheelLinkPath: link_MainBody/SuspensionRight/link/wheel_right/link.

To find the path for a different robot the user would need to open the robot in the Unity Editor and take note of the path between the main body of the robot and each of the drive wheels. For example, the image below shows the expanded hierarchy for the CLOi robot in the Unity Editor.

cloi_tree

The gain values for the PID controller can also be set here, and are by default set to P_Gain: 1, I_Gain: 0.05, D_Gain: 0.

The OdometryTopic is /odom and the OdometryChildFrame is set to odom. The Frame name is set to base_footprint.

The Topic name which refers to the Control Command topic coming from Nav2 is set to /cmd_vel.

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

After completing the setup outlined above follow these steps to run a simulation with Nav2.

  • 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

  • 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

This launch file will start the lgsvl_bridge node, the pointcloud_to_laserscan node, the odom_tf node and a set of static tf nodes.

Note: Users who wish to use the Nav2_PointCloud sensor configuration, should use the following command to launch the required nodes, including pointcloud_to_laserscan:

bash ros2 launch svl_robot_bringup robot_tf_launch.py pointcloud:=true

  • In a new terminal launch Nav2:
source /opt/ros/foxy/setup.bash
ros2 launch nav2_bringup bringup_launch.py map:=robot_ws/src/svl_robot_bringup/maps/seocho.yaml params:=robot_ws/src/svl_robot_bringup/params/nav2_params.yml
  • In a new terminal launch rviz:
source /opt/ros/foxy/setup.bash
ros2 launch nav2_bringup rviz_launch.py rviz_config:=robot_ws/src/svl_robot_bringup/rviz/nav2_cloi.rviz
  • In rviz you will need to set the initial pose using the 2D Pose Estimate button. You will need to mark the current position of the robot on the map in rviz. The default starting position in the Seocho map is shown below.

inital_pose

Once the pose is set, the costmaps will be loaded and the particle cloud from AMCL will be displayed.

cost_map

  • In rviz give the robot a destination using the 2D Goal Pose button.

  • The robot should now navigate to its destination.