<?xml version="1.0"?>
<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" 
	   xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
	   xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
	   name="quadrotor">   
	   
<!-- Definicion del robot con helices fijas + plugins de Gazebo -->
  
<!-- ****************************BASE******************************* -->
<link name="Base"> 
  <visual>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_base_escala.dae" 
           scale="0.0255 0.0255 0.0255"/> 
    </geometry>       
  </visual>
  <collision>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_base_escala.dae" 
           scale="0.0255 0.0255 0.0255"/> 
    </geometry>
  </collision>
  <inertial>
	 <origin xyz="0 0 5.9e-3" rpy="0 0 0"/>
    <mass value="1.578"/>
    <inertia ixx="0.01632655" ixy="0.063e-7" ixz="0.124e-7" 
             iyy="0.01632655" iyz="0.113e-7" izz="0.02642555"/>
  </inertial>
</link>
  
<!-- ****************************MOTORES**************************** -->
    
<link name="motor_1"> 
  <visual>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_motor_escala.dae" 
           scale="0.0255 0.0255 0.0255"/>     
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </visual>
  <collision>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_motor_escala.dae" 
           scale="0.0255 0.0255 0.0255"/> 
    </geometry>
  </collision>
  <inertial>
	 <origin xyz="0 0 0.020" rpy="0 0 0"/>
    <mass value="0.033"/>
    <inertia ixx="63.23e-7" ixy="0.0" ixz="0.0" 
             iyy="63.23e-7" iyz="0.0" izz="63.23e-7"/>
  </inertial>
</link>

<joint name="base_a_motor_1" type="fixed"> 
  <parent link="Base"/>
  <child link="motor_1"/>
  <origin rpy="0 0 0" xyz="0.2873 0 0.015"/>
</joint>
  
<link name="motor_4"> 
  <visual>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_motor_escala.dae" 
           scale="0.0255 0.0255 0.0255"/>     
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </visual>
  <collision>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_motor_escala.dae" 
           scale="0.0255 0.0255 0.0255"/> 
    </geometry>
  </collision>
  <inertial>
	 <origin xyz="0 0 0.020" rpy="0 0 0"/>
    <mass value="0.033"/>
    <inertia ixx="63.23e-7" ixy="0.0" ixz="0.0" 
             iyy="63.23e-7" iyz="0.0" izz="63.23e-7"/>
  </inertial>
</link>

<joint name="base_a_motor_4" type="fixed"> 
  <parent link="Base"/>
  <child link="motor_4"/>
  <origin rpy="0 0 1.57079" xyz="0 0.2873 0.015"/>
</joint>
   
<link name="motor_3"> 
  <visual>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_motor_escala.dae" 
           scale="0.0255 0.0255 0.0255"/>     
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </visual>
  <collision>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_motor_escala.dae" 
           scale="0.0255 0.0255 0.0255"/> 
    </geometry>
  </collision>
  <inertial>
	 <origin xyz="0 0 0.020" rpy="0 0 0"/>
    <mass value="0.033"/>
    <inertia ixx="63.23e-7" ixy="0.0" ixz="0.0" 
             iyy="63.23e-7" iyz="0.0" izz="63.23e-7"/>
  </inertial>
</link>

<joint name="base_a_motor_3" type="fixed"> 
  <parent link="Base"/>
  <child link="motor_3"/>
  <origin rpy="0 0 3.14159" xyz="-0.2873 0 0.015"/>
</joint>
  
<link name="motor_2"> 
  <visual>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_motor_escala.dae" 
           scale="0.0255 0.0255 0.0255"/>     
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </visual>
  <collision>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_motor_escala.dae" 
           scale="0.0255 0.0255 0.0255"/> 
    </geometry>
  </collision>
  <inertial>
    <origin xyz="0 0 0.020" rpy="0 0 0"/>
    <mass value="0.033"/>
    <inertia ixx="63.23e-7" ixy="0.0" ixz="0.0" 
             iyy="63.23e-7" iyz="0.0" izz="63.23e-7"/>
  </inertial>
</link>

<joint name="base_a_motor_2" type="fixed"> 
  <parent link="Base"/>
  <child link="motor_2"/>
  <origin rpy="0 0 4.71239" xyz="0 -0.2873 0.015"/>
</joint>

<!-- **************************HELICES****************************** -->

<link name="helice_1"> 
  <visual>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_helice_escala_amarillo.dae" 
           scale="0.0255 0.0255 0.0255"/>     
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </visual>
  <collision>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_helice_escala_amarillo.dae" 
           scale="0.0255 0.0255 0.0255"/> 
    </geometry>
  </collision>
  <inertial>
    <origin xyz="0 0 0.022" rpy="0 0 0"/>
    <mass value="0.005"/>
    <inertia ixx="2.6e-7" ixy="4.13e-7" ixz="0.0" 
             iyy="199.012e-7" iyz="0.0" izz="198.388e-7"/>
  </inertial>
