ego-planner开源代码之simulator.xml介绍&分析

1. 源由

ego-planner开源代码之数据流分析工作的延续。

• 对于算法逻辑的数据流，在前面已经初步做了整理，部分主要模块已经有比较清晰的认识。
• 从数据流的角度，理解模块的工作逻辑，还是非常直观、简洁的。

如何从系统的角度，思考问题，尤其是一个系统由众多模块组成的复杂体系，要能够整体的理解，反而并不能一下子上手。

反之，若能够将整个系统作为一个模块来理解，入参和出参分别如何对应和理解呢？

2. simulator配置

• simulator.xml

2.1 配置入参

  <arg name="init_x" value="-18.0"/>
  <arg name="init_y" value="0.0"/>
  <arg name="init_z" value="0.0"/> 
  <arg name="obj_num" value="1" />
  <arg name="map_size_x_"/>
  <arg name="map_size_y_"/>
  <arg name="map_size_z_"/>
  <arg name="c_num"/>
  <arg name="p_num"/>
  <arg name="min_dist"/>
  <arg name="odometry_topic"/>

在启动文件中传递参数值给节点。每个 arg 标签定义了一个参数名和一个默认值（如果有的话），并且这些参数可以在启动节点时被传递和使用。

1. <arg name="init_x" value="-18.0"/>

   • 含义: 初始位置的 x 坐标，值为 -18.0。

2. <arg name="init_y" value="0.0"/>

   • 含义: 初始位置的 y 坐标，值为 0.0。

3. <arg name="init_z" value="0.0"/>

   • 含义: 初始位置的 z 坐标，值为 0.0。

4. <arg name="obj_num" value="1" />

   • 含义: 对象的数量，值为 1。

5. <arg name="map_size_x_"/>

   • 含义: 地图在 x 方向上的大小。这里没有指定默认值，实际值需要在运行时传递。

6. <arg name="map_size_y_"/>

   • 含义: 地图在 y 方向上的大小。这里没有指定默认值，实际值需要在运行时传递。

7. <arg name="map_size_z_"/>

   • 含义: 地图在 z 方向上的大小。这里没有指定默认值，实际值需要在运行时传递。

8. <arg name="c_num"/>

   • 含义: 参数 c 的数量。这里没有指定默认值，实际值需要在运行时传递。

9. <arg name="p_num"/>

   • 含义: 参数 p 的数量。这里没有指定默认值，实际值需要在运行时传递。

10. <arg name="min_dist"/>

    • 含义: 最小距离值。这里没有指定默认值，实际值需要在运行时传递。

11. <arg name="odometry_topic"/>

    • 含义: 里程计数据的 ROS 话题名称。这里没有指定默认值，实际值需要在运行时传递。

在 simple_run.launch 中相关的覆盖值定义是：

  <arg name="map_size_x" value="40.0"/>
  <arg name="map_size_y" value="40.0"/>
  <arg name="map_size_z" value=" 3.0"/>

  <!-- topic of your odometry such as VIO or LIO -->
  <arg name="odom_topic" value="/visual_slam/odom" />

  <!-- use simulator -->
  <include file="$(find ego_planner)/launch/simulator.xml">
    <arg name="map_size_x_" value="$(arg map_size_x)"/>
    <arg name="map_size_y_" value="$(arg map_size_y)"/>
    <arg name="map_size_z_" value="$(arg map_size_z)"/>
    <arg name="c_num" value="200"/>
    <arg name="p_num" value="200"/>
    <arg name="min_dist" value="1.2"/>

    <arg name="odometry_topic" value="$(arg odom_topic)" />
  </include>

2.2 mockamap_node 地图生成节点

  <node pkg="mockamap" type="mockamap_node" name="mockamap_node" output="screen">  
      <remap from="/mock_map" to="/map_generator/global_cloud"/>

      <param name="seed" type="int" value="127"/>
      <param name="update_freq" type="double" value="0.5"/>

      <!--  box edge length, unit meter-->
      <param name="resolution" type="double" value="0.1"/>

      <!-- map size unit meter-->
      <param name="x_length" value="$(arg map_size_x_)"/>
      <param name="y_length" value="$(arg map_size_y_)"/>
      <param name="z_length" value="$(arg map_size_z_)"/>

      <param name="type" type="int" value="1"/>
      <!-- 1 perlin noise parameters -->
      <!-- complexity:    base noise frequency,
                              large value will be complex
                              typical 0.0 ~ 0.5 -->
      <!-- fill:          infill persentage
                              typical: 0.4 ~ 0.0 -->
      <!-- fractal:       large value will have more detail-->
      <!-- attenuation:   for fractal attenuation
                              typical: 0.0 ~ 0.5 -->

      <param name="complexity"    type="double" value="0.05"/>
      <param name="fill"          type="double" value="0.12"/>
      <param name="fractal"       type="int"    value="1"/>
      <param name="attenuation"   type="double" value="0.1"/>
  </node>

