Wiki

Stage_Ros2模擬平臺

更新於 2023-12-30約 17 分鐘閱讀

本文由簡體中文內容確定性轉換，並受版本化術語表保護。

stage_ros2是基於Stage模擬器的ROS2功能包，用於在虛擬環境中模擬和控制機器人的行為。它提供了強大的工具和通訊介面，可用於開發、測試和驗證機器人系統。通過stage_ros2，使用者可以輕鬆建立虛擬環境，並使用ROS 2傳送指令控制機器人的運動，同時接收感測器資料。這個功能包可以幫助使用者以更低的成本和風險進行機器人開發，提供了高效且靈活的機器人模擬環境。

stage使用手冊：

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

（ 可以不學 ，連ROS2官方都已經移除掉了Stage， 用更牛X的Gazebo更好 ，但學一學也問題不大）

1.stage_ros2安裝與執行

1.安裝

因為ROS2官方已經移除掉了Stage，所以我們要通過原始碼編譯手動安裝，已經有大佬適配了ROS2版本的Stage：

請先呼叫如下指令安裝依賴：

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

進入ROS2工作空間的src目錄，呼叫如下指令下載相關倉庫：

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

進入工作空間目錄，使用colcon build指令進行構建。

（這是第三方作者適配Humble版本的，暫時不支援Jazzy版本，Jazzy版本目前編譯會報錯，可能作者會更新Jazzy版本的）

2.節點說明

stage_ros2節點是stage_ros2功能包的核心節點之一。它通過載入world檔案來建立一個模擬場景，包括地圖、障礙物和機器人等元素。world檔案描述了虛擬環境的屬性，節點會根據檔案中的描述構建對應的環境。stage_ros2節點利用這個虛擬環境來模擬機器人的運動、感知和控制。通過該節點和world檔案的結合，可以進行機器人的模擬和測試。

3.使用

在功能包下，已經內建了使用示例，終端下可以執行如下指令啟動示例：

ros2 launch stage_ros2 my_house.launch.py

模擬器以及rviz2將被啟動，並且在rviz2中可以顯示里程計、雷射雷達以及深度相機等相關資訊，執行結果如下。

2.stage_ros2模擬框架搭建

本節開始，我們將介紹如何通過stage_ros2自定義模擬環境。

1.準備工作

首先請建立一個功能包，指令如下：

ros2 pkg create demo_stage_sim --dependencies stage_ros2

然後在功能包下新建launch、world、config目錄，並修改功能包下的CMakeLists.txt檔案，新增如下程式碼：

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

其中，launch目錄用於儲存launch檔案，config用於儲存rviz2的配置檔案，world用於儲存模擬相關檔案。world目錄中請新建maps目錄，並將課程配套資料中的圖片素材複製進去。

2.搭建程式碼框架

在功能包的launch目錄下，新建stage_sim.launch.py檔案，並輸入如下內容：

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')}],
        )
    ])

該launch檔案將執行stage_ros2節點，並載入world目錄下的sim.world檔案。

在world目錄下，新建sim.world檔案，並輸入如下內容：


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

# 设置窗体

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

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

sim.world檔案當前只是聲明瞭模擬環境的一些通用引數。

3.編譯

終端中進入當前工作空間，編譯功能包：

colcon build --packages-select demo_stage_sim

4.執行

當前工作空間下，啟動終端，並輸入如下指令：

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

執行該launch檔案後，將生成一個視窗，該視窗中並無任何內容，下一步我們就可以著手建立具體的模擬內容了。

5.world檔案引數

摘要和預設值

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

詳解

