<!--
Launch file for running the multirobot simulation on gazebo
-->

<!-- Launches Kobuki Gazebo simulation in an empty world -->
<launch>
<env name="GAZEBO_RESOURCE_PATH" value="$(find rrt_exploration_tutorials)/launch/includes/meshes"/>
<!-- start Gazebo with an empty world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="use_sim_time" value="true"/>
<arg name="debug" value="false"/>
<arg name="world_name" value="$(find rrt_exploration_tutorials)/launch/includes/worlds/largeMap.world"/>
</include>




<!-- spawn robot 1-->
<group ns="/robot_1">
	<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
		<arg name="robot_name" value="robot_1"/>
		<arg name="init_pose" value="-x 0.0 -y 0.0 -z 0.0"/>
	</include>
	<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
		<arg name="namespace" value="robot_1"/>
	</include> 
</group>

<!-- spawn robot 2-->
<group ns="/robot_2">
	<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
		<arg name="robot_name" value="robot_2"/>
		<arg name="init_pose" value="-x 0.0 -y -0.8 -z 0.0"/>
	</include>
	<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
		<arg name="namespace" value="robot_2"/>
	</include> 
</group>

<!-- spawn robot 3-->
<group ns="/robot_3">
	<include file="$(find rrt_exploration_tutorials)/launch/includes/robot.launch.xml">
		<arg name="robot_name" value="robot_3"/>
		<arg name="init_pose" value="-x 0.0 -y 0.8 -z 0.0"/>
	</include>
	<include file="$(find rrt_exploration_tutorials)/launch/includes/move_baseSafe.launch">
		<arg name="namespace" value="robot_3"/>
	</include> 
</group>



<!-- transformation between robots-->
<node pkg="tf" type="static_transform_publisher" name="robot2_to_robot1" args="0 -0.8 0 0 0 0 /robot_1/map /robot_2/map 20" />
<node pkg="tf" type="static_transform_publisher" name="robot3_to_robot1" args="0 0.8 0 0 0 0 /robot_1/map /robot_3/map 20" />

<!-- Map megring (know inital position case)-->
<include file="$(find multi_kobuki_gazebo)/launch/includes/initposes.launch"/>
<include file="$(find multirobot_map_merge)/launch/map_merge.launch"/>



<!-- run RViz node (visualization) -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find multi_kobuki_gazebo)/launch/rviz_config/three.rviz">
<remap from="move_base_simple/goal" to="robot_1/move_base_simple/goal"/>
</node>


</launch>