定义了一个基于噪声的地图生成节点，可以通过指定分辨率、地图大小以及噪声的复杂度等参数生成复杂的三维地图。

1. <node pkg="mockamap" type="mockamap_node" name="mockamap_node" output="screen">

   • pkg=“mockamap”: 表示该节点属于mockamap包。
   • type=“mockamap_node”: 节点的可执行文件名称为mockamap_node。
   • name=“mockamap_node”: 设置该节点的名称为mockamap_node。
   • output=“screen”: 输出将显示在终端（屏幕）上。

2. <remap from="/mock_map" to="/map_generator/global_cloud"/>

   • from=“/mock_map”: 将节点中的话题/mock_map重新映射到其他话题。
   • to=“/map_generator/global_cloud”: 重映射的目标是/map_generator/global_cloud，即节点会将原本发布在/mock_map上的数据发布到/map_generator/global_cloud上。

3. <param name="seed" type="int" value="127"/>

   • seed: 设置随机种子，类型为整数，值为127，用于生成一致的伪随机地图。

4. <param name="update_freq" type="double" value="0.5"/>

   • update_freq: 设置地图更新的频率，类型为双精度浮点数，值为0.5，即每秒更新0.5次（2秒更新一次）。

5. <param name="resolution" type="double" value="0.1"/>

   • resolution: 地图的分辨率，类型为双精度浮点数，值为0.1米，即每个网格的边长为0.1米。

6. <param name="x_length" value="$(arg map_size_x_)"/>, <param name="y_length" value="$(arg map_size_y_)"/>, <param name="z_length" value="$(arg map_size_z_)"/>

   • x_length/y_length/z_length: 地图在x、y和z方向上的长度，单位为米，分别从外部传入参数map_size_x_、map_size_y_和map_size_z_来设置。

7. <param name="type" type="int" value="1"/>

   • type: 地图生成的类型，值为1，通常与特定的生成算法相关联（例如噪声生成类型）。

8. 噪声生成的具体参数：

   • complexity: 基础噪声的频率，值为0.05，表示噪声的复杂度。值越大，噪声越复杂，典型值为0.0 ~ 0.5。
   • fill: 填充百分比，值为0.12，表示生成地图时的填充率，典型值为0.4 ~ 0.0。
   • fractal: 分形的级数，值为1，值越大，细节越丰富。
   • attenuation: 分形的衰减系数，值为0.1，表示分形噪声的衰减，典型值为0.0 ~ 0.5。

2.3 quadrotor_simulator_so3 四旋翼仿真节点

  <node pkg="so3_quadrotor_simulator" type="quadrotor_simulator_so3" name="quadrotor_simulator_so3" output="screen">
        <param name="rate/odom" value="200.0"/>
        <param name="simulator/init_state_x" value="$(arg init_x)"/>
        <param name="simulator/init_state_y" value="$(arg init_y)"/>
        <param name="simulator/init_state_z" value="$(arg init_z)"/>

        <remap from="~odom" to="/visual_slam/odom"/>
        <remap from="~cmd" to="so3_cmd"/>
        <remap from="~force_disturbance" to="force_disturbance"/>    
        <remap from="~moment_disturbance" to="moment_disturbance"/>        
  </node>

启动了一个四旋翼仿真节点，属于so3_quadrotor_simulator包，节点名称为quadrotor_simulator_so3，初始状态由外部参数指定，频率高达200Hz的里程计信息用于其他系统（如SLAM）。仿真还支持控制指令的重映射，以及力和力矩的干扰处理。

1. <node pkg="so3_quadrotor_simulator" type="quadrotor_simulator_so3" name="quadrotor_simulator_so3" output="screen">

   • pkg=“so3_quadrotor_simulator”: 节点属于so3_quadrotor_simulator包。
   • type=“quadrotor_simulator_so3”: 启动的可执行文件为quadrotor_simulator_so3，这是四旋翼仿真器的SO3版本。
   • name=“quadrotor_simulator_so3”: 节点名称为quadrotor_simulator_so3。
   • output=“screen”: 输出显示在终端（屏幕）上。