name
用於世界的識別名稱，例如在圖形使用者介面的標題欄中使用。
interval_sim
每次呼叫 World::Update() 時執行的模擬時間量。每個模型都有其自己可配置的更新間隔，可以大於或小於此值，但比此值更短的間隔在圖形使用者介面或 World 更新回撥中是不可見的。您可能不需要改變預設值100毫秒：此值在客戶端內部使用，如Player和WebSim。
quit_time 在模擬時間達到指定的秒數後停止模擬。在 libstage 中，World::Update() 返回 true。在帶有圖形使用者介面的 Stage 中，模擬被暫停。在沒有圖形使用者介面的 Stage 中，Stage 會退出。
resolution 底層點陣圖模型的解析度（以米為單位）。較大的值可以加快射線追蹤速度，但會以碰撞檢測和感知精度為代價。通常情況下，預設值是一個合理的選擇。
show_clock
如果非零，每經過 $show_clock_interval 次更新就在標準輸出上列印模擬時間。這對於觀察非 GUI 模擬的進展非常有用。
show_clock_interval
設定在啟用 $show_clock 的情況下，在標準輸出上列印時間之間的更新次數。預設值是每經過 10 秒模擬時間列印一次。較小的值會稍微降低模擬速度。
threads
要生成的工作執行緒數。一些模型可以並行更新（例如雷射器、測距儀），在這裡執行 2 個或更多執行緒可能會使模擬執行更快，取決於可用的 CPU 核心數量和世界檔案的情況。如果您有啟用並行的高解析度模型（例如帶有數百或數千個樣本的雷射器或大量模型），則每個核心使用一個執行緒。

3.stage_ros2設定窗體

stage 窗體由選單欄和模擬世界的檢視組成。我們可以放大和縮小檢視，並滾動檢視以檢視模擬世界的更多內容。模擬的機器人裝置、障礙物等被渲染為彩色多邊形。窗體中還可以實現各種感測器和執行器模型的資料和配置的視覺化。選單則具有用於控制呈現哪些資料和配置的選項。本節我們將介紹如何設定以及操作stage窗體。

1.設定窗體

設定窗體相關引數，可以在stage_sim.launch.py檔案中新增如下內容：


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

上述程式碼建立了一個窗體物件，該窗體解析度為700*700，使用了35倍縮放比，後續載入的地圖相對於窗體無偏移無旋轉且會以視覺化的方式顯示感測器資料。

2.編譯執行

構建功能包並執行launch檔案後，執行結果如下。

3.窗體引數

摘要和預設值

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
)

詳解

speedup
Stage 將嘗試以即時的這個倍數執行。如果設定為 -1，Stage 將盡可能以最快速度執行，並且不會嘗試跟蹤即時時間。
size [width:int height:int ]
視窗的大小（以畫素為單位）。
center [<x:float><y:float> ]
視窗中心的位置，使用世界座標表示（以米為單位）。
rotate [pitch:float yaw:float ]
相對於垂直向上的角度，旋轉角度（以度為單位）。
scale
世界座標到畫素座標的比例（視窗縮放）。
pcam_loc [<x:int><y:int><z:int> ]
透視攝像機的位置（以米為單位）。
pcam_angle [pitch:float yaw:float ]
透視攝像機的垂直和水平角度。
pcam_on
是否啟用透視攝像機（0/1）。

4.操作窗體

滾動檢視

在背景上左鍵單擊並拖動以移動你對世界的檢視。

縮放檢視

滾動滑鼠滾輪可放大或縮小滑鼠游標處的位置。

儲存世界

你可以儲存當前世界中所有事物的位置，使用“檔案/儲存”選單項。警告：儲存的位置將覆蓋當前的世界檔案。在儲存之前，先複製你的世界檔案，以保留舊的位置。另外，可以使用“檔案/另存為”選單項將其儲存到一個新的世界檔案中。

暫停和恢復時鐘

按下“p”鍵可以暫停或恢復模擬。按下“.”（句號）鍵可以執行一個模擬步驟。按住“.”鍵可以重複多次步進。步進會讓模擬暫停，所以按下“p”鍵可以恢復執行。可以在世界檔案中使用“paused”屬性來設定初始的暫停/執行狀態。

選擇模型

可以通過點選左鍵來選擇模型。通過按住Shift鍵並點選多個模型，也可以選擇多個模型。選擇的模型可以通過拖動來移動，或者通過按住右鍵並移動滑鼠來旋轉。點選世界中的空白位置可以清除選擇。在清除選擇後，最後選中的單個模型將被儲存為影響特定模型的幾個檢視選項的目標。

檢視選項

檢視選單提供了許多影響世界渲染方式的功能。在每個選項的右側，通常有一個按鍵快捷鍵，可快速切換相關選項。

“Data”選項（快捷鍵'd'）可切換感測器資料視覺化。濾波器資料選項（快捷鍵Shift+'d'）開啟對話方塊，可啟用或停用特定感測器的視覺化。對話方塊中的“Visualize All”選項可切換是否為所有模型啟用感測器視覺化，還是僅為當前選擇的模型啟用。

