第 10 節

Stage_Ros2 Simulation Platform

0瀏覽次數0訪問次數--跳出率--平均停留

stage_ros2 is a ROS 2 package based on the Stage simulator, used to simulate and control robot behavior in a virtual environment. It provides powerful tools and communication interfaces for developing, testing, and validating robotic systems. With stage_ros2, users can easily create virtual environments, send commands via ROS 2 to control robot motion, and receive sensor data. This package helps users conduct robot development at lower cost and risk, offering an efficient and flexible robot simulation environment.

Stage User Manual:

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

(Optional — ROS2 has already removed Stage, and Gazebo is a much better alternative, but learning it won't hurt.)

Installation and Running of stage_ros2

Installation

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

Please 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 src directory of the ROS2 workspace and run the following command to download the relevant repositories:

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

Enter the workspace directory and use the colcon build command to build.

(This is a third-party author's adaptation for the Humble version, which currently does not support the Jazzy version. Compiling the Jazzy version will result in errors; the author may update it for Jazzy in the future.)

Node Description

stage_ros2 node is one of the core nodes of the stage_ros2 package. It creates a simulation scene by loading a world file, which includes elements such as maps, obstacles, and robots. The world file describes the properties of the virtual environment, and the node builds the corresponding environment based on the descriptions in the file. The stage_ros2 node uses this virtual environment to simulate robot motion, perception, and control. By combining this node with the world file, robot simulation and testing can be carried out.

Usage

Under the function 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 depth camera can be displayed. The running results are as follows.

stage_ros2 simulation framework setup

Starting from this section, we will introduce how to customize a simulation environment using stage_ros2.

Preparation

First, create a function package with the following instructions:

ros2 pkg create demo_stage_sim --dependencies stage_ros2

Then create the launch, world, and config directories under the package, and modify the CMakeLists.txt file under the package by adding 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 rviz2 configuration files, and world is used to store simulation-related files. In the world directory, please create a new maps directory and copy the image assets from the course supporting materials into it.

Build the Code Framework

In the launch directory of the function package, create a new file named stage_sim.launch.py 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 file named sim.world and enter the following content:


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

# 设置窗体

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

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

The sim.world file currently only declares some general parameters for the simulation environment.

Compilation

In the terminal, navigate to the current workspace and compile the package:

colcon build --packages-select demo_stage_sim

Execute

In the current workspace, open 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 appear with no content inside. Next, we can proceed to create the specific simulation content.

world file parameters

Summary 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
A name used for identification in the world, such as in the title bar of a 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 can be greater or smaller than this value, but intervals shorter than this value are not visible in the graphical user interface or World update callbacks. You likely 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 perception accuracy. In general, the default value is a reasonable choice.
show_clock
If non-zero, print the simulation time to standard output every $show_clock_interval updates. This is useful for observing the progress of non-GUI simulations.
show_clock_interval
When $show_clock is enabled, prints the number of updates between time prints on standard output. The default is to print once every 10 seconds of simulated time. Smaller values will slightly reduce simulation speed.
threads
Number of worker threads to generate. Some models can update in parallel (e.g., lasers, rangefinders). Running 2 or more threads here may speed up simulation, 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.

stage_ros2 setup window

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 to see more of the simulated world. Simulated robot devices, obstacles, and other elements are rendered as colored polygons. The window also supports visualization of data and configuration for various sensor and actuator models. The menu provides options for controlling which data and configurations are displayed. In this section, we will introduce how to set up and operate the stage window.

Set up the form

Set the window-related parameters by adding 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 700x700, using a zoom ratio of 35x. The subsequently loaded map has no offset or rotation relative to the window and will display sensor data in a visual format.

Compile and Execute

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

Form Parameters

Summary 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 real-time multiplier. 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 ]
Rotation angle in degrees, relative to the vertical upward direction.
scale
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 ]
Perspective camera's vertical and horizontal angles.
pcam_on
Whether to enable the perspective camera (0/1).

Operation Window

Scroll View

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

Zoom View

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

Save the World

You can save the positions of all objects in the current world using the "File/Save" menu item. Warning: Saving will overwrite the current world file. Before saving, copy your world file to preserve the old positions. Alternatively, you can use the "File/Save As" menu item to save it to a new world file.

Pausing and Resuming the Clock

Pressing the "p" key can pause or resume the simulation. Pressing the "." (period) key runs one simulation step. Holding down the "." key repeats stepping multiple times. Stepping pauses the simulation, so pressing the "p" key resumes it. The initial pause/run state can be set using the "paused" attribute in the world file.

Select Model

You can select a model by left-clicking on it. You can also select multiple models by holding down the Shift key and clicking on several models. Selected models can be moved by dragging, or rotated by holding down the right mouse button and moving the mouse. Clicking on an empty spot in the world clears the selection. After clearing the selection, the last single model that was selected is 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 for quickly toggling the related setting.

The "Data" option (shortcut 'd') toggles sensor data visualization. The Filter Data option (shortcut Shift+'d') opens a dialog that allows enabling or disabling visualization for 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 locked to the last selected model.

The "Perspective camera" option switches from an orthographic view to a perspective view.

Save screenshot

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

stage_ros2 basic model

The basic model simulates objects with fundamental attributes: position, size, velocity, color, and visibility to various sensors. The basic model also has a body composed of a linear list. Internally, the basic model is used as the base class for all other model types. We can use the basic model to simulate environmental objects. In Stage, chassis models, radar models, camera models, and so on can all be considered subclasses of the basic model.