</link>

<joint name="motor_a_helice_1" type="fixed">  
  <parent link="motor_1"/>
  <child link="helice_1"/>
  <origin rpy="0 0 0" xyz="0 0 0.04"/>
</joint>
 
<link name="helice_4"> 
  <visual>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_helice_escala.dae" 
           scale="0.0255 0.0255 0.0255"/>     
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </visual>
  <collision>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_helice_escala.dae" 
           scale="0.0255 0.0255 0.0255"/> 
    </geometry>
  </collision>
  <inertial>
	 <origin xyz="0 0 0.022" rpy="0 0 0"/>
    <mass value="0.005"/>
    <inertia ixx="2.6e-7" ixy="4.13e-7" ixz="0.0" 
             iyy="199.012e-7" iyz="0.0" izz="198.388e-7"/>
  </inertial>
</link>

<joint name="motor_a_helice_4" type="fixed"> 
  <parent link="motor_4"/>
  <child link="helice_4"/>
  <origin rpy="0 0 0" xyz="0 0 0.04"/>
</joint>

<link name="helice_3"> 
  <visual>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_helice_escala.dae" 
           scale="0.0255 0.0255 0.0255"/>     
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </visual>
  <collision>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_helice_escala.dae" 
           scale="0.0255 0.0255 0.0255"/> 
    </geometry>
  </collision>
  <inertial>
    <origin xyz="0 0 0.022" rpy="0 0 0"/>
    <mass value="0.005"/>
    <inertia ixx="2.6e-7" ixy="4.13e-7" ixz="0.0" 
             iyy="199.012e-7" iyz="0.0" izz="198.388e-7"/>
  </inertial>
</link>

<joint name="motor_a_helice_3" type="fixed"> 
  <parent link="motor_3"/>
  <child link="helice_3"/>
  <origin rpy="0 0 0" xyz="0 0 0.04"/>
</joint>
  
<link name="helice_2"> 
  <visual>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_helice_escala.dae" 
           scale="0.0255 0.0255 0.0255"/>     
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </visual>
  <collision>
    <geometry>
     <mesh filename="package://modelquad/visual/pelican_helice_escala.dae" 
           scale="0.0255 0.0255 0.0255"/> 
    </geometry>
  </collision>
  <inertial>
    <origin xyz="0 0 0.022" rpy="0 0 0"/>
    <mass value="0.005"/>
    <inertia ixx="2.6e-7" ixy="4.13e-7" ixz="0.0" 
             iyy="199.012e-7" iyz="0.0" izz="198.388e-7"/>
  </inertial>
</link>

<joint name="motor_a_helice_2" type="fixed"> 
  <parent link="motor_2"/>
  <child link="helice_2"/>
  <origin rpy="0 0 0" xyz="0 0 0.04"/>
</joint>
  
<!-- **************************SOPORTES***************************** -->

<link name="Soporte_1"> 
  <visual>
    <geometry>
	 <cylinder length="0.5" radius="0.005"/>
	</geometry> 
	<material name="blue">
     <color rgba="0 0.5 0.5 1"/>
    </material>      
  </visual>
  <collision>
	<geometry>
	 <cylinder length="0.5" radius="0.005"/> 
	</geometry>
  </collision>
  <inertial>
	<origin xyz="0 0 0" rpy="0 0 0"/>
	<mass value="0"/>
	<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
  </inertial>
</link>
  
<joint name="base_a_Soporte_1" type="fixed" >
  <parent link="Base" />
  <child link="Soporte_1" />
  <origin rpy="0 0 0" xyz="0.2873 0 -0.242"/>
</joint> 
  
<gazebo reference="Soporte_1">
  <material>Gazebo/Blue</material>
</gazebo>
 
<link name="Soporte_2"> 
  <visual>
	<geometry>
	 <cylinder length="0.5" radius="0.005"/>
	</geometry> 
	<material name="blue">
     <color rgba="0 0.5 0.5 1"/>
    </material>      
  </visual>
  <collision>
	<geometry>
	 <cylinder length="0.5" radius="0.005"/> 
	</geometry>
  </collision>
  <inertial>
	<origin xyz="0 0 0" rpy="0 0 0"/>
	<mass value="0"/>
	<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
  </inertial>
</link>
  
<joint name="base_a_Soporte_2" type="fixed" >
  <parent link="Base" />
  <child link="Soporte_2" />
  <origin rpy="0 0 0" xyz="0 0.2873 -0.242"/>
