Spawn multiple object in and track them in rviz

edit

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:

• robot_state_publisher to visualize the stl of the part ( <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/> )
• ground_truth plugin from gazebo to track the position (libgazebo_ros_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]:

<group ns="$(arg part_name_1)">

<!-- 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" />  -->

</group>

<node name="odom_to_part_1" pkg="message_to_tf" type="message_to_tf"> </node>

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 ...