Add basic model

In the stage_sim.launch.py file, add the following content:


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

# 添加障碍物
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 code above, a green model object was created directly using the default base model, and a custom my_block model inheriting from model was also defined, followed by creating an instance of that model.

Compile and Execute

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

Form Parameters

Summary 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: y: z: heading: ] Specifies the model's pose in its parent coordinate system.
size [ x: y: z: ] specifies the model's dimensions along each axis.
origin x: y: z: heading: Specifies the position of the object's center relative to its orientation.
velocity x: y: z: heading: omega: specifies the initial velocity of the model. Note that if the model collides with an obstacle, its velocity will be set to zero.
velocity_enable int (default 0) Most models ignore their velocity state. This saves processing time, since most models never have their velocity set to a non-zero value. Some subclasses (such as ModelPosition) change this default because they expect movement. Users 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 object's color using a color name from the X11 database (rgb.txt).
bitmap filename: Draws a model by interpreting lines in the 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, and its Init() function will be called with the entire string as an argument (including the library name). The controller needs to parse the string (if parameters are required).
fiducial_return fiducial_id If non-zero, this model is detected by the fiducial finder sensor. This value is used as the fiducial identifier.
fiducial_key The fiducial_id model will only be detected by a fiducialfinder when the model's fiducial_key value matches that of the fiducialfinder. This allows you to have several independent types of fiducial markers in the same environment, with each type only appearing in the fiducialfinders that are "tuned" for it.
obstacle_return If set to 1, this model can collide with other models that have this attribute enabled.
ranger_return If set to 1, this model can be detected by the ranger sensor.
blob_return If set to 1, the model can be detected in blob_finder (depending on its color).
laser_return If set to 0, this model will not be detected by the laser sensor. If set to 1, the model appears as normal (0) reflection in the laser sensor. If set to 2, it appears as high (1) reflection.
gripper_return If set to 1, the model can be grasped by the gripper and can be pushed through collision with anything that has a non-zero obstacle_return.
gui_nose If set to 1, draws a nose on the model to indicate its orientation (positive X-axis).
gui_grid If set to 1, draws a scale grid on the model.
gui_outline If set to 1, draws a bounding box around the model to indicate its size.
gui_move If set to 1, the model can be moved with the mouse in the GUI window.

stage_ros2 adding a map

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

Add Map

In the stage_sim.launch.py file, add the following content:


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

# 设置地图(定义模型)
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 code above defines a custom floorplan model and creates a floorplan object loaded with map data based on this model.

Compile and Execute

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

stage_ros2 add robot

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

Code Framework Setup

In the world directory, create a new robot directory. Inside the robot directory, create the files car_base.inc, camera.inc, laser.inc, and mycar.inc. The purpose of each file is as follows:

car_base.inc: Used to configure the robot chassis module.
camera.inc: used to configure the robot's camera module;
laser.inc: used to configure the robot's LiDAR module;
mycar.inc: Used to assemble the various modules of the robot.

Additionally, it will include mycar.inc in sim.world and call its robot generation functionality.

Set up the robot chassis

The position model in stage can simulate a mobile robot chassis using differential, omnidirectional, or Ackermann steering. 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

The "diff", "omni", or "car" drive options select the differential steering model, omnidirectional mode, or car-like Ackermann mode.
localization "gps" or "odom". If "gps" is selected, the position model will report location with perfect accuracy. If "odom" is selected, a simple odometry model will be used, and the position data will drift from the true location over time. The odometry model is parameterized by the odom_error attribute.
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 copies the model's initial pose, so the robot will report its position relative to the starting location. Tip: If you set localization_origin to 0 0 0 0 and the localization method to "gps", the model will return its true global position. This setting is unrealistic but useful in research where you want to abstract away localization details.
odom_error [x y z theta] These are the odometry error model parameters used when selecting the "odom" positioning mode. Each value represents the maximum error ratio when integrating x, y, and theta velocities for odometry position estimation. For each axis, if the value specified here is E, the actual ratio is randomly chosen at startup within the range of -E/2 to +E/2. Note that due to rounding errors, setting these values to zero does not make localization perfect — to achieve that, you need to select the "gps" positioning mode.

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: height:] Camera resolution.
range [ min: max: ] The distance range reported by the camera, in meters. Objects closer than min or farther than max will not be displayed. min The smaller the number, the lower the depth accuracy - do not set this value too close to 0.
fov [ horizontal: vertical: ] The horizontal and vertical field of view angles, in degrees.
pantilt [ pan: tilt: ] The camera's orientation angle, in degrees. The left-right position is called pan, and the up-down position is called tilt.

Set up the LiDAR

The ranger model in stage can simulate a 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 Default Values

 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 The 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 calculates the true distance for the nth laser sample. Missing samples will be filled using 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 required computation, which is suitable for fixed-size laser vectors.

Assemble 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()
)

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

Spawning a robot in the simulation environment

Add the following code in sim.world to generate a robot:


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

# 生成机器人

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

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

Build Execution

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

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

stage_ros2 adding multiple robots

In stage_ros2, it is also very convenient to generate multiple robots.

Generate multiple robots

In sim.world, you can continue creating my_car objects to generate new robot models. Add the following code:

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

Build Execution

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

When multiple robots are running simultaneously, the topics published by different robots will use their respective name values as namespaces, and the coordinate transforms published by different robots will also be prefixed with the name value.