Visualizing Kinect data from two Turtlebots in Gazebo

edit

Hi,

The problem you have with respect to rviz is due to malformed tf. When you are using multiple robots, you need to resolve the tf data for each of the robots by adding a "tf_prefix". By doing so all the links of the robot have unique names like robot1_tf/base_link, robot2_tf/odom etc and tf trees can be constructed for both robots and rviz can display both the robots.

To resolve the sensor data of the robots you have to provide a namespace for each robot. In the below shown case the scan topic become /robot1/scan and /robot1/camera/image_raw etc and the rviz is able to display the scan data of both the robots.

I use the following launch file:

   <launch>
  <!-- start gazebo with an empty plane -->
  <param name="/use_sim_time" value="true" />



  <node launch-prefix="optirun" name="gazebo" pkg="gazebo" type="gazebo" args="$(find angen_gazebo)/worlds/angen_empty.world" respawn="false" output="screen"/>
  <node name="gazebo_gui" pkg="gazebo" type="gui" />

<param name="robot_description"
    command="$(find xacro)/xacro.py $(find turtlebot_description)/urdf/turtlebot.urdf.xacro" />


  <group ns="robot1">
    <param name="tf_prefix" value="robot1" />
    <include file="$(find angen_turtlebot)/launch/fuerte_final_r1.launch" >
      <arg name="init_pose" value="-x 9 -y 5 -z 0.05" />
      <arg name="robot_name"  value="Robot1" />
</include>
  </group>

  <group ns="robot2">
    <param name="tf_prefix" value="robot2" />
    <include file="$(find angen_turtlebot)/launch/fuerte_final_r1.launch" >
      <arg name="init_pose" value="-x 9 -y 4 -z 0.05" />
      <arg name="robot_name"  value="Robot2" />
</include>
  </group>

</launch>

fuerte_final_r1.launch:

<launch>

    <arg name="init_pose"/>
    <arg name="robot_name"/>

  <param name="robot_description" command="$(find xacro)/xacro.py '$(find turtlebot_description)/urdf/turtlebot.urdf.xacro'" />

  <node name="spawn_turtlebot_model" pkg="gazebo" type="spawn_model" args="$(arg init_pose) -unpause -urdf -param /robot_description -model $(arg robot_name) -robotNamespace $(arg robot_name)" respawn="false" output="screen"/>

  <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" >
    <rosparam command="load" file="$(find turtlebot_bringup)/config/diagnostics.yaml" />
  </node>

  <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" output="screen">
    <param name="publish_frequency" type="double" value="30.0" />
  </node>


  <!-- The odometry estimator -->

  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="publish_tf" value="true"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="false"/>
    <param name="vo_used" value="false"/>
    <!--<param name="output_frame" value="odom"/>-->
  </node>


  <!-- throttling -->
  <node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager" respawn="true">
    <param name="max_rate" value="20.0"/>
    <remap from="cloud_in" to="/camera/depth/points"/>
    <remap from="cloud_out" to="cloud_throttled"/>
  </node>

  <!-- Fake Laser -->
  <node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
    <param name="output_frame_id" value="/camera_depth_frame"/>
    <!-- heights are in the (optical?) frame of the kinect -->
    <param name="min_height" value="-0.15"/>
    <param name="max_height" value="0.15"/>
    <remap from="cloud" to="/cloud_throttled"/>
  </node>

  <!-- Fake Laser (narrow one, for localization -->
  <node pkg="nodelet" type="nodelet" name="kinect_laser_narrow" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
    <param name="output_frame_id" value="/camera_depth_frame"/>
    <!-- heights are in the (optical?) frame of the kinect -->
    <param name="min_height" value="-0.025"/>
    <param name="max_height" value="0.025 ...