Hey there,

I would like to spawn multiple objects in gazebo and track them in rviz. This means if the position of one object changes also the position in rviz should change.

Currently I think this just works if I create for each part:

• robotstatepublisher to visualize the stl of the part ( <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/> )
• groundtruth plugin from gazebo to track the position (`libgazeboros_p3d.so`)
• passing the tf ( <node name="odom_to_part_1" pkg="message_to_tf" type="message_to_tf"> )

This seems to me rather expensive. Is there a less expensive method you know?

I tried it like this:

main.launch [Code Snippet]:

<!-- load the model and start robot state publisher -->
<include file="$(find part_description)/launch/load_part2.launch">
   <arg name="part_name" value="$(arg part_name_1)" />
   <arg name="x" value="0.2"/>
</include>

<!-- spawns the model of one part to gazebo -->
<node name="spawn_urdf_part1" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg part_name_1) -param robot_description
-x 0.0 -y 0.0 -z 0.0
-R 0.0 -P 0.0 -Y 0.0" />

<!-- I could do this again at a different position, but then I cannot track it - I need a second odom as well or? 
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model $(arg part_name_2) -param robot_description
-x 0.1 -y 0.0 -z 0.0
-R 0.0 -P 0.0 -Y 0.0" />  -->

load_part2.launch:

  <!-- Load the sensor:-->
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find part_description)/urdf/part.xacro'
  part_name:=$(arg part_name)
  x:=$(arg x)" />

  <!-- required that the part is not in the origin of rviz -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>  

part.xacro:

<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="$(arg part_name)">

    <xacro:include filename="$(find part_description)/urdf/part.urdf.xacro" />


    <xacro:part_macro prefix="1" x="$(arg x)" y="0.1" z="0.5" r="0.0" p="0.0" yy="0.0"/>


</robot>

part.urdf.xacro:

<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" 
       xmlns:interface="http://ros.org/wiki/xacro"
       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" 
       xmlns:xacro="http://ros.org/wiki/xacro" 
       name="$(arg part_name)">


   <xacro:include filename="$(find part_description)/urdf/part.gazebo.xacro" />
   <xacro:include filename="$(find part_description)/urdf/common_properties.xacro"/>

   <xacro:property name="mass_washer"   value="0.08" />
   <xacro:property name="radius_washer" value="0.2" />



   <link name="part_footprint" />


   <xacro:macro name="part_macro" params="prefix x y z r p yy">
      <link name="part_link_${prefix}">
       <visual name="_part">
     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
     <geometry>
            <mesh filename="package://part_description/meshes/washer.stl" scale="0.001 0.001 0.001"/>
     </geometry>
     <cast_shadows>1</cast_shadows>
     <transparency>0</transparency>
    <!-- change the color in rviz with: -->
        <material name="white" >        </material>
       </visual>

         <inertial>
     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
            <mass value="${mass_washer}" />
            <inertia ixx="${mass_washer*radius_washer*radius_washer*0.25}" ixy="0" ixz="0" iyy="${mass_washer*radius_washer*radius_washer*0.25}" iyz="0" izz="${mass_washer*radius_washer*radius_washer*0.5}" />
         </inertial>
         <collision>
     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0" />
            <geometry>
            <mesh filename="package://part_description/meshes/washer.stl" scale="0.001 0.001 0.001"/>
            </geometry>
         </collision>
      </link>

      <joint name="part_joint_${prefix}" type="fixed">
     <origin rpy="${r} ${p} ${yy}" xyz="${x} ${y} ${z}" />
         <parent link="part_footprint" />
         <child link="part_link_${prefix}" />
      </joint>
   </xacro:macro>



</robot>

part.gazebo.xacro:

  <gazebo>
      <plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
         <frameName>world</frameName>
         <bodyName>part_footprint</bodyName>
         <topicName>/gazebo/$(arg part_name)/odom</topicName>
         <updateRate>100</updateRate>
      </plugin>
   </gazebo>

<!-- pass the tf of the part to rviz -->