Stage_Ros2仿真平台

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

stage使用手冊：

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

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

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中可以顯示里程計、激光雷達以及深度相機等相關信息，運行結果如下。

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 內核數量和世界文件的情況。如果您有啓用並行的高分辨率模型（例如帶有數百或數千個樣本的激光器或大量模型），則每個內核使用一個線程。

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:intheight:int ]
• 窗口的大小（以像素為單位）。
• center [<x:float><y:float> ]
• 窗口中心的位置，使用世界座標表示（以米為單位）。
• rotate [pitch:floatyaw:float ]
• 相對於垂直向上的角度，旋轉角度（以度為單位）。
• scale
• 世界座標到像素座標的比例（窗口縮放）。
• pcam_loc [<x:int><y:int><z:int> ]
• 透視攝像機的位置（以米為單位）。
• pcam_angle [pitch:floatyaw:float ]
• 透視攝像機的垂直和水平角度。
• pcam_on
• 是否啓用透視攝像機（0/1）。

4.操作窗體

滾動視圖

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

縮放視圖

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

保存世界

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

暫停和恢復時鐘

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

選擇模型

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

視圖選項

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

“Data”選項（快捷鍵'd'）可切換傳感器數據可視化。濾波器數據選項（快捷鍵Shift+'d'）打開對話框，可啓用或禁用特定傳感器的可視化。對話框中的“Visualize All”選項可切換是否為所有模型啓用傳感器可視化，還是僅為當前選擇的模型啓用。

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

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

保存截圖

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

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窗口中通過鼠標移動。

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文件後，運行結果如下。

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可以查看仿真環境下傳感器相關數據，啓動鍵盤控制節點後，也可以控制機器人運動。

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值為前綴。