2. <param name="rate/odom" value="200.0"/>

   • rate/odom: 发布里程计（odom）数据的频率，值为200.0Hz，表示每秒发布200次里程计数据，提供高精度的位置和速度信息。

3. <param name="simulator/init_state_x" value="$(arg init_x)"/>
<param name="simulator/init_state_y" value="$(arg init_y)"/>
<param name="simulator/init_state_z" value="$(arg init_z)"/>

   • simulator/init_state_x, init_state_y, init_state_z: 设置四旋翼仿真器的初始状态，分别是x、y、z三个方向的初始位置。参数init_x、init_y和init_z是通过外部传入的ROS参数。这些值通常用于设置四旋翼在仿真中的起始位置。

4. <remap from="~odom" to="/visual_slam/odom"/>

   • ~odom: 仿真器发布的局部odom话题被重映射到全局的/visual_slam/odom话题。通常，这是为了与视觉SLAM系统共享里程计数据。

5. <remap from="~cmd" to="so3_cmd"/>

   • ~cmd: 仿真器接收的控制指令话题~cmd被重映射到so3_cmd话题。这里的so3_cmd通常表示四旋翼姿态控制的SO(3)指令，用于控制仿真器的姿态。

6. <remap from="~force_disturbance" to="force_disturbance"/>

   • ~force_disturbance: 仿真器中影响四旋翼的力干扰（force disturbance）话题。此话题被直接映射，没有更改话题名称。

7. <remap from="~moment_disturbance" to="moment_disturbance"/>

   • ~moment_disturbance: 仿真器中影响四旋翼的力矩干扰（moment disturbance）话题，也保持话题名称不变。

2.4 Nodelet机制 四旋翼控制节点

  <node pkg="nodelet" type="nodelet" args="standalone so3_control/SO3ControlNodelet" name="so3_control" required="true" output="screen">
        <param name="so3_control/init_state_x" value="$(arg init_x)"/>
        <param name="so3_control/init_state_y" value="$(arg init_y)"/>
        <param name="so3_control/init_state_z" value="$(arg init_z)"/>
        <remap from="~odom" to="/visual_slam/odom"/>
        <remap from="~position_cmd" to="/planning/pos_cmd"/>
        <remap from="~motors" to="motors"/>
        <remap from="~corrections" to="corrections"/>
        <remap from="~so3_cmd" to="so3_cmd"/>
        <rosparam file="$(find so3_control)/config/gains_hummingbird.yaml"/>
        <rosparam file="$(find so3_control)/config/corrections_hummingbird.yaml"/>
        <param name="mass" value="0.98"/>
        <param name="use_angle_corrections " value="false"/>
        <param name="use_external_yaw "      value="false"/>
        <param name="gains/rot/z" value="1.0"/>    
        <param name="gains/ang/z" value="0.1"/>        
  </node> 

配置启动了一个使用Nodelet机制的四旋翼控制节点，属于so3_control包，节点名为so3_control，使用SO(3)控制算法。节点重映射了多个话题用于接收视觉SLAM、规划模块的指令，并发布电机指令等。该控制器的增益和校正参数从外部YAML文件载入，并允许自定义质量和校正选项。

1. <node pkg="nodelet" type="nodelet" args="standalone so3_control/SO3ControlNodelet" name="so3_control" required="true" output="screen">

   • pkg=“nodelet”: 使用nodelet包，这意味着这是一个Nodelet节点，Nodelet可以将多个节点加载到同一进程中，以减少进程间通信的开销。
   • type=“nodelet” args=“standalone so3_control/SO3ControlNodelet”: 启动一个独立的Nodelet实例（standalone），加载so3_control包中的SO3ControlNodelet。
   • name=“so3_control”: 节点名称为so3_control。
   • required=“true”: 表示这个节点是必需的，若它崩溃，则会终止整个ROS系统。
   • output=“screen”: 输出信息显示在终端上。

2. <param name="so3_control/init_state_x" value="$(arg init_x)"/>
<param name="so3_control/init_state_y" value="$(arg init_y)"/>
<param name="so3_control/init_state_z" value="$(arg init_z)"/>

   • so3_control/init_state_x, init_state_y, init_state_z: 初始化控制节点中四旋翼的位置，分别是x、y、z三个方向的初始位置。这些值通过外部参数传入（init_x, init_y, init_z）。

3. <remap from="~odom" to="/visual_slam/odom"/>

   • ~odom: 里程计话题~odom重映射到/visual_slam/odom，用于从视觉SLAM系统接收里程计数据。

