0 专栏介绍

本专栏旨在通过对ROS的系统学习，掌握ROS底层基本分布式原理，并具有机器人建模和应用ROS进行实际项目的开发和调试的工程能力。

🚀详情：《ROS从入门到精通》

1 动态生成障碍应用场景

动态生成障碍物在机器人工程领域应用非常广泛，例如

机器人导航与路径规划：动态生成障碍物可以用于评估导航算法在实时环境中的性能。通过在仿真环境中随机生成或基于真实环境数据生成障碍物，可以模拟真实世界中不断变化的环境条件，评估机器人导航算法的鲁棒性和适应性；
碰撞检测与避障算法测试：动态生成障碍物可以用于测试机器人的碰撞检测和避障算法。通过在仿真环境中生成障碍物，并模拟它们的运动，可以评估机器人系统在动态环境下的避障能力，并优化算法以适应快速变化的障碍物；
多机器人协作与冲突解决：动态生成障碍物可以用于研究多机器人系统的协作和冲突解决策略。通过在仿真环境中生成障碍物和多个机器人，并模拟它们的动态行为，可以评估多机器人系统在共享资源和避免冲突方面的效率和可行性；
…

动态生成障碍物技术的使用能够帮助开发人员和研究人员评估和改进机器人系统的感知、规划和控制算法，提高机器人在复杂和动态环境中的性能和鲁棒性。

在这里插入图片描述
本文就来探索如何基于ROS Gazebo动态生成障碍

2 基于Gazebo动态生成障碍

2.1 spawn_model服务

在Gazebo仿真环境中，gazebo/spawn_urdf_model服务用于在Gazebo中动态生成和加载URDF模型。对URDF格式不了解的同学请看ROS从入门到精通3-1：详解urdf语法并自定义机器人

具体来说，gazebo/spawn_urdf_model服务允许通过ROS的服务调用，将URDF模型加载到Gazebo仿真环境中，其参数含义是

model_name：要加载的模型的名称，必须是唯一的。
model_xml：包含URDF模型描述的XML字符串。
robot_namespace：机器人的命名空间，用于区分多个机器人模型。
initial_pose：机器人模型在仿真环境中的初始位置和姿态。

使用gazebo/spawn_urdf_model服务使得在仿真环境中可以动态地添加、移除或替换机器人模型或其他物体。这对于机器人开发和测试非常有用，因为可以在仿真环境中快速调试和验证新的机器人设计、传感器配置和控制算法。

2.2 动态构造障碍物URDF

URDF文件本质上是XML格式，所以我们可以很方便地用python的xml相关库创建URDF字符串，传入gazebo/spawn_urdf_model服务中，而不是使用真是存在的障碍物URDF文件，这对于动态配置障碍物属性非常重要。

动态构造障碍物URDF的核心代码如下，因为完整的URDF包含碰撞属性、可视属性等，所以代码篇幅较长，只截取了部分用于说明

def constructURDF(self, model_type: str, **kwargs) -> str:
     # parameters checking
     if model_type.upper() == "BOX":
         assert "m" in kwargs and \
             "w" in kwargs and \
             "d" in kwargs and \
             "h" in kwargs and \
             "c" in kwargs,    \
             "Parameters of {} are `m`, `w`, `d`, `h`, `c` which mean mass, \
                 width, depth, height and color, ".format(model_type)
         ixx = (kwargs["m"] / 12.0) * (pow(kwargs["d"], 2) + pow(kwargs["h"], 2))
         iyy = (kwargs["m"] / 12.0) * (pow(kwargs["w"], 2) + pow(kwargs["h"], 2))
         izz = (kwargs["m"] / 12.0) * (pow(kwargs["w"], 2) + pow(kwargs["d"], 2))
         geometry = ObstacleGenerator.createElement("geometry")
         geometry.append(ObstacleGenerator.createElement(model_type.lower(),
             props={"size": "{:f} {:f} {:f}".format(kwargs["w"], kwargs["d"], kwargs["h"])}))
     elif model_type.upper() == "CYLINDER":
         ...
     elif model_type.upper() == "SPHERE":
         ...
     else:
         raise NotImplementedError

     # URDF generation dynamically
     static_obs = ObstacleGenerator.createElement("robot", props={"name": model_type.lower()})
     link = ObstacleGenerator.createElement("link", props={"name": model_type.lower() + "_link"})

     ...

     return ET.tostring(static_obs).decode()

2.3 请求服务与动态生成

我们可以设置一个配置文件来配置动态障碍属性，脚本中读取这个配置

self.obs_cfg = ObstacleGenerator.yamlParser(self.root_path + "user_config/" + self.user_cfg["obstacles"])

接着按参数接口请求服务即可

pose = [float(i) for i in obs["pose"].split()]
x, y, z, r, p, yaw = pose
orient = tf.transformations.quaternion_from_euler(r, p, yaw)
orient = Quaternion(orient[0], orient[1], orient[2], orient[3])
params = obs["props"]
params["c"] = obs["color"]
urdf = self.constructURDF(obs["type"], **params)

if obs["type"] == "BOX":
    z = z if z else obs["props"]["h"] / 2
    pose = Pose(Point(x=x, y=y, z=z), orient)
    proxy(obs["type"] + str(len(self.box_obs)), urdf, "", pose, "world")
    self.box_obs.append(obs)

3 实测演示

假设用户配置文件为

# static obstacles
obstacles:
  - type: BOX
    pose: 5 2 0 0 0 0
    color: Grey
    props:
      m: 1.00
      w: 0.25
      d: 0.50
      h: 0.80
  - type: BOX
    pose: 6 2 0 0 0 0
    color: White
    props:
      m: 1.00
      w: 0.25
      d: 0.50
      h: 0.80
  - type: BOX
    pose: -7 3 0 0 0 0
    color: Grey
    props:
      m: 1.00
      w: 1.00
      d: 1.00
      h: 1.00