Launch Multiple Moving Obstacles based off of YAML file in Gazebo

edit

I am trying to create a launch file that can read a YAML file that describes obstacles and there speed and then builds a world. Currently, I have two issues

1) How do I programatically launch multiple obstacles with a launch file?

I am able to launch a single robot given my code:

Launch file:

<?xml version="1.0"?>
<launch>

  <arg name="name" default="hmmwv"/>

  <!-- start gazebo and load the world -->
  <include file="$(find mavs_gazebo)/launch/world.launch" />

  <!-- spawn obstacles -->
  <param command="$(find xacro)/xacro --inorder $(find mavs_description)/urdf/obstacles.urdf.xacro " name="obstacles" />
  <node args="-urdf -param obstacles -model obstacles" name="spawn_obstacles" output="screen" pkg="gazebo_ros" respawn="false" type="spawn_model" />

</launch>

obstacles.urdf.xacro

<?xml version="1.0"?>
<robot name="obstacles" xmlns:xacro="http://www.ros.org/wiki/xacro" >

  <!-- obstacles-->
  <xacro:include filename="$(find mavs_description)/urdf/obstacle.urdf.xacro"/>

  <xacro:property name="mavs_params_file" value="$(find mavs_gazebo)/config/mavs_params.yaml"/>
  <xacro:property name="mavs" value="${load_yaml(mavs_params_file)}"/>

  <xacro:property name="radius" value="${mavs['mavs']['obs']['radius']}" />
  <xacro:property name="length" value="${mavs['mavs']['obs']['length']}" />
  <xacro:property name="x0" value="${mavs['mavs']['obs']['x0']}" />
  <xacro:property name="y0" value="${mavs['mavs']['obs']['y0']}" />

  <obstacle idx="0"/>

</robot>

obstacle.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="obstacle" params="idx">

    <link name="obstacle_${idx}" type="fixed">

      <inertial>
        <origin xyz="${x0[idx]} ${y0[idx]} ${length[idx]/2}" rpy="0 0 0"/>
        <mass value="1.0" />
        <inertia  ixx="0.0" ixy="0.0"  ixz="0.0"  iyy="0.0"  iyz="0.0"  izz="0.0" />
      </inertial>

      <visual>
        <origin xyz="${x0[idx]} ${y0[idx]} ${length[idx]/2}" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${radius[idx]}" length="${length[idx]}"  />
        </geometry>
      </visual>

      <collision>
        <origin xyz="${x0[idx]} ${y0[idx]} ${length[idx]/2}" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${radius[idx]}" length="${length[idx]}" />
        </geometry>
      </collision>

    </link>

    <gazebo>
      <static>true</static>
      <material>Gazebo/Black</material>
    </gazebo>

  </xacro:macro>

</robot>
enter code here

mavs_params.yaml

mavs:
 # obstacles
 obs:
   num: 1
   radius: [1.0,2.0]
   length: [2.,2.5]
   x0: [5.,-5]
   y0: [10.,15]
   vx: [0.,0]

So, this launches a single obstacle fine. But, if I add in two obstacles to obstacle.urdf.xacro with

  <obstacle idx="0"/>
  <obstacle idx="1"/>

The code complains because and says

Error [parser_urdf.cc:3423] Unable to call parseURDF on robot model
Error [parser.cc:342] parse as old deprecated model file failed.

So, it does not like me trying to create multiple obstacles inside a single node. I have looked into Populations, but I want flexibility of where they are located and I did not notice any options for placing everything in a particular place.

Eventually, to generate as many obstacles as I have in the YAML file I was going to use this code:

  <xacro:macro name="loop" params="idx">
      <obstacle idx="${idx}"/>
      <xacro:if value="${idx}">
          <xacro:loop idx="${idx-1}" />
      </xacro:if>
  </xacro:macro>

  <xacro:loop idx="${obstacle_num}" />

But, I realized that I cannot even ... (more)