4. <remap from="~position_cmd" to="/planning/pos_cmd"/>

   • ~position_cmd: 重映射控制节点的位置信息指令~position_cmd到/planning/pos_cmd，用于从规划模块接收位置指令。

5. <remap from="~motors" to="motors"/>

   • ~motors: 电机控制指令~motors被重映射到话题motors。

6. <remap from="~corrections" to="corrections"/>

   • ~corrections: 纠正信息话题~corrections，通常用于从外部传感器或模块获取姿态或其他校正数据。

7. <remap from="~so3_cmd" to="so3_cmd"/>

   • ~so3_cmd: SO(3)控制指令话题~so3_cmd，用于控制四旋翼的姿态。

8. <rosparam file="$(find so3_control)/config/gains_hummingbird.yaml"/>

   • gains_hummingbird.yaml: 载入so3_control包中的gains_hummingbird.yaml文件，用于配置控制器的增益参数，尤其是姿态控制的增益。

9. <rosparam file="$(find so3_control)/config/corrections_hummingbird.yaml"/>

   • corrections_hummingbird.yaml: 载入corrections_hummingbird.yaml，该文件包含姿态校正相关的参数。

10. <param name="mass" value="0.98"/>

    • mass: 设置四旋翼的质量为0.98公斤，控制算法会根据这个质量进行动态计算。

11. <param name="use_angle_corrections" value="false"/>

    • use_angle_corrections: 是否使用角度校正，设置为false，表示当前不启用角度校正。

12. <param name="use_external_yaw" value="false"/>

    • use_external_yaw: 是否使用外部的偏航角校正，设置为false，即不使用外部的偏航控制。

13. <param name="gains/rot/z" value="1.0"/>

    • gains/rot/z: 设置z轴上的旋转增益，值为1.0，用于控制四旋翼在z轴上的姿态响应。

14. <param name="gains/ang/z" value="0.1"/>

    • gains/ang/z: 设置z轴上的角度增益，值为0.1，用于姿态控制中的角度调整。

2.5 odom_visualization 里程计数据

  <node pkg="odom_visualization" name="odom_visualization" type="odom_visualization" output="screen">
        <remap from="~odom" to="/visual_slam/odom"/>
        <param name="color/a" value="1.0"/>    
        <param name="color/r" value="0.0"/>        
        <param name="color/g" value="0.0"/>        
        <param name="color/b" value="0.0"/>       
        <param name="covariance_scale" value="100.0"/>       
        <param name="robot_scale" value="1.0"/>
        <param name="tf45" value="true"/>
  </node>

配置启动了一个名为odom_visualization的节点，用于可视化视觉SLAM系统中的里程计数据，并配置了颜色、缩放、以及其他可视化相关的参数。

1. <node pkg="odom_visualization" name="odom_visualization" type="odom_visualization" output="screen">

   • pkg=“odom_visualization”: 该节点属于odom_visualization包，用于可视化里程计数据。
   • name=“odom_visualization”: 节点名称为odom_visualization。
   • type=“odom_visualization”: 启动可执行文件名称为odom_visualization。
   • output=“screen”: 输出显示在终端（屏幕）上。

2. <remap from="~odom" to="/visual_slam/odom"/>

   • ~odom: 节点接收的本地odom话题重映射到/visual_slam/odom，该话题通常是视觉SLAM系统发布的里程计数据。

3. <param name="color/a" value="1.0"/>, <param name="color/r" value="0.0"/>, <param name="color/g" value="0.0"/>, <param name="color/b" value="0.0"/>

   • color/a: 设置里程计可视化中颜色的透明度，值为1.0，表示完全不透明。
   • color/r, color/g, color/b: 设置颜色的RGB值，分别为红（r）、绿（g）、蓝（b）。此处RGB值为(0, 0, 0)，表示可视化物体为黑色。

4. <param name="covariance_scale" value="100.0"/>

   • covariance_scale: 设置协方差的可视化比例，值为100.0，表示将协方差矩阵在可视化中按此比例放大。

5. <param name="robot_scale" value="1.0"/>

   • robot_scale: 设置机器人模型的可视化缩放比例，值为1.0，即保持与实际尺寸相同。

6. <param name="tf45" value="true"/>

   • tf45: 此参数设置为true，具体含义依赖于节点的实现，通常用于处理坐标变换或指定某些特殊的可视化模式。可能与坐标系变换TF相关，比如应用一个45度的变换矩阵。

