模拟
本章介绍如何从traffic_editor
文件生成建筑模型,然后在这些模型中模拟机器人队伍。
动机
用于测试机器人解决方案的模拟环境在研发和部署的各个阶段都具有巨大的价值。更值得注意的是,模拟具有以下好处:
-
Time and resource saving: 虽然硬件测试不可或缺,但这个过程可能会减慢开发速度,因为需要额外的设置时间、机器人停机时间和试验之间的重置时间。随着参与者数量的增加,购买硬件和测试耗材的成本也会增加。在使用 RMF 等解决方案时尤其如此,该解决方案旨在将多个移动/固定机器人与门和电梯等建筑系统集成在一起。模拟提供了一种潜在的经济高效且省时的替代方案,用于评估机器人系统在规模上的行为。更重要的是,模拟可以帮助回答部署前的问题,例如可以支持多少参与者,或者随着新车队的引入,现有行为将如何变化,这两者都可以为设施所有者提供购买决策信息。
-
Robust testing: 模拟中的机器人既不会耗尽电池,也不会在不幸撞到什么东西时产生费用。可以连续数小时以更快的速度测试场景,以微调算法并验证其稳健性。关于运行适当场景测试数量的一个考虑因素是,这取决于您希望为模拟提供多少计算能力。随着云模拟的引入,这个限制也是成本和速度的权衡。由于模拟中的场景是可重复的,因此可以轻松验证遇到的不良错误的修复。还可以通过模拟研究系统对罕见但后果严重的边缘情况的反应。从硬件试验记录的数据可用于在模拟中重现场景,这可能对调试更有帮助。最后,长时间运行的模拟可以在部署之前让设施所有者充满信心。
基于物理的模拟器(例如“Gazebo”)具有通过“gazebo_ros_pkgs”提供的包装器轻松与 ROS 2 节点交互的优势。
可以开发 Gazebo 插件来准确模拟机器人、传感器和基础设施系统的行为,从而提高模拟的整体保真度。值得强调的是,用于运行模拟的完全相同的代码也将在物理系统上运行,而无需进行任何更改。
然而,尽管有这些令人信服的好处,开发人员和系统集成商很少使用模拟,理由是生成环境和使用适当的插件配置它们的复杂性。在最近由 Afsoon Afzal、Deborah S. Katz、Claire Le Goues 和 Christopher S. Timperley 发表的《关于使用机器人模拟器进行测试的挑战的研究》中,他们指出了参与者不为特定项目使用模拟的主要原因,并总结了他们的发现如下:
Reason for not using simulation | # | % |
---|---|---|
Lack of time or resources | 15 | 53.57% |
Not realistic/accurate enough | 15 | 53.57% |
Lack of expertise or knowledge on how to use software-based simulation | 6 | 21.43% |
There was no simulator for the robot | 4 | 14.29% |
Not applicable | 4 | 14.29% |
Too much time or compute resources | 2 | 7.14% |
Nobody suggested it | 0 | 0.00% |
Other | 2 | 7.14% |
RMF 项目旨在通过简化设置多车队交通控制模拟环境的过程来解决这些障碍,我们将在本节中进一步解释。
建筑地图生成器
上一章讨论的“traffic_editor”是一种以供应商中立的方式用特定于车队的交通信息注释建筑平面图的工具。 这包括感兴趣的航点、交通车道和共享资源,如门口和电梯。它还可用于标记墙壁和地板,并添加环境中的物品缩略图。使用此注释地图自动生成 3D 世界的能力对于简化模拟的创建和管理具有重要价值。为此,“traffic_editor”中的“building_map_tools”包包含一个可执行文件“building_map_generator”。可执行文件以两种模式运行:
-
生成符合 Gazebo/Ignition 的“.world”文件
-
以导航图的形式导出特定于车队的交通信息,供“fleet_adapters”用于规划
为了自动生成 Gazebo 模拟世界,可执行文件接受命令参数“gazebo”以及下面描述的其他参数:
usage: building_map_generator gazebo [-h] [-o [OPTIONS [OPTIONS ...]]] [-n]
[-m MODEL_PATH] [-c CACHE]
INPUT OUTPUT_WORLD OUTPUT_MODEL_DIR
positional arguments:
INPUT Input building.yaml file to process
OUTPUT_WORLD Name of the .world file to output
OUTPUT_MODEL_DIR Path to output the map model files
optional arguments:
-h, --help show this help message and exit
-o [OPTIONS [OPTIONS ...]], --options [OPTIONS [OPTIONS ...]]
Generator options
-n, --no_download Do not download missing models from Fuel
-m MODEL_PATH, --model_path MODEL_PATH
Gazebo model path to check for models
-c CACHE, --cache CACHE
Path to pit_crew model cache
该脚本解析 .building.yaml
文件并生成每个楼层的地板和墙壁的网格。然后,这些网格被组合成 OUTPUT_MODEL_DIR/
目录中的 model.sdf
文件。每个楼层的 model.sdf
文件都导入到 .world
中,文件路径为 OUTPUT_WORLD
。traffic_editor
中注释的各种静态对象的模型子元素包含在 .world
中,如以下代码片段所示:
<include>
<name>OfficeChairBlack_6</name>
<uri>model://OfficeChairBlack</uri>
<pose>4.26201267190027 -7.489812761393875 0 0 0 1.1212</pose>
<static>True</static>
</include>
为带注释的机器人生成了类似的块。在 Gazebo 中加载 .world
文件之前,用户有责任将环境变量 $GAZEBO_MODEL_PATH
和模型的相关路径附加到该变量中。此过程可以通过 ROS 2 启动文件简化,将在后面的章节中讨论。
解析器还包括其他动态资产(如门和电梯)的 sdf 元素。它们的机制将在下一节中讨论。可以使用命令参数 ignition
生成与 Ignition 兼容的世界。
重新配置模拟环境变得像编辑 2D 图纸上的注释并重新运行 building_map_generator
一样简单。这对于在设施中的空间配置发生变化时快速评估交通流量非常有用。
要为车队适配器生成导航图,请使用命令参数 nav
执行 building_map_generator
。导航图以 .yaml
文件形式生成,并在启动期间由相应的舰队适配器进行解析。
usage: building_map_generator nav [-h] INPUT OUTPUT_DIR
positional arguments:
INPUT Input building.yaml file to process
OUTPUT_DIR Path to output the nav .yaml files
RMF 资产和插件
资产在模拟环境中重现起着关键作用。RMF、SubT 等项目允许开发人员创建和开源机器人、机械基础设施系统和场景对象的 3D 模型。它们可在 Ignition Fuel 应用程序 上下载。除了提供视觉准确性之外,资产还可以动态化,并通过插件与 RMF 核心系统交互。
为了模拟机器人模型和基础设施系统等硬件的行为,已经设计了多个 Gazebo 插件。这些插件是 ModelPlugin 类的衍生产品,并与标准 ROS 2 和 RMF 核心消息相结合以提供必要的功能。以下部分简要介绍了其中一些插件。
Robots
如前所述,已有多种机器人模型(SESTO、MiR100、Magni、Hospi)开源用于模拟。为了使这些模型模拟已与 RMF 集成的物理对应模型的行为,它们需要 1) 与 rmf_fleet_adapters
交互,以及 2) 导航到模拟世界中的位置。对于“完全控制”机器人类型,这些功能是通过 slotcar
插件 实现的。该插件订阅 /robot_path_requests
和 /robot_mode_requests
主题,并响应其 rmf_fleet_adapter
发布的相关 PathRequest
和 ModeRequest
消息。该插件还将机器人的状态发布到 /robot_state
主题。
为了通过 PathRequest
消息中的路径点来导航机器人,我们利用了一种简单的
“轨道式”导航算法,该算法使机器人沿着直线从当前位置加速和减速到下一个路径点。
该插件依赖于以下基本假设:
- 机器人模型为两轮差速驱动机器人
- 左、右轮关节分别命名为“joint_tire_left”和“joint_tire_right”
其他参数(其中大部分是机器人的运动学特性)是从 sdf 参数推断出来的:
<plugin name="slotcar" filename="libslotcar.so">
<nominal_drive_speed>0.5</nominal_drive_speed>
<nominal_drive_acceleration>0.25</nominal_drive_acceleration>
<max_drive_acceleration>0.75</max_drive_acceleration>
<nominal_turn_speed>0.6</nominal_turn_speed>
<nominal_turn_acceleration>1.5</nominal_turn_acceleration>
<max_turn_acceleration>2.0</max_turn_acceleration>
<tire_radius>0.1</tire_radius>
<base_width>0.3206</base_width>
<stop_distance>0.75</stop_distance>
<stop_radius>0.75</stop_radius>
</plugin>
在模拟过程中,假设机器人的路径没有静态障碍物,但插件仍包含逻辑,如果在路径中检测到障碍物,则暂停机器人的运动。虽然可以部署基于传感器的导航堆栈,但避免使用这种方法,以尽量减少系统在模拟中为每个机器人运行导航堆栈的计算负载。鉴于重点是异构车队的交通管理而不是机器人导航,slotcar
插件提供了一种有效的方法来模拟 RMF 核心系统和机器人之间的交互。
slotcar
插件旨在作为一种通用解决方案。我们鼓励供应商开发和分发能够更准确地代表其机器人功能和与 RMF 集成水平的插件。
Doors
与几何形状固定且可直接包含在生成的 .world
文件中的机器人模型不同,门是在 traffic_editor
中自定义的,并具有自己的生成管道。如下图所示,带注释的门具有多个属性,包括其末端的位置、门的类型(铰链门、双铰链门、滑动门、双滑动门)及其运动范围(对于铰链门)。
building_map_generator gazebo
脚本解析 .building.yaml
文件中的任意门,并自动生成带有门所需的链接和接头的 sdf 子元素以及配置的插件。上图中为门生成的 sdf 子元素如下所示。
<model name="coe_door">
<pose>8.077686357313898 -5.898342045416362 0.0 0 0 1.1560010438234292</pose>
<plugin filename="libdoor.so" name="door">
<v_max_door>0.5</v_max_door>
<a_max_door>0.3</a_max_door>
<a_nom_door>0.15</a_nom_door>
<dx_min_door>0.01</dx_min_door>
<f_max_door>500.0</f_max_door>
<door left_joint_name="left_joint" name="coe_door" right_joint_name="empty_joint" type="SwingDoor" />
</plugin>
<link name="left">
<pose>0 0 1.11 0 0 0</pose>
<visual name="left">
<material>
<ambient>120 60 0 0.6</ambient>
<diffuse>120 60 0 0.6</diffuse>
</material>
<geometry>
<box>
<size>0.8766026166317483 0.03 2.2</size>
</box>
</geometry>
</visual>
<collision name="left">
<surface>
<contact>
<collide_bitmask>0x02</collide_bitmask>
</contact>
</surface>
<geometry>
<box>
<size>0.8766026166317483 0.03 2.2</size>
</box>
</geometry>
</collision>
<inertial>
<mass>50.0</mass>
<inertia>
<ixx>20.17041666666667</ixx>
<iyy>23.36846728119012</iyy>
<izz>3.20555061452345</izz>
</inertia>
</inertial>
</link>
<joint name="left_joint" type="revolute">
<parent>world</parent>
<child>left</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1.57</lower>
<upper>0</upper>
</limit>
</axis>
<pose>0.44330130831587417 0 0 0 0 0</pose>
</joint>
</model>
门插件 响应 DoorRequest
消息,其中 door_name
与其 model name
sdf 标签匹配。这些消息通过 /door_requests
主题发布。该插件与定义的门类型无关,并依靠 left_joint_name
和 right_joint_name
参数来确定在打开和关闭运动期间要启动哪些关节。在这些运动期间,关节被命令达到其在父元素中指定的适当限度。关节运动遵循 sdf 参数指定的运动学约束,同时遵循与 slotcar
类似的加速度和减速度曲线。
为了避免一个机器人请求另一个机器人关闭门的情况,在实践中部署了一个 door_supervisor
节点。该节点发布到 /door_requests
并订阅 /adapter_door_requests
,当机器人需要通过门进入时,队列适配器会发布到该节点。door_supervisor
会跟踪系统中所有队列适配器的请求,并将请求转发给门适配器,同时避免上述冲突。
Lifts
测试电梯集成的能力至关重要,因为这些系统通常是设施中的运营瓶颈,因为它们由人类和多机器人车队共同使用。与带注释的门一样,电梯可以在“traffic_editor”GUI 中以多种方式进行自定义,包括机舱的尺寸和方向以及将机舱门映射到建筑物楼层。
building_map_generator gazebo
脚本解析 .building.yaml
文件中的电梯定义,并自动生成客舱、客舱门和电梯井门的 sdf 元素。
客舱底部定义了一个棱柱形关节,由电梯插件驱动,使客舱在不同楼层之间移动。
虽然客舱门是客舱结构的一部分,但井门固定在建筑物上。
两组门在给定的楼层同时打开和关闭,并由电梯插件本身控制。
这些门使用与建筑物中其他门相同的方法创建,并且还包括门插件。
building_map_generator
还附加了一个升降机插件(TODO 将带有所需参数的链接元素添加到升降机的模型 sdf 块。)
<plugin filename="liblift.so" name="lift">
<lift_name>Lift1</lift_name>
<floor elevation="0.0" name="L1">
<door_pair cabin_door="CabinDoor_Lift1_door1" shaft_door="ShaftDoor_Lift1_L1_door1" />
</floor>
<floor elevation="10.0" name="L2">
<door_pair cabin_door="CabinDoor_Lift1_door1" shaft_door="ShaftDoor_Lift1_L2_door1" />
<door_pair cabin_door="CabinDoor_Lift1_door2" shaft_door="ShaftDoor_Lift1_L2_door2" />
</floor>
<floor elevation="20.0" name="L3">
<door_pair cabin_door="CabinDoor_Lift1_door1" shaft_door="ShaftDoor_Lift1_L3_door1" />
</floor>
<reference_floor>L1</reference_floor>
<v_max_cabin>2.0</v_max_cabin>
<a_max_cabin>1.2</a_max_cabin>
<a_nom_cabin>1.0</a_nom_cabin>
<dx_min_cabin>0.001</dx_min_cabin>
<f_max_cabin>25323.0</f_max_cabin>
<cabin_joint_name>cabin_joint</cabin_joint_name>
</plugin>
该插件订阅 /lift_requests
主题,并使用与其 model name
sdf 标签匹配的 lift_name
来响应 LiftRequest
消息。
计算机舱当前高度与 destination_floor
高度之间的位移,并对机舱关节施加合适的速度。
在任何运动之前,机舱门都会关闭,并且只有在 LiftRequest
消息中指定的情况下,才会在 destination_floor
处打开。
由于机舱和井道门配置了 door
插件,因此它们通过 lift
插件发布的 DoorRequest
消息进行命令。
类似于 door_supervisor
,在实践中启动了一个 lift_supervisor
节点 来管理来自不同机器人车队的请求。
Workcells
一种常见的用例是机器人在设施内执行交付,因此“交付”任务被配置到“rmf_fleet_adapters”中。 在交付任务中,有效载荷在一个位置(拾取路径点)装载到机器人上,并在另一个位置(卸下路径点)卸载。 设施中的机器人/工作单元可以自动将有效载荷装载到机器人上和从机器人上卸载。这些设备此后分别称为分配器和摄取器。
为了在模拟中复制装载和卸载过程,我们设计了 TeleportDispenser
和 TeleportIngestor
插件。
这些插件分别附加到 TeleportDispenser
和 TeleportIngestor
3D 模型。
要在模拟中设置有效载荷装载站:
- 在拾取航点旁边添加一个
TeleportDispenser
模型并为其分配一个 唯一的name
- 在
TeleportDispenser
模型旁边添加有效载荷模型(下图中的可乐罐) 要在模拟中设置有效载荷卸载站: - 在卸货航点旁边添加一个
TeleportIngestor
模型并为其分配一个 唯一的name
当发布 DispenserRequest
消息时,如果 target_guid
与 TeleportDispenser
模型的名称匹配,则插件会将有效载荷传送到最近的机器人模型上。相反,当发布 IngestorRequest
消息时,如果 target_guid
与 TeleportIngestor
模型的名称匹配,则 TeleportIngestor
插件会将有效载荷从机器人传送到其在世界上的位置。这些插件的组合允许模拟交付请求。
将来,这些机制将被实际的工作单元或机器人手臂取代,但底层消息交换将保持不变。
Crowdsim
人群模拟,又名“CrowdSim”,是 RMF 模拟中的一项可选功能。用户可以选择在“rmf_traffic_editor”上启用 crowdsim。在 RMF 中,crowdsim 插件使用 menge 作为核心来控制世界上每个模拟代理。
rmf_demos 的“airport_world”上演示了 crowdsim 的一个例子:
ros2 launch rmf_demos_gz airport_terminal.launch.xml use_crowdsim:=1
有关 crowdsim
的工作原理和配置方法的更多详细信息,
请深入了解 使用 Crowdsim 的详细指南。
创建模拟和运行场景
本节旨在概述 rmf_demos
存储库 中的各个组件,这些组件可作为设置其他模拟和为机器人分配任务的参考。在这里,我们将重点关注“办公室”世界。
地图包
rmf_demos_maps
包包含带注释的 traffic_editor
文件,这些文件将用于生成 3D 世界。在 traffic_editor
中打开 office.project.yaml
文件会显示一个单层平面图,其中标注了墙壁、地板、比例尺、门、车道和模型。所有机器人车道都设置为 双向
,graph_idx
等于“0”。后者表示所有车道都属于同一车队。在 airport
世界中,我们有两组图表,索引分别为“0”和“1”,分别反映两个车队可占用的车道。下图突出显示了分配给车道和作为机器人生成位置的航点的属性。
要导出 3D 世界文件以及导航图,请使用 building_map_generator
脚本。此包的 CMakeLists.txt
文件配置为在构建包时自动运行生成器脚本。输出安装到包的 share/
目录中。这样,演示中的其他包就可以轻松找到并使用生成的文件。
foreach(path ${traffic_editor_paths})
# Get the output world name
string(REPLACE "." ";" list1 ${path})
list(GET list1 0 name)
string(REPLACE "/" ";" list2 ${name})
list(GET list2 -1 world_name)
set(map_path ${path})
set(output_world_name ${world_name})
set(output_dir ${CMAKE_CURRENT_BINARY_DIR}/maps/${output_world_name})
set(output_world_path ${output_dir}/${output_world_name}.world)
set(output_model_dir ${output_dir}/models)
# first, generate the world
add_custom_command(
OUTPUT ${output_world_path}
COMMAND ros2 run rmf_building_map_tools building_map_generator gazebo ${map_path} ${output_world_path} ${output_model_dir}
DEPENDS ${map_path}
)
add_custom_target(generate_${output_world_name} ALL
DEPENDS ${output_world_path}
)
# now, generate the nav graphs
set(output_nav_graphs_dir ${output_dir}/nav_graphs/)
set(output_nav_graphs_phony ${output_nav_graphs_dir}/phony)
add_custom_command(
OUTPUT ${output_nav_graphs_phony}
COMMAND ros2 run rmf_building_map_tools building_map_generator nav ${map_path} ${output_nav_graphs_dir}
DEPENDS ${map_path}
)
add_custom_target(generate_${output_world_name}_nav_graphs ALL
DEPENDS ${output_nav_graphs_phony}
)
install(
DIRECTORY ${output_dir}
DESTINATION share/${PROJECT_NAME}/maps
)
endforeach()
启动文件
rmf_demos
包包含启动模拟世界和启动各种 RMF 服务所需的所有基本启动文件。使用 office.launch.xml
文件启动办公室模拟。首先,加载并启动 common.launch.xml
文件:
rmf_traffic_schedule
节点负责维护机器人轨迹数据库并监控交通冲突。如果检测到冲突,则向相关车队适配器发送通知,这些适配器开始协商过程以找到最佳解决方案。building_map_server
发布 UI 用于可视化的BuildingMap
消息。可执行文件将相关.building.yaml
文件的路径作为参数。使用find-pkg-share
替换命令可以找到rmf_demos_maps
包安装的office.building.yaml
文件,该文件存储在config_file
参数中。rmf_schedule_visualizer
是一个基于 RViz 的 UI,用于可视化车道、机器人的实际位置、反映在rmf_traffic_schedule
中的机器人预期轨迹以及门和电梯等建筑系统的状态。door_supervisor
和lift_supervisor
节点用于管理车队适配器和 UI 提交的请求。
<!-- Common launch -->
<include file="$(find-pkg-share demos)/common.launch.xml">
<arg name="use_sim_time" value="true"/>
<arg name="viz_config_file" value ="$(find-pkg-share demos)/include/office/office.rviz"/>
<arg name="config_file" value="$(find-pkg-share rmf_demos_maps)/office/office.building.yaml"/>
</include>
要在 Gazebo 中启动模拟世界,下面显示了来自 rmf_demos_gz 的代码片段。同样,用户也可以选择使用 ignition 模拟器 rmf_demos_ign 运行
<group>
<let name="world_path" value="$(find-pkg-share rmf_demos_maps)/maps/office/office.world" />
<let name="model_path" value="$(find-pkg-share rmf_demos_maps)/maps/office/models:$(find-pkg-share rmf_demos_assets)/models:/usr/share/gazebo-9/models" />
<let name="resource_path" value="$(find-pkg-share rmf_demos_assets):/usr/share/gazebo-9" />
<let name="plugin_path" value="$(find-pkg-prefix rmf_gazebo_plugins)/lib:$(find-pkg-prefix building_gazebo_plugins)/lib" />
<executable cmd="gzserver --verbose -s libgazebo_ros_factory.so -s libgazebo_ros_init.so $(var world_path)" output="both">
<env name="GAZEBO_MODEL_PATH" value="$(var model_path)" />
<env name="GAZEBO_RESOURCE_PATH" value="$(var resource_path)" />
<env name="GAZEBO_PLUGIN_PATH" value="$(var plugin_path)" />
<env name="GAZEBO_MODEL_DATABASE_URI" value="" />
</executable>
<executable cmd="gzclient --verbose $(var world_path)" output="both">
<env name="GAZEBO_MODEL_PATH" value="$(var model_path)" />
<env name="GAZEBO_RESOURCE_PATH" value="$(var resource_path)" />
<env name="GAZEBO_PLUGIN_PATH" value="$(var plugin_path)" />
</executable>
</group>
最后,为地图中注释的每种机器人类型启动“完全控制”rmf_fleet_adapter
的实例。building_map_generator
脚本生成的每个车队的导航图通过 nav_graph_file
参数传递。对于办公室地图,定义了一个 Magni
机器人车队。因此,启动一个配置了此机器人类型的运动学属性以及用于规划的空间阈值的单个 magni_adapter.launch.xml
文件。与车队适配器一起启动的还有 robot_state_aggregator
节点。此节点使用包含 robot_prefix
参数的 RobotState.name
聚合 RobotState
消息,并使用 fleet_name
参数指定的 FleetState.name
将聚合发布到 /fleet_states
。
<group>
<let name="fleet_name" value="magni"/>
<include file="$(find-pkg-share rmf_demos)/include/adapters/magni_adapter.launch.xml">
<arg name="fleet_name" value="$(var fleet_name)"/>
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="nav_graph_file" value="$(find-pkg-share rmf_demos_maps)/maps/office/nav_graphs/0.yaml" />
</include>
<include file="$(find-pkg-share rmf_fleet_adapter)/robot_state_aggregator.launch.xml">
<arg name="robot_prefix" value="magni"/>
<arg name="fleet_name" value="$(var fleet_name)"/>
<arg name="use_sim_time" value="true"/>
</include>
</group>
在使用硬件测试 RMF 时,可以使用相同的启动文件,但启动“Gazebo”除外。 有关使用硬件运行演示的更多信息,请参阅有关集成的章节。
任务请求
RMF 支持各种开箱即用的任务。有关更多信息,请参阅 RMF 中的任务
提供基于 Web 的仪表板,允许用户向 RMF 发送命令。
启动 仪表板服务器 后,可通过 https://open-rmf.github.io/rmf-panel-js/ 访问。
另外,在“rmf_demos_tasks”中还存在几个脚本,可帮助用户从终端提交请求。目前,“dispatch_loop.py”、“dispatch_delivery.py”和“dispatch_clean.py”脚本可用于提交“Loop”、“Delivery”和“Clean”请求。
结论
本章介绍了如何使用 traffic_editor
工具创建带注释的地图,以便自动生成用于模拟的 3D 世界。
它还介绍了模拟中使用的资产以及 ROS 2 和 RMF 与它们交互所需的相应插件。
提供了一个这些组件一起运行的工作示例,以 rmf_demos_maps
包的形式提供,作为如何实现自定义系统的参考。
下一章将介绍 RMF 背后的基本概念。