“Follow”選項可保持檢視始終在最後選擇的模型上。

“Perspective camera”選項可以從正交檢視切換到透視檢視。

儲存截圖

要儲存世界的一系列截圖，從檢視選單中選擇“Save screenshots”選項以開始錄製影像，然後再次從選單中選擇該選項以停止。

4.stage_ros2基本模型

基本模型（model）模擬具有基本屬性的物件；位置、大小、速度、顏色、對各種感測器的可見性等。基本模型還具有由線性列表組成的主體。在內部，基本模型被用作所有其他模型型別的基類。我們可以使用基本模型來模擬環境物件。在stage中，底盤模型、雷達模型、相機模型等都可以看作是基本模型的子類。

1.新增基本模型

在stage_sim.launch.py檔案中新增如下內容：


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

# 添加障碍物
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")

上述程式碼中，直接使用預設的基礎model建立了一個綠色模型物件，並且還自定義了繼承自model的my_block模型，然後建立了該模型物件。

2.編譯執行

構建功能包並執行launch檔案後，執行結果如下。

3.窗體引數

摘要和預設值

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
)

詳解

pose [ x: y: z: heading: ] 指定模型在其父座標系中的姿態。
size [ x: y: z: ] 指定模型在各個維度上的大小。
origin [ x: y: z: heading: ] 指定物件中心的位置，相對於其姿態。
velocity [ x: y: z: heading: omega: ] 指定模型的初始速度。請注意，如果模型撞到障礙物，其速度將被設定為零。
velocity_enable int (預設為0) 大多數模型忽略其速度狀態。這樣可以節省處理時間，因為大多數模型的速度永遠不會被設定為非零值。一些子類（例如ModelPosition）會改變這個預設值，因為它們期望移動。使用者可以在這裡指定一個非零值來啟用對該模型的速度控制。這與呼叫Model::VelocityEnable()的效果相同。
color 使用X11資料庫（rgb.txt）中的顏色名稱指定物件的顏色。
bitmap filename: 通過解釋點陣圖中的線條來繪製模型（支援bmp、jpeg、gif、png）。檔案被開啟並解析為一組線條。這些線條被縮放以適應模型當前大小所定義的矩形內。
ctrl 指定模型的控制器模組及其引數字串。例如，字串"foo bar bash"將載入libfoo.so，其Init()函式將以整個字串作為引數進行呼叫（包括庫名稱）。控制器需要解析字串（如果需要引數）。
fiducial_return fiducial_id 如果非零，則通過fiducialfinder感測器檢測到此模型。該值用作基準識別符號。
fiducial_key 僅當模型和fiducialfinder的fiducial_key值匹配時，fiducial_id模型才會被fiducialfinder檢測到。這允許您在同一環境中擁有幾種獨立型別的基準標識，每種型別只顯示在為其"調諧"的fiducialfinders中。
obstacle_return 如果為1，則此模型可以與具有此屬性設定的其他模型發生碰撞。
ranger_return 如果為1，則該模型可以被ranger感測器檢測到。
blob_return 如果為1，則該模型可以在blob_finder中被檢測到（取決於其顏色）。
laser_return 如果為0，則此模型不會被雷射感測器檢測到。如果為1，則該模型在雷射感測器中顯示為正常（0）反射。如果為2，則顯示為高（1）反射。
gripper_return 如果為1，則該模型可以被夾爪夾取，並且可以通過與具有非零obstacle_return的任何東西的碰撞來推動。
gui_nose 如果為1，則在模型上繪製顯示其朝向的鼻子（正X軸）。
gui_grid 如果為1，則在模型上繪製比例網格。
gui_outline 如果為1，則在模型周圍繪製邊界框，指示其大小。
gui_move 如果為1，則模型可以在GUI視窗中通過滑鼠移動。

5.stage_ros2新增地圖

本節我們將在模擬環境中，新增一張全域性地圖，請先準備一張作為全域性地圖的圖片。

1.新增地圖

在stage_sim.launch.py檔案中新增如下內容：


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

# 设置地图(定义模型)
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
)

上述程式碼自定義了floorplan模型，並根據此模型建立了一個載入了地圖資料的floorplan物件。