2.6 pcl_render_node 本地感知

  <node pkg="local_sensing_node" type="pcl_render_node" name="pcl_render_node" output="screen">
        <rosparam command="load" file="$(find local_sensing_node)/params/camera.yaml" />
        <param name="sensing_horizon"  value="5.0" />
        <param name="sensing_rate"     value="30.0"/>
        <param name="estimation_rate"  value="30.0"/>

        <param name="map/x_size"     value="$(arg map_size_x_)"/>
        <param name="map/y_size"     value="$(arg map_size_y_)"/>
        <param name="map/z_size"     value="$(arg map_size_z_)"/>

        <remap from="~global_map" to="/map_generator/global_cloud"/>
        <remap from="~odometry"   to="$(arg odometry_topic)"/>
  </node>

配置启动了一个名为pcl_render_node的节点，属于local_sensing_node包，主要用于本地感知，并处理从相机和里程计等传感器获得的数据，感知范围为5米，传感和估计的频率为30Hz。节点还加载了相机的配置文件，使用了重映射来处理全局地图和里程计数据。

1. <node pkg="local_sensing_node" type="pcl_render_node" name="pcl_render_node" output="screen">

   • pkg=“local_sensing_node”: 该节点属于local_sensing_node包，通常用于本地感知和处理点云（PCL）。
   • type=“pcl_render_node”: 启动可执行文件为pcl_render_node，用于渲染点云或进行相关的传感任务。
   • name=“pcl_render_node”: 节点名称为pcl_render_node。
   • output=“screen”: 输出信息显示在终端（屏幕）上。

2. <rosparam command="load" file="$(find local_sensing_node)/params/camera.yaml" />

   • command=“load”: 加载指定的参数文件。
   • file=“$(find local_sensing_node)/params/camera.yaml”: 从local_sensing_node包中的params文件夹中加载camera.yaml文件。这通常是相机相关的参数配置文件，如视角、分辨率等。

3. <param name="sensing_horizon" value="5.0" />

   • sensing_horizon: 设置感知范围为5.0米，即传感器能感知的最大距离为5米。

4. <param name="sensing_rate" value="30.0"/>

   • sensing_rate: 设置传感的频率为30.0Hz，表示传感器每秒采集30次数据。

5. <param name="estimation_rate" value="30.0"/>

   • estimation_rate: 设置估计频率为30.0Hz，表示估计模块每秒处理30次数据（如位置、姿态估计）。

6. <param name="map/x_size" value="$(arg map_size_x_)"/>
<param name="map/y_size" value="$(arg map_size_y_)"/>
<param name="map/z_size" value="$(arg map_size_z_)"/>

   • map/x_size, map/y_size, map/z_size: 设置地图的尺寸，x_size、y_size、z_size分别表示地图在x、y、z方向上的大小。这些参数通过外部传入（map_size_x_, map_size_y_, map_size_z_）。

7. <remap from="~global_map" to="/map_generator/global_cloud"/>

   • ~global_map: 节点接收的本地global_map话题重映射为/map_generator/global_cloud，通常这是一个全局的点云地图，用于本地传感器进行匹配或融合。

8. <remap from="~odometry" to="$(arg odometry_topic)"/>

   • ~odometry: 节点接收的odometry（里程计）话题重映射为外部参数odometry_topic指定的话题。这允许从不同的来源获取里程计数据，例如从视觉SLAM或IMU等模块。

3. 总结

1. 经过topic整理，对于这么多节点在一起干了什么就清楚了很多，同时对于实飞情况下，应该注意哪些消息主题也就更加清楚了。

仿真消息主题：

• “/map_generator/global_cloud”
• “/visual_slam/odom”
• “/planning/pos_cmd”

四旋翼仿真消息主题：

• “so3_cmd”
• “force_disturbance”
• “moment_disturbance”
• “motors”
• “corrections”

2. 仿真环境内部配置文件

so3_control：

• “config/gains_hummingbird.yaml”
• “config/corrections_hummingbird.yaml”

# Vision Gain for Hummingbird
gains:
  pos: {x: 2.0, y: 2.0, z: 3.5}
  vel: {x: 1.8, y: 1.8, z: 2.0}
  rot: {x: 1.0, y: 1.0, z: 0.3}
  ang: {x: 0.07, y: 0.07, z: 0.02}  

corrections:
  z: 0.0
  r: 0.0
  p: 0.0

local_sensing_node：

• “params/camera.yaml”

cam_width:  640
cam_height: 480
cam_fx:     387.229248046875
cam_fy:     387.229248046875
cam_cx:     321.04638671875
cam_cy:     243.44969177246094

总的来说，如果不是研究模拟器的朋友，知道就可以，并不需要太过纠结！