Problem with multirobots navigation

edit

I'm trying to do a multirobots navigation in Gazebo simulator using two turtlebot. I put the two turtlebot in Gazebo and I see in correct mode the robots in the environment with a correct transformation frame and topic.

This is the transformation frame https://db.tt/4CqQcmEe

and this is what I see with rqt_graph https://db.tt/UHNbe1Zb

These are the videos that document the problem:

move_Robot1.mp4 https://db.tt/EkmPTF75

move_Robot2.mp4 https://db.tt/YyXJpyiG

The video is very slow because of simulation that slow down the pc.

I run Rviz and I see the correct environment, RobotModel and the correct topic in Displays. In the first video (move_Robot1.mp4) I set a robot1/mose_base_simple/goal. In Rviz I see that Robot1 navigation is correct and it reach the goal position, but Robot2 has the same rotation than Robot1, even if it stays in place. Differently in Gazebo Robot2 stops, also in rotation. What I see in Rviz is not what I see in Gazebo. Another problem: as you can see in the second video, when I set a robot2/move_base_simple/goal in Rviz I see Robot2 stops, while in Gazebo Robot2 rotates continuosly, but it doesn't proceed back and forth.

cmd_vel topics seems correct and I supposed that the problem is in navigation driver. But I hope in some help.

This is my .launch file:

<launch> 
<arg name="base"      value="$(optenv TURTLEBOT_BASE kobuki)"/> <!-- create, roomba --> 
<arg name="battery"   value="$(optenv TURTLEBOT_BATTERY /proc/acpi/battery/BAT0)"/>  <!-- /proc/acpi/battery/BAT0 --> 
<arg name="stacks"    value="$(optenv TURTLEBOT_STACKS hexagons)"/>  <!-- circles, hexagons --> 
<arg name="3d_sensor" value="$(optenv TURTLEBOT_3D_SENSOR kinect)"/>  <!-- kinect, asus_xtion_pro --> 

<param name="/use_sim_time" value="true" /> 

<!-- start world --> 
<node name="gazebo" pkg="gazebo_ros" type="gzserver"   args="$(find tesi_gazebo)/worlds/playground.world" respawn="false" output="screen" /> 

<!-- start gui --> 
<node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen"/> 

<!-- BEGIN ROBOT 1--> 
<group ns="robot1"> 
<param name="tf_prefix" value="robot1" /> 
<arg name="robot_name" default="robot1" /> 

<include file="$(find tesi_gazebo)/launch/includes/kobuki.launch.xml"> 
<arg name="base" value="$(arg base)"/> 
<arg name="stacks" value="$(arg stacks)"/> 
<arg name="3d_sensor" value="$(arg 3d_sensor)"/> 
<arg name="init_pose" value="-x 1 -y 0 -z 0" /> 
<arg name="robot_name"  value="robot1" /> 
</include> 

<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"> 
<param name="publish_frequency" type="double" value="30.0" /> 
<param name="tf_prefix" type="string" value="robot1" /> 
</node> 


<!-- Fake laser --> 

<node pkg="nodelet" type="nodelet" name="$(arg robot_name)_laserscan_nodelet_manager" args="manager"/> 
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" 
    args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet $(arg robot_name)_laserscan_nodelet_manager"> 
<param name="scan_height" value="10"/> 
<param name="output_frame_id" value="/$(arg robot_name)/camera_depth_frame"/> 
<param name="range_min" value="0.45"/> 
<remap from="image" to="/$(arg robot_name)/camera/depth/image_raw"/> 
<remap from="scan" to="/$(arg robot_name)/scan"/> 
</node> 

</group> 

<!-- BEGIN ROBOT 2--> 
<group ns="robot2"> 
<param name="tf_prefix" value="robot2" /> 
<arg name="robot_name" default="robot2" /> 

<include file="$(find tesi_gazebo)/launch/includes/kobuki.launch.xml"> 
<arg name="base" value="$(arg base)"/> 
<arg name="stacks" value="$(arg stacks)"/> 
<arg name="3d_sensor" value="$(arg 3d_sensor)"/> 
<arg ...