Stage_Ros2 simulation platform

stage_ros2 is a ROS2 package based on the Stage simulator, used to simulate and control robot behavior in a virtual environment. It provides powerful tools and communication interfaces that can be used to develop, test, and validate robotic systems. Through stage_ros2, users can easily create virtual environments and use ROS 2 to send commands to control robot motion while receiving sensor data. This package helps users develop robots at lower cost and risk, providing an efficient and flexible robot simulation environment.

stage User Manual:

http://rtv.github.io/Stage/modules.html

( You don't have to learn it , even ROS2 has already removed Stage officially, use the more awesome Gazebo instead , but it's not a big deal to learn it as well)

1.stage_ros2安装与运行

1. Installation

Since ROS2 has officially removed Stage, we need to manually install it by compiling from source. Someone has already adapted the Stage for ROS2 version:

First, run the following command to install dependencies:

sudo apt-get install git cmake g++ libjpeg8-dev libpng-dev libglu1-mesa-dev libltdl-dev libfltk1.1-dev

Enter the ROS2 workspace's src directory and run the following command to download the relevant repository:

git clone https://github.com/damuxt/Stage.git
git clone https://github.com/damuxt/stage_ros2.git

Enter the workspace directory and use thecolcon buildcommand to build.

(This is a third-party author's adaptation for the Humble version; the Jazzy version is not supported for now. The Jazzy version currently fails to compile, and the author may update it for the Jazzy version.)

2. Node Description

stage_ros2The node is one of the core nodes of the stage_ros2 package. It creates a simulation scene by loading a world file, including elements such as the map, obstacles, and robot. The world file describes the properties of the virtual environment, and the node builds the corresponding environment according to the description in the file. The stage_ros2 node uses this virtual environment to simulate the robot's motion, perception, and control. Through the combination of this node and the world file, robot simulation and testing can be performed.

3. Use

Under the feature package, usage examples are already built in. You can run the following command in the terminal to start the example:

ros2 launch stage_ros2 my_house.launch.py

The simulator and rviz2 will be launched, and in rviz2, relevant information such as odometry, LiDAR, and the depth camera can be displayed. The running results are as follows.

2.stage_ros2 Simulation framework construction

Starting from this section, we will introduce how to customize the simulation environment through stage_ros2.

1. Preparatory work

First, please create a function package with the following command:

ros2 pkg create demo_stage_sim --dependencies stage_ros2

Then create new launch, world, and config directories under the package, and modify the CMakeLists.txt file under the package to add the following code:

install(DIRECTORY launch world config DESTINATION share/${PROJECT_NAME})

Among them, the launch directory is used to store launch files, config is used to store the rviz2 configuration files, and world is used to store simulation-related files. In the world directory, create a new maps directory and copy the image assets from the course accompanying materials into it.

2. Build the code framework

In the launch directory of the package, create a new stage_sim.launch.py file and enter the following content:

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    this_directory = get_package_share_directory('demo_stage_sim')
return LaunchDescription([
        Node(
            package='stage_ros2',
            executable='stage_ros2',
            name='stage',
            parameters=[{"world_file": os.path.join(this_directory,'world','sim.world')}],
        )
    ])

This launch file will run the stage_ros2 node and load the sim.world file from the world directory.

In the world directory, create a new sim.world file and enter the following content:

# -----------------------------------------------------------------------------

# 设置窗体

# 设置模拟器地图分辨率(以 米/像素 为单位)
resolution 0.02

# 设置模拟器的时间步长(以 毫秒 为单位)
interval_sim 100

sim.world文件当前只是声明了仿真环境的一些通用参数。

3. Compile

In the terminal, enter the current workspace and compile the package:

colcon build --packages-select demo_stage_sim

4. Execute

In the current workspace, start a terminal and enter the following command:

. install/setup.bash
ros2 launch demo_stage_sim stage_sim.launch.py

After executing this launch file, a window will be generated. The window will not contain any content. Next, we can begin creating the specific simulation content.

5.world file parameters

Abstract and default values

name                      <worldfile name>
interval_sim              100
quit_time                 0
resolution                0.02
show_clock                0
show_clock_interval       100
threads                   0

Detailed explanation

• name

• The identifying name for the world, for example used in the title bar of the graphical user interface.

• interval_sim

• The amount of simulated time that runs each time World::Update() is called. Each model has its own configurable update interval, which may be greater than or less than this value, but intervals shorter than this value are not visible in the graphical user interface or in the World update callback. You probably do not need to change the default value of 100 milliseconds: this value is used internally by clients such as Player and WebSim.

• quit_time Stops the simulation after the simulated time reaches the specified number of seconds. In libstage, World::Update() returns true. In Stage with a graphical user interface, the simulation is paused. In Stage without a graphical user interface, Stage exits.

• resolution The resolution of the underlying bitmap model, in meters. Larger values can speed up ray tracing, but at the cost of collision detection and sensing accuracy. In general, the default value is a reasonable choice.

• show_clock

• If nonzero, prints the simulation time to standard output every $show_clock_interval updates. This is very useful for observing the progress of non-GUI simulations.

• show_clock_interval

• Sets the number of updates between printing the time to standard output when $show_clock is enabled. The default is to print once every 10 seconds of simulated time. Smaller values slightly reduce simulation speed.

• threads

• The number of worker threads to spawn. Some models can be updated in parallel (e.g., lasers, rangefinders), and running 2 or more threads here may make the simulation run faster, depending on the number of available CPU cores and the world file. If you have high-resolution models with parallelism enabled (e.g., lasers with hundreds or thousands of samples, or a large number of models), use one thread per core.

3.stage_ros2设置窗体

The stage window consists of a menu bar and a view of the simulated world. We can zoom in and out of the view and scroll the view to see more of the simulated world. The simulated robot devices, obstacles, etc. are rendered as colored polygons. The window can also implement visualization of data and configurations for various sensor and actuator models. The menu has options for controlling which data and configurations are displayed. In this section, we will introduce how to set up and operate the stage window.

1. Setting the Form

To set form-related parameters, you can add the following content to the stage_sim.launch.py file:

# 配置窗体参数
window
(
  size [ 700.000 700.000 ] # 窗体尺寸(以 像素 为单位)
  scale 35  # 缩放比
  center [ 0  0 ] # 地图相对于窗体的偏移量(以 米 为单位)
  rotate [ 0  0 ] # 地图旋转角度
  show_data 1     # 是否显示传感器数据 1=on 0=off
)

The above code creates a window object with a resolution of 700\*700 and a scaling ratio of 35; the map subsequently loaded has no offset or rotation relative to the window and will display the sensor data visually.

2. Compile and execute

After building the function package and executing the launch file, the run result is as follows.

3. Form parameters

Abstract and default values

window
(
  size [ 400 300 ]

  # camera options
  center [ 0 0 ]
  rotate [ 0 0 ]
  scale 1.0

  # perspective camera options
  pcam_loc [ 0 -4 2 ]
  pcam_angle [ 70 0 ]

  # GUI options
  show_data 0
  show_flags 1
  show_blocks 1
  show_clock 1
  show_footprints 0
  show_grid 1
  show_trailarrows 0
  show_trailrise 0
  show_trailfast 0
  show_occupancy 0
  show_tree 0
  pcam_on 0
  screenshots 0
)

Detailed explanation

• speedup

• Stage will attempt to run at this multiple of real time. If set to -1, Stage will run as fast as possible and will not attempt to track real time.

• size $$<width:int><height:int>$$

• The size of the window (in pixels).

• center $$<x:float><y:float>$$

• The position of the window center, expressed in world coordinates (in meters).

• rotate $$<pitch:float><yaw:float>$$

• The rotation angle relative to vertical up, in degrees.

• scale

• The ratio of world coordinates to pixel coordinates (window scaling).

• pcam_loc $$<x:int><y:int><z:int>$$

• The position of the perspective camera (in meters).

• pcam_angle $$<pitch:float><yaw:float>$$

• The vertical and horizontal angles of the perspective camera.

• pcam_on

• Whether to enable the perspective camera (0/1).

4. Operation Window

ScrollView

Left-click and drag on the background to move your view of the world.

Scale view

Scroll the mouse wheel to zoom in or out at the mouse cursor position.

Save the World

你可以保存当前世界中所有事物的位置，使用“文件/保存”菜单项。警告：保存的位置将覆盖当前的世界文件。在保存之前，先复制你的世界文件，以保留旧的位置。另外，可以使用“文件/另存为”菜单项将其保存到一个新的世界文件中。

Suspend and resume the clock

按下“p”键可以暂停或恢复模拟。按下“.”（句号）键可以运行一个模拟步骤。按住“.”键可以重复多次步进。步进会让模拟暂停，所以按下“p”键可以恢复运行。可以在世界文件中使用“paused”属性来设置初始的暂停/运行状态。

Select model

Models can be selected by clicking the left mouse button. Multiple models can also be selected by holding down the Shift key and clicking on them. Selected models can be moved by dragging, or rotated by holding down the right mouse button and moving the mouse. Clicking on an empty location in the world clears the selection. After clearing the selection, the last individually selected model will be saved as the target for several view options that affect specific models.

View Options

The View menu provides many features that affect how the world is rendered. To the right of each option, there is usually a keyboard shortcut that quickly toggles the relevant option.

The "Data" option (shortcut 'd') toggles sensor data visualization. The Filter Data option (shortcut Shift+'d') opens a dialog that allows enabling or disabling the visualization of specific sensors. The "Visualize All" option in the dialog toggles whether sensor visualization is enabled for all models or only for the currently selected model.

The “Follow” option keeps the view always on the most recently selected model.

The “Perspective camera” option can switch from orthographic view to perspective view.

Save screenshot

To save a series of screenshots of the world, select the "Save screenshots" option from the View menu to start recording images, then select that option again from the menu to stop.

4.stage_ros2 Basic Model

The base model simulates objects with basic properties: position, size, velocity, color, visibility to various sensors, etc. The base model also has a body composed of a linear list. Internally, the base model is used as the base class for all other model types. We can use the base model to simulate environmental objects. In stage, the chassis model, radar model, camera model, etc. can all be regarded as subclasses of the base model.

1. Add basic model

Add the following content to the stage_sim.launch.py file:

# -----------------------------------------------------------------------------

# 添加障碍物
model( pose [ -2 -4 0 0 ] color "green")

# define a block
define my_block model
(
  size [1.0 1.0 1.0]
  gui_nose 0
  gui_grid 0
  gui_outline 0
)

# throw in a block
my_block( pose [ 0 4 0 90 ] color "red" bitmap "maps/ghost.png")

In the above code, a green model object is created directly using the default base model, and a my_block model that inherits from the model is also customized, and then an object of this model is created.

2. Compile and execute

After building the function package and executing the launch file, the run result is as follows.

3. Form parameters

Abstract and default values

model (
    pose [ 0.0 0.0 0.0 0.0 ]
    size [ 0.1 0.1 0.1 ]
    origin [ 0.0 0.0 0.0 0.0 ]
    velocity [ 0.0 0.0 0.0 0.0 ]
    color "red"
    color_rgba [ 0.0 0.0 0.0 1.0 ]
    bitmap ""
    ctrl ""

    # determine how the model appears in various sensors
    fiducial_return 0
    fiducial_key 0
    obstacle_return 1
    ranger_return 1
    blob_return 1
    laser_return LaserVisible
    gripper_return 0
    gravity_return 0
    sticky_return 0

    # GUI properties
    gui_nose 0
    gui_grid 0
    gui_outline 1
    gui_move 0 (1 if the model has no parents);
    boundary 0
    mass 10.0
    map_resolution 0.1
    say ""
    alwayson 0
)

Detailed explanation

• pose $$x:<float> y:<float> z:<float> heading:<float>$$ Specifies the pose of the model in its parent coordinate system.

• size $$x:<float> y:<float> z:<float>$$ Specifies the size of the model in each dimension.

• origin $$x:<float> y:<float> z:<float> heading:<float>$$ Specifies the position of the object's center, relative to its pose.

• velocity $$x:<float> y:<float> z:<float> heading:<float> omega:<float>$$ Specifies the initial velocity of the model. Note that if the model hits an obstacle, its velocity will be set to zero.

• velocity_enable int (default 0) Most models ignore their velocity state. This saves processing time because the velocity of most models will never be set to a non-zero value. Some subclasses (e.g., ModelPosition) change this default because they expect to move. The user can specify a non-zero value here to enable velocity control for this model. This has the same effect as calling Model::VelocityEnable().

• color Specifies the color of the object using a color name from the X11 database (rgb.txt).

• bitmap filename: Draws the model by interpreting lines in a bitmap (supports bmp, jpeg, gif, png). The file is opened and parsed into a set of lines. These lines are scaled to fit within the rectangle defined by the model's current size.

• ctrl Specifies the model's controller module and its parameter string. For example, the string "foo bar bash" will load libfoo.so, whose Init() function will be called with the entire string as the argument (including the library name). The controller needs to parse the string (if parameters are needed).

• fiducial_return fiducial_id If non-zero, this model is detected by fiducialfinder sensors. The value is used as the fiducial identifier.

• fiducial_key Only when the fiducial_key values of the model and the fiducialfinder match will the fiducial_id model be detected by the fiducialfinder. This allows you to have several independent types of fiducial identification in the same environment, each type only visible to fiducialfinders "tuned" for it.

• obstacle_return If 1, this model can collide with other models that have this property set.

• ranger_return If 1, this model can be detected by ranger sensors.

• blob_return If 1, this model can be detected in blob_finder (depending on its color).

• laser_return If 0, this model will not be detected by laser sensors. If 1, this model appears as normal (0) reflection in laser sensors. If 2, it appears as high (1) reflection.

• gripper_return If 1, this model can be grasped by grippers and can be pushed by collisions with anything that has a non-zero obstacle_return.

• gui_nose If 1, a nose indicating its orientation (positive X axis) is drawn on the model.

• gui_grid If 1, a scale grid is drawn on the model.

• gui_outline If 1, a bounding box is drawn around the model, indicating its size.

• gui_move If 1, the model can be moved with the mouse in the GUI window.

5.stage_ros2添加地图

In this section, we will add a global map to the simulation environment. Please first prepare an image to serve as the global map.

1. Add a map

Add the following content to the stage_sim.launch.py file:

# -----------------------------------------------------------------------------

# 设置地图(定义模型)
define floorplan model
(
  color "gray30"  # 颜色
  boundary 1  # 为地图设置边框

  gui_nose 0
  gui_grid 0 

  gui_outline 0
  gripper_return 0
  fiducial_return 0
  laser_return 1
)

# 加载地图
floorplan
( 
  name "room"
  size [16.000 16.000 0.800]
  pose [0 0 0 0]
  bitmap "maps/room.jpg"
  gui_move 0
)

The above code customizes a floorplan model and, based on this model, creates a floorplan object loaded with map data.

2. Compile and execute

After building the function package and executing the launch file, the run result is as follows.

6.stage_ros2添加机器人

This section will add a virtual robot composed of a chassis, a camera, and a lidar in the simulation environment.

1. Code Framework Setup

Create a new robot directory under the world directory, and within the robot directory create car_base.inc, camera.inc, laser.inc, mycar.inc, and other files. The roles of each file are as follows:

• car_base.inc: Used to configure the robot chassis module;

• camera.inc: Used to configure the robot camera module;

• laser.inc: Used to configure the robot LiDAR module;

• mycar.inc: Used to assemble the robot's modules.

In addition, it also includes sim.world and calls its function of generating robots in mycar.inc.

2. Set up the robot chassis

The position model in stage can simulate a mobile robot chassis in differential, omnidirectional, or Ackermann modes. Enter the following code in car_base.inc:

# 机器人底盘配置
define car_base position 
(
  color "red"                   # 车身颜色
  drive "diff"                  # 车辆运动学模型                  
  obstacle_return 1             
  ranger_return 1           
  blob_return 1                  
  fiducial_return 1             

  localization "odom"           # 定位方式
  odom_error [ 0.05 0.05 0.0 0.1 ]  # 里程计误差

  # localization_origin [0 0 0 0]   # 定位原点，默认为机器人的初始位置。

  # [ xmin xmax ymin ymax zmin zmax amin amax ]        
  velocity_bounds [-1 1 0 0 0 0 -45.0 45.0 ]         # 速度最值 
  acceleration_bounds [-0.5 0.5 0 0 0 0 -45 45.0 ]   # 加速度最值

  size [0.44 0.38 0.22]  # 车体尺寸
  origin [0 0 0 0] # 旋转中心与车体中心的偏移量
  mass 23.0 # 车体质量，单位kg

  gui_nose 0 # 是否绘制方向指示标记

  block( 
    points 8
    point[0] [-0.2 0.18]
    point[1] [-0.2 -0.18]
    point[2] [-0.15 -0.27]
    point[3] [0.12 -0.23]
    point[4] [0.2 -0.12]
    point[5] [0.2 0.12]
    point[6] [0.12 0.23]
    point[7] [-0.15 0.27]
    z [0 0.22]
  )
)

position summary and default values

position(
    drive "diff"
    localization "gps"
    localization_origin [<defaults to model's start pose>]
    odom_error [0.03 0.03 0.00 0.05]
    velocity_enable 1
)

Detailed explanation

• drive "diff", "omni" or "car" selects a differential steering model, omnidirectional mode, or a car-like Ackermann mode.

• localization "gps" or "odom" If "gps" is selected, the position model will report position with completely accurate precision. If "odom" is selected, a simple odometry model will be used, and the position data will drift over time from the true position. The odometry model is parameterized by the odom_error property.

• localization_origin $$x y z theta$$ You can use the localization_origin parameter to set the origin of the localization coordinate system. By default, it will copy the model's initial pose, so the robot will report its position relative to the starting position. Tip: If localization_origin is set to $$0 0 0 0$$ and the localization method is "gps", the model will return its true global position. This setup is unrealistic, but it is useful in research where you want to abstract away localization details.

• odom_error $$x y z theta$$The odometry error model parameters used when the "odom" localization method is selected. Each value is the maximum error ratio for integrating the x, y, and theta velocities when computing the odometry position estimate. For each axis, if the value specified here is E, the actual ratio is randomly selected at startup within the range from -E/2 to +E/2. Note that due to rounding errors, setting these values to zero will not make localization perfectly accurate - to achieve that, you need to select the "gps" localization method.

3. Set up the camera

The camera model in stage can simulate a depth camera. Enter the following code in camera.inc:

define my_camera camera
(
    range [ 0.3 3.0 ] # 相机采样范围
    resolution [ 160 90 ]  #相机分辨率 1280 × 720 / 8
    fov [ 87 58 ] # 相机视场
    pantilt [ 0 0 ] # 相机姿态
    alwayson 1 # 是否一直处于启动状态
    size [ 0.025 0.09 0.025 ] # 相机尺寸
    color "gray" # 相机颜色
)

Camera summary and default values

camera(
  resolution [ 32 32 ]
  range [ 0.2 8.0 ]
  fov [ 70.0 40.0 ]
  pantilt [ 0.0 0.0 ]
  size [ 0.1 0.07 0.05 ]
  color "black"
  watts 100.0 
)

Detailed explanation

• resolution $$width:<int> height:<int>$$ Camera resolution.

• range $$min:<float> max:<float>$$ Range in meters that the camera reports. Objects closer than min or farther than max will not be displayed.minThe smaller the number, the lower the depth precision - do not set this value too close to 0.

• fov $$horizontal: <float> vertical: <float>$$ Horizontal and vertical field of view angles, in degrees.

• pantilt $$pan:<float> tilt:<float>$$ Camera orientation angles, in degrees. Left-right position is called pan, up-down position is called tilt.

4. Set up LiDAR

The ranger model in the stage can simulate LiDAR. Enter the following code in laser.inc:

define my_laser ranger
(
  sensor(

    range [ 0.0  15.0 ]     # 雷达数据采集区间
    fov 360.0               # 视角
    samples 720             # 采样数
    color_rgba [ 0 0 1 0.15 ] # 可视化光束颜色以及透明度
  )
  model # 雷达外观
  (
    pose [ 0 0 0 0 ]        # 雷达位姿
    size [ 0.07 0.07 0.05 ] # 雷达尺寸信息
    color "blue"            # 雷达颜色
  )
)

ranger summary and defaults

 sensor(
   samples 180
   range_max 8.0
   fov 360.0
   resolution 1
   size [ 0.15 0.15 0.2 ]
   color "blue"
 )

Detailed explanation

• samples Number of laser samples per scan.

• range_max Maximum distance reported by the laser scanner, in meters. The scanner will not detect objects beyond this range.

• fov Angular field of view of the laser scanner.

• resolution Only compute the true distance of every nth laser sample. Missing samples will be filled in by linear interpolation. In general, using fewer samples is better, but some (poorly implemented) programs require a fixed number of samples. Setting this number greater than 1 can reduce the amount of computation required, suitable for fixed-size laser vectors.

5. Assembling the robot

Implement robot assembly in mycar.inc:

# 组装机器人各个模块
include "robot/car_base.inc"
include "robot/camera.inc"
include "robot/laser.inc"

define my_car car_base(
    my_camera(pose [0.15 0 0 0])
    my_laser()
)

my_car model is built on top of the car_base model, integrating the my_camera and my_laser models.

6. Generate a robot in the simulation environment

Add the following code to sim.world to generate a bot:

# -----------------------------------------------------------------------------

# 生成机器人

# 文件包含
include "robot/mycar.inc"

my_car(
  name "robot_0"
  color "red"
  pose [ -3 -7 0 90 ] 
)

7. Build execution

After building the function package and executing the launch file, the run result is as follows.

Start rviz2 to view sensor-related data in the simulation environment. After starting the keyboard control node, you can also control the robot's movement.

7.stage_ros2 Adding multiple robots

In stage_ros2, multiple robots can also be generated very conveniently.

1. Generate multiple robots

In sim.world, you can continue to create a my_car object to generate a new robot model. Add the following code:

my_car(
  name "robot_1"
  color "yellow"
  pose [ -1 -7 0 90 ] 
)

2. Build execution

After building the function package and executing the launch file, the run result is as follows.

When multiple robots are running, topics published by different robots will use their corresponding name values as namespaces, and coordinate transforms published by different robots will also use the name value as a prefix.