2.編譯執行

構建功能包並執行launch檔案後，執行結果如下。

6.stage_ros2新增機器人

本節將在模擬環境中新增一個由底盤、攝像頭以及雷射雷達組成的虛擬機器人。

1.程式碼框架搭建

在world目錄下新建robot目錄，robot目錄中再建立car_base.inc、camera.inc、laser.inc以及mycar.inc等檔案，各檔案作用如下：

car_base.inc：用於設定機器人底盤模組；
camera.inc：用於設定機器人相機模組；
laser.inc：用於設定機器人雷達模組；
mycar.inc：用於組裝機器人各模組。

除此之外，還會在sim.world中包含mycar.inc並呼叫其生成機器人的功能。

2.設定機器人底盤

stage中的position模型可以以差速、全向或阿克曼的方式模擬移動機器人底盤。在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摘要和預設值

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
)

詳解

drive "diff", "omni" 或 "car" 選擇差速轉向模型、全向模式或類似汽車的阿克曼模式。
localization "gps" 或 "odom" 如果選擇 “gps”，位置模型將以完全準確的精度報告位置。如果選擇 “odom”，將使用簡單的里程計模型，並且位置資料會隨時間與真實位置的差異而漂移。里程計模型由 odom_error 屬性引數化。
localization_origin [x y z theta] 您可以使用 localization_origin 引數來設定定位座標系的原點。預設情況下，它將複製模型的初始姿態，因此機器人將報告相對於起始位置的位置。提示: 如果將 localization_origin 設定為 [0 0 0 0] 並且定位方式為 “gps”，模型將返回其真實的全域性位置。這種設定是不現實的，但在希望抽象定位細節的研究中很有用。
odom_error [x y z theta] 在選擇 “odom” 定位方式時用到的里程計誤差模型引數，每個值是計算里程計位置估計時積分 x、y 和 theta 速度的誤差比例的最大值。對於每個軸，如果在此處指定的值為 E，則實際比例在啟動時在 -E/2 到 +E/2 的範圍內隨機選擇。請注意，由於舍入誤差，將這些值設定為零並不能讓定位完美無誤 - 為了實現這一點，您需要選擇 “gps” 定位方式。

3.設定攝像頭

stage中的camera模型可以模擬深度相機。在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摘要和預設值

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 
)

詳解

resolution [ width: height:] 相機解析度。
range [ min: max: ] 相機報告的距離範圍，以米為單位。距離小於min或大於max的物體將無法顯示。min數字越小，深度精度越低 - 不要將此值設定得太接近 0。
fov [ horizontal: vertical: ] 水平和垂直視野的角度，以度為單位。
pantilt [ pan: tilt: ] 相機的朝向角度，以度為單位。左右位置稱為 pan，上下位置稱為 tilt。

4.設定雷射雷達

stage中的ranger模型可以模擬雷射雷達。在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摘要和預設值

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

詳解

samples 每次掃描的雷射樣本數量。
range_max 雷射掃描器所報告的最大距離，以米為單位。掃描器將不會檢測超出此範圍的物體。
fov 雷射掃描器的角度視野。
resolution 僅計算第 n 個雷射樣本的真實距離。缺失的樣本將用線性插值填充。一般來說，使用較少的樣本會更好，但某些（實現不好的）程式需要固定數量的樣本數。設定此數字大於 1 可以減少所需的計算量，適用於固定大小的雷射向量。

5.組裝機器人

在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模型是在car_base模型之上，集成了my_camera和my_laser模型。

6.在模擬環境中生成機器人

在sim.world中新增如下程式碼生成一個機器人：


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

# 生成机器人

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

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

7.構建執行

構建功能包並執行launch檔案後，執行結果如下。

啟動rviz2可以檢視模擬環境下感測器相關資料，啟動鍵盤控制節點後，也可以控制機器人運動。

7.stage_ros2新增多機器人

在stage_ros2中也可以很方便的生成多個機器人。

1.生成多個機器人

在sim.world中還可以繼續建立my_car物件以生成新的機器人模型，新增程式碼如下：

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

2.構建執行

構建功能包並執行launch檔案後，執行結果如下。

在多機器人執行時，不同的機器人釋出的話題會以各自對應的name值為名稱空間，且不同機器人釋出的座標變換也會以name值為字首。