</joint> 
  
<gazebo reference="Soporte_2">
  <material>Gazebo/Blue</material>
</gazebo>

<link name="Soporte_3"> 
  <visual>
	<geometry>
	 <cylinder length="0.5" radius="0.005"/>
	</geometry> 
	<material name="blue">
     <color rgba="0 0.5 0.5 1"/>
    </material>      
  </visual>
  <collision>
	<geometry>
	 <cylinder length="0.5" radius="0.005"/> 
	</geometry>
  </collision>
  <inertial>
	<origin xyz="0 0 0" rpy="0 0 0"/>
	<mass value="0"/>
	<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
  </inertial>
</link>
  
<joint name="base_a_Soporte_3" type="fixed" >
  <parent link="Base" />
  <child link="Soporte_3" />
  <origin rpy="0 0 0" xyz="-0.2873 0 -0.242"/>
</joint> 
  
<gazebo reference="Soporte_3">
  <material>Gazebo/Blue</material>
</gazebo>
  
<link name="Soporte_4"> 
  <visual>
	<geometry>
	 <cylinder length="0.5" radius="0.005"/>
	</geometry> 
	<material name="blue">
     <color rgba="0 0.5 0.5 1"/>
    </material>      
  </visual>
  <collision>
	<geometry>
	 <cylinder length="0.5" radius="0.005"/> 
	</geometry>
  </collision>
  <inertial>
	<origin xyz="0 0 0" rpy="0 0 0"/>
	<mass value="0"/>
	<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
  </inertial>
</link>
  
<joint name="base_a_Soporte_4" type="fixed" >
  <parent link="Base" />
  <child link="Soporte_4" />
  <origin rpy="0 0 0" xyz="0 -0.2873 -0.242"/>
</joint> 

<gazebo reference="Soporte_4">
  <material>Gazebo/Blue</material>
</gazebo>   
  
  
    
<!--************************GAZEBO PLUGINS************************** -->  

<!--********SENSOR DE ODOMETRIA************-->

<gazebo>   
  <controller:gazebo_ros_p3d name="sensor_odometria" plugin="libgazebo_ros_p3d.so"> 
	<alwaysOn>true</alwaysOn>  		<!--ON/OFF-->
	<updateRate>10000</updateRate> 	<!--Frecuencia del topic-->
	<bodyName>Base</bodyName>  		<!--Cuerpo sobre el que se monta-->
	<frameName>plane</frameName> 	<!--Frame de referencia-->
	<topicName>Odometria</topicName> <!--Topic de salida-->
	<gaussianNoise>0.0</gaussianNoise> <!--Ruido en las medidas-->    
  </controller:gazebo_ros_p3d>		
</gazebo>  
 
<!--**************CAMARA********************-->		  
<link name="Camara">
 <inertial>
   <mass value="1e-100" />    
   <origin xyz="0 0 0" />
   <inertia ixx="1e-100" ixy="0"  ixz="0"
		    iyy="1e-100" iyz="0"  izz="1e-100" />
 </inertial> 
 <visual>
   <origin xyz="0 0 -0.025" rpy="0 0 0"/>
   <geometry>
	 <cylinder length="0.01" radius="0.01"/>
   </geometry>
 </visual> 
 <collision>
   <origin xyz="0 0 -0.025" rpy="0 0 0"/>	 
   <geometry> 
	 <cylinder length="0.005" radius="0.01"/>
   </geometry>
 </collision> 
</link>

<joint name="Sujec_camara" type="fixed" >
	<parent link="Base" />
	<origin xyz="0 0 -0.0" rpy="0 0 0.785398" />
	<child link="Camara" />
</joint> 

						   
<gazebo reference="Camara">  
	<material>Gazebo/Red</material>
	<!--PLUGIN SENSOR CAMARA-->			
	<sensor:camera name="Camara_sensor">
		<imageSize>640 480</imageSize> <!--Resolucion-->
		<nearClip>0.01</nearClip> <!-- Distancia [m] a la que empieza a mostrar-->
		<farClip>10</farClip> <!-- Distancia a la que deja de mostrar-->
		<controller:gazebo_ros_camera name="Camara_controller" plugin="libgazebo_ros_camera.so">
			<alwaysOn>true</alwaysOn>
			<updateRate>100.0</updateRate>
			<imageTopicName>/Quadrotor/Camara/image_raw</imageTopicName>
			<cameraInfoTopicName>/Quadrotor/Camara/camera_info</cameraInfoTopicName>
			<frameName>Camara</frameName>
		</controller:gazebo_ros_camera>
	</sensor:camera> 
</gazebo>  

</robot>

