Latest revision as of 11:03, 29 September 2014

Artículos realcionados

Otros artículos

Control brazo MYRAbot (bioloid+arduino)
Detección y cálculo de posición de objetos (cámara web)
Modelo para simulación brazo MYRAbot (urdf+gazebo)
Integración de MYRAbot en moveIt! (gazebo+moveIt!)
Órdenes y confirmación mediante voz (sphinx+festival)
Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)

Control brazo y base CeRVaNTeS (maxon+epos2)
Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)
Modelo para simulación CeRVaNTeS (urdf+gazebo)
Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)
Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)

Programas de control para el robot Turtlebot sobre ROS

Componentes del modelo de MYRAbot

Se ha comenzado realizado un modelo de la estructura del MYRAbot a la cual se le ha añadido la base Roomba, la cámara kinect, el modelo del brazo y la cámara web Logitech Webcam Pro 9000. Hemos comenzado creando un package que va a contener el modelo de nuestro robot, que llamaremos "myrabot_fer_modelo", con las dependencias urdf gazebo sensor_msgs roscpp.

URDF estructura MYRAbot

Se ha realizado previamente un modelo 3D de la estructura con un programa CAD, del que vamos a emplear las partes con una geometria singular, ya que el diseño empleando los elementos geométricos básicos sería complicado.

Diseño CAD 3D de la estructura del MYRAbot

Para el archivo principal crearemos un archivo llamado "estructura-myrabot.xacro" en el directorio "urdf" de el package anteriormente creado, con el siguiente contenido:

<?xml version="1.0"?>

<robot>

	<include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot-macros.xacro" />

<macro name="estructura_myrabot" params="parent *origin">	

<joint name="world_joint" type="fixed">
  <parent link="${parent}"/>
  <child link="e_base_1_link"/>
  <insert_block name="origin" />
</joint>
       
  <link name="e_base_1_link">
    <inertial>
      <mass value="0.05"/>
      <origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
      <geometry>
        <cylinder radius="0.135" length="0.015" />
      </geometry>
      <material name="wood">
		<color rgba="${205/255} ${133/255} ${63/255} 1" />
	  </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
      <geometry>
        <cylinder radius="0.135" length="0.015" />
      </geometry>
    </collision>
  </link> 

<separador_base id="1" />
<e_separador_base id="1" xsim="1" ysim="1" />

<separador_base id="2" />
<e_separador_base id="2" xsim="1" ysim="-1" />

<separador_base id="3" />
<e_separador_base id="3" xsim="-1" ysim="-1" />

<separador_base id="4" />
<e_separador_base id="4" xsim="-1" ysim="1" />


<joint name="base_2" type="fixed">
  <parent link="e_base_1_link"/>
  <child link="e_base_2_link"/>
  <origin xyz="0 0 0.135" rpy="0 0 0" />
</joint>

  <link name="e_base_2_link">
    <inertial>
      <mass value="0.05"/>
      <origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
      <geometry>
        <cylinder radius="0.135" length="0.02" />
      </geometry>
      <material name="wood">
		<color rgba="${205/255} ${133/255} ${63/255} 1" />
	  </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
      <geometry>
        <cylinder radius="0.135" length="0.02" />
      </geometry>
    </collision>
  </link>

<joint name="pilar" type="fixed">
  <parent link="e_base_2_link"/>
  <child link="e_pilar_link"/>
  <origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>
  
<link name="e_pilar_link">
    <inertial>
      <mass value="0.08"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.7/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_pilar.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_pilar.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>

<joint name="ultrasonidos" type="fixed">
  <parent link="e_base_2_link"/>
  <child link="e_ultrasonidos_link"/>
  <origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>
  
<link name="e_ultrasonidos_link">
    <inertial>
      <mass value="0.04"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 ${0.04/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="-0.160 -0.135 0"/>
      <geometry>
        <mesh filename="package://myrabot_robot_model/meshes/e_ultrasonidos.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.160 -0.135 0"/>
      <geometry>
        <mesh filename="package://myrabot_robot_model/meshes/e_ultrasonidos.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>

<joint name="botonera" type="fixed">
  <parent link="e_base_2_link"/>
  <child link="e_botonera_link"/>
  <origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>
  
<link name="e_botonera_link">
    <inertial>
      <mass value="0.06"/>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
      <geometry>
        <box size="0.2 0.09 0.1"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
      <geometry>
        <box size="0.2 0.09 0.1"/>
      </geometry>
    </collision>
  </link>
  
<joint name="start_button_1" type="fixed">
  <parent link="e_botonera_link"/>
  <child link="e_start_button_1_link"/>
  <origin xyz="-0.06 0 0.1" rpy="0 0 0" />
</joint>

<link name="e_start_button_1_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
      <geometry>
        <cylinder radius="0.009" length="0.002"/>
      </geometry>
      <material name="black">
		  <color rgba="0 0 0 1"/>
      </material>		  
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
      <geometry>
        <cylinder radius="0.009" length="0.002"/>
      </geometry>
    </collision>
  </link>
  
<joint name="start_button_2" type="fixed">
  <parent link="e_start_button_1_link"/>
  <child link="e_start_button_2_link"/>
  <origin xyz="0.0 0 0.002" rpy="0 0 0" />
</joint>

<link name="e_start_button_2_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
      <geometry>
        <cylinder radius="0.0075" length="0.004"/>
      </geometry>
      <material name="green">
		  <color rgba="0 1 0 1"/>
      </material>		  
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
      <geometry>
        <cylinder radius="0.0075" length="0.004"/>
      </geometry>
    </collision>
  </link>
  
<joint name="emergency_button_1" type="fixed">
  <parent link="e_botonera_link"/>
  <child link="e_emergency_button_1_link"/>
  <origin xyz="0.06 0 0.1" rpy="0 0 0" />
</joint>

<link name="e_emergency_button_1_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
      <geometry>
        <cylinder radius="0.0125" length="0.015"/>
      </geometry>
      <material name="metal" />		  
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
      <geometry>
        <cylinder radius="0.0125" length="0.015"/>
      </geometry>
    </collision>
  </link>
  
<joint name="emergency_button_2" type="fixed">
  <parent link="e_emergency_button_1_link"/>
  <child link="e_emergency_button_2_link"/>
  <origin xyz="0.0 0 0.015" rpy="0 0 0" />
</joint>

<link name="e_emergency_button_2_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
      <geometry>
        <cylinder radius="0.018" length="0.003"/>
      </geometry>
      <material name="red">
		  <color rgba="1 0 0 1"/>
      </material>		  
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
      <geometry>
        <cylinder radius="0.018" length="0.003"/>
      </geometry>
    </collision>
  </link>              

<joint name="base_kinect" type="fixed">
  <parent link="e_pilar_link"/>
  <child link="e_base_kinect_link"/>
  <origin xyz="0 0 ${0.395-0.155}" rpy="0 0 0" />
</joint>
  
<link name="e_base_kinect_link">
    <inertial>
      <mass value="0.02"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_base_kinect.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_base_kinect.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  
<joint name="base_brazo_1" type="fixed">
  <parent link="e_pilar_link"/>
  <child link="e_base_brazo_1_link"/>
  <origin xyz="0 0 ${0.708-(0.395-0.155)}" rpy="0 0 0" />
</joint>
  
<link name="e_base_brazo_1_link">
    <inertial>
      <mass value="0.03"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>

<separador_base_brazo id="1" />
<e_separador_base_brazo id="1" xsim="1" ysim="1" />

<separador_base_brazo id="2" />
<e_separador_base_brazo id="2" xsim="1" ysim="-1" />

<separador_base_brazo id="3" />
<e_separador_base_brazo id="3" xsim="-1" ysim="-1" />

<separador_base_brazo id="4" />
<e_separador_base_brazo id="4" xsim="-1" ysim="1" />

<joint name="base_brazo_2" type="fixed">
  <parent link="e_base_brazo_1_link"/>
  <child link="e_base_brazo_2_link"/>
  <origin xyz="0 0 0.074" rpy="0 0 0" />
</joint>
  
<link name="e_base_brazo_2_link">
    <inertial>
      <mass value="0.005"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>    

<joint name="soporte_1" type="fixed">
  <parent link="e_pilar_link"/>
  <child link="e_soporte_1_link"/>
  <origin xyz="0 0 ${0.995-(0.708-0.155)-(0.395-0.155)}" rpy="0 0 0" />
</joint>
  
<link name="e_soporte_1_link">
    <inertial>
      <mass value="0.05"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.64/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  
<joint name="soporte_2" type="fixed">
  <parent link="e_soporte_1_link"/>
  <child link="e_soporte_2_link"/>
  <origin xyz="0 0.2 0.61" rpy="-0.12 0 0" />
</joint>
  
<link name="e_soporte_2_link">
    <inertial>
      <mass value="0.01"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.06/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 -0.2 -0.03"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.2 -0.03"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>   

<joint name="monitor" type="fixed">
  <parent link="e_soporte_2_link"/>
  <child link="e_monitor_link"/>
  <origin xyz="0 -0.2 -0.09" rpy="0 0 0" />
</joint>
  
<link name="e_monitor_link">
    <inertial>
      <mass value="0.1"/>
      <origin rpy="0 0 0" xyz="0 0 ${0.180/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_monitor.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="wood" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/e_monitor.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  
<joint name="pantalla" type="fixed">
  <parent link="e_monitor_link"/>
  <child link="e_pantalla_link"/>
  <origin xyz="0.135 ${0.135-0.005-0.03-0.0001} 0.086" rpy="0 0 0" />
</joint>
  
<link name="e_pantalla_link">
    <inertial>
      <mass value="0.0001"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.22 0.0001 0.130" />
      </geometry>
      <material name="black" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.22 0.0001 0.127" />
      </geometry>
    </collision>
  </link>       
 
<gazebo_propiedades nombre="e_base_1_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_2_link" material="LightWood" />

<gazebo_propiedades nombre="e_separador_base_1_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_2_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_3_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_4_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_1_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_2_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_3_link" material="Grey" />

<gazebo_propiedades nombre="e_separador_base_brazo_4_link" material="Grey" />

<gazebo_propiedades nombre="e_pilar_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_kinect_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_brazo_1_link" material="LightWood" />

<gazebo_propiedades nombre="e_base_brazo_2_link" material="LightWood" />

<gazebo_propiedades nombre="e_botonera_link" material="LightWood" />

<gazebo_propiedades nombre="e_start_button_1_link" material="Black" />

<gazebo_propiedades nombre="e_start_button_2_link" material="Green" />

<gazebo_propiedades nombre="e_emergency_button_1_link" material="Grey" />

<gazebo_propiedades nombre="e_emergency_button_2_link" material="Red" />

<gazebo_propiedades nombre="e_soporte_1_link" material="LightWood" />

<gazebo_propiedades nombre="e_soporte_2_link" material="LightWood" />

<gazebo_propiedades nombre="e_monitor_link" material="LightWood" />

<gazebo_propiedades nombre="e_pantalla_link" material="Black" />

</macro>
  
</robot>

Los archivos empleados, con los modelos de malla 3D ".stl", en la descrición de la estructura del robot se pueden descargar a continuación, deben guardarse en el directorio "meshes" de nuestro package:

Los macros que estamos usando en el archivo principal del URDF son los que se muestran continuación, que guardaremos en un archivo llamado "estructura-myrabot-macros.xacro" en el directorio "urdf" de nuestro package:

<?xml version="1.0"?>

<robot>

<macro name="default_inertia_e">
	<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
</macro>

<macro name="separador_base" params="id"> 
<joint name="separador_base_${id}" type="fixed">
  <parent link="e_base_1_link"/>
  <child link="e_separador_base_${id}_link"/>
  <origin xyz="0.135 0.135 0.015" rpy="0 0 0" />
</joint>
</macro>

<macro name="e_separador_base" params="id xsim ysim">
  <link name="e_separador_base_${id}_link">
    <inertial>
      <mass value="0.02"/>
      <origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
      <geometry>
        <cylinder radius="0.0075" length="0.120" />
      </geometry>
      <material name="metal">
		<color rgba="${211/255} ${211/255} ${211/255} 1" />
	  </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
      <geometry>
        <cylinder radius="0.0075" length="0.120" />
      </geometry>
    </collision>
  </link> 
</macro> 

<macro name="separador_base_brazo" params="id"> 
<joint name="separador_base_brazo_${id}" type="fixed">
  <parent link="e_base_brazo_1_link"/>
  <child link="e_separador_base_brazo_${id}_link"/>
  <origin xyz="0.135 ${0.135-0.082} 0.075" rpy="0 0 0" />
</joint>
</macro> 

<macro name="e_separador_base_brazo" params="id xsim ysim">
  <link name="e_separador_base_brazo_${id}_link">
    <inertial>
      <mass value="0.002"/>
      <origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
      <default_inertia_e />
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
      <geometry>
        <cylinder radius="0.002" length="0.034" />
      </geometry>
      <material name="metal" />
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
      <geometry>
        <cylinder radius="0.002" length="0.034" />
      </geometry>
    </collision>
  </link> 
</macro>

<macro name="gazebo_propiedades" params="nombre material">
<gazebo reference="${nombre}">
  <material>Gazebo/${material}</material>
  <mu1>0.5</mu1>
  <mu2>0.5</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo> 
</macro>

</robot>

URDF Roomba

Para la base móvil Roomba se ha empleado el modelo del turtlebot, aunque equipado con una base móvil create, esta tiene unas físicas y sensores similares a la nuestra. Para esto necesitamos instalar los stacks turtlebot y turtlebot_simulator, que contienen el modelo y los plugins para gazebo, respectivamente. Eliminando lo que no necesitamos del modelo del turtlebot y modificacndo el elemento de la base, el contenido del archivo que guardaremos con el nombre "roomba.xacro" en el directorio "urdf" de nuestro package, es el siguiente:

<?xml version="1.0"?>

<robot xmlns:xacro="http://ros.org/wiki/xacro">


  <include filename="$(find myrabot_fer_modelo)/urdf/roomba-gazebo.xacro"/>


<xacro:macro name="roomba">

    <material name="Green">
      <color rgba="0.0 0.8 0.0 1.0"/>
    </material>

  <link name="base_footprint">
    <inertial>
      <mass value="0.0001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
              iyy="0.0001" iyz="0.0" 
              izz="0.0001" />
    </inertial>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.001 0.001 0.001" />
      </geometry>
      <material name="Green" />
    </visual>

    <collision>
      <origin xyz="0 0 0.017" rpy="0 0 0" />
      <geometry>
       <box size="0.001 0.001 0.001" />
      </geometry>
    </collision>
  </link>

  <link name="base_link">
    <inertial>
      <mass value="3.6" />
      <origin xyz="0 0 ${0.063/2}" />
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.5" />
    </inertial>

    <visual>
      <origin xyz=" 0 0 ${0.063/2}" rpy="0 0 0" />
      <geometry>
        <cylinder length="0.063" radius="0.17"/>
      </geometry>
      <material name="white">
	    <color rgba="1 1 1 1" />  
      </material>
    </visual>

    <collision>
      <origin xyz="0.0 0.0 ${0.063/2}" rpy="0 0 0" />
      <geometry>
        <cylinder length="0.063" radius="0.17"/>
      </geometry>
    </collision>
  </link>

  <link name="wall_sensor_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>


  <link name="left_cliff_sensor_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>

  <link name="right_cliff_sensor_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>



  <link name="leftfront_cliff_sensor_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="0.01" iyz="0.0" izz="0.01" />
    </inertial>
  </link>

  <link name="rightfront_cliff_sensor_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
  </link>


  <joint name="base_footprint_joint" type="fixed">
    <origin xyz="0 0 0.017" rpy="0 0 0" />        
    <parent link="base_footprint"/>
    <child link="base_link" />
  </joint>

  <joint name="base_wall_sensor_joint" type="fixed">
    <origin xyz="0.09 -0.120 0.042" rpy="0 0 -1.0" />        
    <parent link="base_link"/>
    <child link="wall_sensor_link" />
  </joint>

  <joint name="base_left_cliff_sensor_joint" type="fixed">
    <origin xyz="0.07 0.14 0.01" rpy="0 1.57079 0" />        
    <parent link="base_link"/>
    <child link="left_cliff_sensor_link" />
  </joint>

  <joint name="base_right_cliff_sensor_joint" type="fixed">
    <origin xyz="0.07 -0.14 0.01" rpy="0 1.57079 0" />        
    <parent link="base_link"/>
    <child link="right_cliff_sensor_link" />
  </joint>

  <joint name="base_leftfront_cliff_sensor_joint" type="fixed">
    <origin xyz="0.15 0.04 0.01" rpy="0 1.57079 0" />        
    <parent link="base_link"/>
    <child link="leftfront_cliff_sensor_link" />
  </joint>

  <joint name="base_rightfront_cliff_sensor_joint" type="fixed">
    <origin xyz="0.15 -0.04 0.01" rpy="0 1.57079 0" />        
    <parent link="base_link"/>
    <child link="rightfront_cliff_sensor_link" />
  </joint>

  <link name="left_wheel_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.01" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>

    <visual>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <cylinder radius="0.033" length = "0.023"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <cylinder radius="0.033" length = "0.023"/>
      </geometry>
    </collision>
  </link>

  <joint name="left_wheel_joint" type="continuous">
    <origin xyz="0 0.13 0.015" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="left_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

  <link name="right_wheel_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.01" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>

    <visual>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <cylinder radius="0.033" length = "0.023"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <cylinder radius="0.033" length = "0.023"/>
      </geometry>
    </collision>
  </link>

  <joint name="right_wheel_joint" type="continuous">
    <origin xyz="0 -0.13 0.015" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="right_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

  <link name="rear_wheel_link">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.001" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
        iyy="0.0001" iyz="0.0" izz="0.0001" />
    </inertial>

    <visual>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
      <geometry>
        <sphere radius="0.015" />
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <sphere radius="0.015" />
      </geometry>
    </collision>
  </link>

  <joint name="rear_castor_joint" type="fixed">
    <origin xyz="-0.13 0 0.0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="rear_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>

  <link name="front_wheel_link">
    <inertial>
      <origin xyz="0 0 0" />
      <mass value="0.01" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>

    <visual>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
      <geometry>
        <sphere radius="0.018" />
      </geometry>
    </visual>

    <collision>
      <origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
      <geometry>
        <sphere radius="0.018" />
      </geometry>
    </collision>
  </link>

  <joint name="front_castor_joint" type="fixed">
    <origin xyz="0.13 0 0.0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="front_wheel_link"/>
    <axis xyz="0 1 0"/>
  </joint>


  <joint name="gyro_joint" type="fixed">
    <axis xyz="0 1 0" />
    <origin xyz="0 0 0.04" rpy="0 0 0" />
    <parent link="base_link"/>
    <child link="gyro_link"/>
  </joint>
  <link name="gyro_link">
    <inertial>
      <mass value="0.001" />
      <origin xyz="0 0 0" rpy="0 0 0" />
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001" />
    </inertial>
  </link>

  <joint name="laser_joint" type="fixed">
    <origin xyz="-0.065 0 0.57" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="laser" />
  </joint>

  <link name="laser" />
  
<gaezebo>
  <material>Gazebo/White</material>
</gaezebo>  
  
  <myrabot_sim_imu/>
  <myrabot_sim_laser/>
  <myrabot_sim_roomba/>
  <myrabot_sim_wall_sensors/>  
  
</xacro:macro>
  
</robot>

Para su uso en el simulador gazebo debemos añadir los plugins de los diferentes sensores de la base. Crearemos un archivo llamado "roomba-gazebo.xacro" en el directorio "urdf" de nuestro package con el siguiente contenido:

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
	

<xacro:macro name="myrabot_sim_imu">
  <gazebo>
    <controller:gazebo_ros_imu name="imu_controller" plugin="libgazebo_ros_imu.so">
      <robotNamespace>/</robotNamespace>
      <alwaysOn>true</alwaysOn>
      <updateRate>30</updateRate>
      <bodyName>gyro_link</bodyName>
      <topicName>imu/data</topicName>
      <gaussianNoise>${0.0017*0.0017}</gaussianNoise>
      <xyzOffsets>0 0 0</xyzOffsets> 
      <rpyOffsets>0 0 0</rpyOffsets>
      <interface:position name="imu_position"/>
    </controller:gazebo_ros_imu>
  </gazebo>
</xacro:macro>

<xacro:macro name="myrabot_sim_laser">
  <gazebo reference="laser">
    <sensor:ray name="laser">
      <rayCount>180</rayCount>
      <rangeCount>180</rangeCount>
      <laserCount>1</laserCount>
      
      <origin>0.0 0.0 0.0</origin>
      <displayRays>false</displayRays>
      
      <minAngle>-${60/2}</minAngle>
      <maxAngle>${60/2}</maxAngle>
      
      <minRange>0.08</minRange>
      <maxRange>5.0</maxRange>
      <resRange>0.01</resRange>
      <updateRate>20</updateRate>
      <controller:gazebo_ros_laser name="gazebo_ros_laser_controller" plugin="libgazebo_ros_laser.so">
	<robotNamespace>/</robotNamespace>	  
	<gaussianNoise>0.005</gaussianNoise>
	<alwaysOn>true</alwaysOn>
	<updateRate>20</updateRate>
	<topicName>scan</topicName>
	<frameName>laser</frameName>
	<interface:laser name="gazebo_ros_laser_iface" />
      </controller:gazebo_ros_laser>
    </sensor:ray>
  </gazebo>
</xacro:macro>

<xacro:macro name="myrabot_sim_roomba">
  <gazebo>
    <controller:gazebo_ros_roomba name="roomba_controller" plugin="libgazebo_ros_create.so">
      <alwaysOn>true</alwaysOn>
      <node_namespace>myrabot_node</node_namespace>
      <left_wheel_joint>left_wheel_joint</left_wheel_joint>
      <right_wheel_joint>right_wheel_joint</right_wheel_joint>
      <front_castor_joint>front_castor_joint</front_castor_joint>
      <rear_castor_joint>rear_castor_joint</rear_castor_joint>
      <wheel_separation>.230</wheel_separation>
      <wheel_diameter>0.070</wheel_diameter>
      <base_geom>base_roomba_link</base_geom>
      <updateRate>40</updateRate>
      <torque>1.0</torque>
    </controller:gazebo_ros_roomba>
  </gazebo>
</xacro:macro>

<xacro:macro name="myrabot_sim_wall_sensors">
  <gazebo reference="wall_sensor_link">
    <sensor:ray name="wall_sensor">
      <alwaysActive>true</alwaysActive>
      <rayCount>1</rayCount>
      <rangeCount>1</rangeCount>
      <resRange>0.1</resRange>

      <minAngle>0</minAngle>
      <maxAngle>0</maxAngle>

      <minRange>0.0160</minRange>
      <maxRange>0.04</maxRange>
      <displayRays>false</displayRays>
    </sensor:ray>
  </gazebo>

  <gazebo reference="left_cliff_sensor_link">
    <sensor:ray name="left_cliff_sensor">
      <alwaysActive>true</alwaysActive>
      <rayCount>1</rayCount>
      <rangeCount>1</rangeCount>
      <resRange>0.1</resRange>

      <minAngle>0</minAngle>
      <maxAngle>0</maxAngle>

      <minRange>0.01</minRange>
      <maxRange>0.04</maxRange>
      <displayRays>false</displayRays>
    </sensor:ray>
  </gazebo>

  <gazebo reference="right_cliff_sensor_link">
    <sensor:ray name="right_cliff_sensor">
      <alwaysActive>true</alwaysActive>
      <rayCount>1</rayCount>
      <rangeCount>1</rangeCount>
      <resRange>0.1</resRange>

      <minAngle>0</minAngle>
      <maxAngle>0</maxAngle>

      <minRange>0.01</minRange>
      <maxRange>0.04</maxRange>
      <displayRays>false</displayRays>
    </sensor:ray>
  </gazebo>


  <gazebo reference="leftfront_cliff_sensor_link">
    <sensor:ray name="leftfront_cliff_sensor">
      <alwaysActive>true</alwaysActive>
      <rayCount>1</rayCount>
      <rangeCount>1</rangeCount>
      <resRange>0.1</resRange>

      <minAngle>0</minAngle>
      <maxAngle>0</maxAngle>

      <minRange>0.01</minRange>
      <maxRange>0.04</maxRange>
      <displayRays>false</displayRays>
    </sensor:ray>
  </gazebo>

  <gazebo reference="rightfront_cliff_sensor_link">
    <sensor:ray name="rightfront_cliff_sensor">
      <alwaysActive>true</alwaysActive>
      <rayCount>1</rayCount>
      <rangeCount>1</rangeCount>
      <resRange>0.1</resRange>

      <minAngle>0</minAngle>
      <maxAngle>0</maxAngle>

      <minRange>0.01</minRange>
      <maxRange>0.04</maxRange>
      <displayRays>false</displayRays>
    </sensor:ray>
  </gazebo>



  <gazebo reference="left_wheel_link">
    <mu1 value="10"/>
    <mu2 value="10"/>
    <kp value="100000000.0"/>
    <kd value="10000.0"/>
    <fdir value="1 0 0"/>
  </gazebo>

  <gazebo reference="right_wheel_link">
    <mu1 value="10"/>
    <mu2 value="10"/>
    <kp value="100000000.0"/>
    <kd value="10000.0"/>
    <fdir value="1 0 0"/>
  </gazebo>
  <gazebo reference="rear_wheel_link">
    <mu1 value="0"/>
    <mu2 value="0"/>
    <kp value="100000000.0"/>
    <kd value="10000.0"/>
  </gazebo>

  <gazebo reference="front_wheel_link">
    <mu1 value="0"/>
    <mu2 value="0"/>
    <kp value="100000000.0"/>
    <kd value="10000.0"/>
  </gazebo>
</xacro:macro>
</robot>

URDF kinect

Para la cámara kinect incluiremos el modelo y el plugin para el simulador en un solo archivo. Crearemos un archivo llamado "kinect.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
	
  <property name="M_PI" value="3.14159"/>

<xacro:macro name="kinect" params="parent *origin">
  
  <joint name="base_camera_joint" type="fixed">
	<insert_block name="origin" />
    <parent link="${parent}" />
    <child link="camera_link" />
  </joint>

  <link name="camera_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0"
        izz="0.001" />
    </inertial>

    <visual>
      <origin xyz=" 0 0 0 " rpy="0 0 0" />
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/kinect.dae"/>
      </geometry>
    </visual>

    <collision>
      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://myrabot_fer_modelo/meshes/kinect.dae"/>
      </geometry>
    </collision>

  </link>

  <joint name="camera_depth_joint" type="fixed">
    <origin xyz="0 0.018 0" rpy="0 0 0" />
    <parent link="camera_link" />
    <child link="camera_depth_frame" />
  </joint>

  <link name="camera_depth_frame">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
        iyy="0.001" iyz="0.0"
        izz="0.001" />
    </inertial>
  </link>

  <joint name="camera_depth_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    <parent link="camera_depth_frame" />
    <child link="camera_depth_optical_frame" />
  </joint>

  <link name="camera_depth_optical_frame">
    <inertial>
      <mass value="0.001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
        iyy="0.0001" iyz="0.0"
        izz="0.0001" />
    </inertial>
  </link>

  <joint name="camera_rgb_joint" type="fixed">
    <origin xyz="0 -0.005 0" rpy="0 0 0" />
    <parent link="camera_link" />
    <child link="camera_rgb_frame" />
  </joint>

  <link name="camera_rgb_frame">
    <inertial>
      <mass value="0.001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
        iyy="0.0001" iyz="0.0"
        izz="0.0001" />
    </inertial>
  </link>

  <joint name="camera_rgb_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
    <parent link="camera_rgb_frame" />
    <child link="camera_rgb_optical_frame" />
  </joint>

  <link name="camera_rgb_optical_frame">
    <inertial>
      <mass value="0.001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
        iyy="0.0001" iyz="0.0"
        izz="0.0001" />
    </inertial>
  </link>

  <gazebo reference="camera_link">
    <sensor:camera name="camera">
      <imageFormat>R8G8B8</imageFormat>
      <imageSize>640 480</imageSize>
      <hfov>60</hfov>
      <nearClip>0.05</nearClip>
      <farClip>5</farClip>
      <updateRate>20</updateRate>
      <baseline>0.1</baseline>
      <controller:gazebo_ros_openni_kinect name="kinect_camera_controller" plugin="libgazebo_ros_openni_kinect.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>20</updateRate>
        <robotNamespace>/</robotNamespace> 
        <imageTopicName>/camera/image_raw</imageTopicName>
        <pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
        <cameraInfoTopicName>/camera/camera_info</cameraInfoTopicName>
        <frameName>camera_depth_optical_frame</frameName>
        <distortion_k1>0.0</distortion_k1>
        <distortion_k2>0.0</distortion_k2>
        <distortion_k3>0.0</distortion_k3>
        <distortion_t1>0.0</distortion_t1>
        <distortion_t2>0.0</distortion_t2>
      </controller:gazebo_ros_openni_kinect>
    </sensor:camera>
  </gazebo>
  
</xacro:macro>

</robot>

Los archivos empleados, con el modelo de malla 3D ".stl", en la descrición de la cámara se pueden descargar a continuación, deben guardarse en el directorio "meshes" de nuestro package:

URDF cámara web

Para la cámara web vamos a emplear el plugin de la cámara kinect, pero sin usar la nube de puntos (point cloud). La cámara web empleada es una Logitech Webcam Pro 9000. Crearemos un archivo llamado "webcam.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
	

<xacro:macro name="webcam" params="parent *origin">
  
  <joint name="webcam_1" type="fixed">
	<insert_block name="origin" />
    <parent link="${parent}" />
    <child link="e_webcam_1_link" />
  </joint>

  <link name="e_webcam_1_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
    <visual>
      <origin xyz=" 0 0 0 " rpy="0 0 0" />
      <geometry>
        <mesh filename="package://myrabot_robot_model/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="black">
	    <color rgba="0 0 0 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://myrabot_robot_model/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  
  <joint name="webcam_2" type="fixed">
    <origin xyz="0 ${0.135-0.04-(0.02-0.009)} ${(0.185/2)+0.0045}" rpy="0.82 0 0" />	
    <parent link="e_webcam_1_link" />
    <child link="e_webcam_2_link" />
  </joint>

  <link name="e_webcam_2_link">
    <inertial>
      <mass value="0.01" />
      <origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
    </inertial>
    <visual>
      <origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://myrabot_robot_model/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="black" />
    </visual>
    <collision>
      <origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://myrabot_robot_model/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link> 
  
  <joint name="webcam_optica" type="fixed">
    <origin xyz="0.135 -0.0285 0" rpy="0 0 -1.57" />	
    <parent link="e_webcam_2_link" />
    <child link="e_webcam_optica_link" />
  </joint>

  <link name="e_webcam_optica_link">
    <inertial>
      <mass value="0.001" />
      <origin xyz="0 0 0" rpy="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 1.57 0" />
      <geometry>
        <cylinder radius="0.017" length="0.004" />
      </geometry>
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 1.57 0" />
      <geometry>
        <cylinder radius="0.017" length="0.004" />
      </geometry>
    </collision>
  </link>   

<gazebo reference="e_webcam_1_link">
  <material>myrabot_fer/Black</material>
  <mu1>0.5</mu1>
  <mu2>0.5</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff> 
</gazebo>

<gazebo reference="e_webcam_2_link">
  <material>myrabot_fer/Black</material>
  <mu1>0.5</mu1>
  <mu2>0.5</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff> 
</gazebo>

<gazebo reference="e_webcam_optica_link">
  <material>myrabot_fer/White</material>
  <mu1>0.5</mu1>
  <mu2>0.5</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff> 
</gazebo>

  <gazebo reference="e_webcam_optica_link">
    <sensor:camera name="webcam">
      <imageFormat>R8G8B8</imageFormat>
      <imageSize>640 480</imageSize>
      <hfov>63.09</hfov>
      <nearClip>0.02</nearClip>
      <farClip>3</farClip>
      <updateRate>20</updateRate>
      <baseline>0.1</baseline>
      <controller:gazebo_ros_openni_kinect name="webcam_camera_controller" filename="libgazebo_ros_openni_kinect.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>20</updateRate>
        <robotNamespace>/</robotNamespace>
        <imageTopicName>/webcam/image_raw</imageTopicName>
        <pointCloudTopicName>/webcam/depth/points</pointCloudTopicName>     
        <cameraInfoTopicName>/webcam/camera_info</cameraInfoTopicName>
        <frameName>e_webcam_optica_link</frameName>
        <distortion_k1>0.0</distortion_k1>
        <distortion_k2>0.0</distortion_k2>
        <distortion_k3>0.0</distortion_k3>
        <distortion_t1>0.0</distortion_t1>
        <distortion_t2>0.0</distortion_t2>
      </controller:gazebo_ros_openni_kinect>
    </sensor:camera>
  </gazebo>
  
</xacro:macro>

</robot>

Los archivos empleados, con los modelos de malla 3D ".stl", en la descrición de la cámara web empleada se pueden descargar a continuación, deben guardarse en el directorio "meshes" de nuestro package:

URDF sensor ultrasonidos

Creación de un plugin para gazebo

Primero crearemos un package que va a contener los plugins de nuestro robot, que llamaremos "myrabot_gazebo_plugins", con las dependencias gazebo sensor_msgs roscpp.. Dentro de la carpeta "src" del package creado para los plugins crearemos un archivo llamado "gazebo_ros_sonar.cpp" con el siguiente contenido:

/*
 * Software License Agreement (Modified BSD License)
 *
 *  Copyright (c) 2012, PAL Robotics, S.L.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of PAL Robotics, S.L. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#include <gazebo/Sensor.hh>
#include <gazebo/Global.hh>
#include <gazebo/XMLConfig.hh>
#include <gazebo/Simulator.hh>
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
#include <gazebo/ControllerFactory.hh>
#include <limits>
#include <myrabot_gazebo_plugin/gazebo_ros_sonar.h>

using namespace gazebo;

GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_sonar", GazeboRosSonar)

GazeboRosSonar::GazeboRosSonar(Entity *parent) : Controller(parent)
{
  sensor_ = dynamic_cast<RaySensor*>(parent);
  if (!sensor_) gzthrow("GazeboRosSonar controller requires a RaySensor as its parent");

  Param::Begin(&parameters);
  topic_param_ = new ParamT<std::string>("topicName", "sonar", false);
  frame_id_param_ = new ParamT<std::string>("frameId", "", false);
  radiation_param_ = new ParamT<std::string>("radiation","ultrasound",false);
  fov_param_ = new ParamT<double>("fov", 0.05, false);
  gaussian_noise_ = new ParamT<double>("gaussianNoise", 0.0, 0);
  Param::End();
}

////////////////////////////////////////////////////////////////////////////////
// Destructor
GazeboRosSonar::~GazeboRosSonar()
{
  delete topic_param_;
  delete frame_id_param_;
  delete radiation_param_;
  delete fov_param_;
}

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosSonar::LoadChild(XMLConfigNode *node)
{
  ROS_INFO("INFO: gazebo_ros_ir plugin loading" );
  topic_param_->Load(node);
  frame_id_param_->Load(node);
  radiation_param_->Load(node);
  fov_param_->Load(node);
  gaussian_noise_->Load(node);
}

///////////////////////////////////////////////////////
// Initialize the controller
void GazeboRosSonar::InitChild()
{
  Range.header.frame_id = **frame_id_param_;

  if (**radiation_param_==std::string("ultrasound"))
      Range.radiation_type = sensor_msgs::Range::ULTRASOUND;
  else
      Range.radiation_type = sensor_msgs::Range::INFRARED;

  Range.field_of_view = **fov_param_;
  Range.max_range = sensor_->GetMaxRange();
  Range.min_range = sensor_->GetMinRange();

  sensor_->SetActive(false);
  node_handle_ = new ros::NodeHandle("");
  publisher_ = node_handle_->advertise<sensor_msgs::Range>(**topic_param_, 10);
}

////////////////////////////////////////////////////////////////////////////////
// Utility for adding noise
double GazeboRosSonar::GaussianKernel(double mu,double sigma)
{
    // using Box-Muller transform to generate two independent standard normally disbributed normal variables
    // see wikipedia
    double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
    double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
    double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
    //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
    // we'll just use X
    // scale to our mu and sigma
    X = sigma * X + mu;
    return X;
}

////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosSonar::UpdateChild()
{
  if (!sensor_->IsActive()) sensor_->SetActive(true);

  Range.header.stamp.sec  = (Simulator::Instance()->GetSimTime()).sec;
  Range.header.stamp.nsec = (Simulator::Instance()->GetSimTime()).nsec;
  Range.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();

  for(int i = 0; i < sensor_->GetRangeCount(); ++i) {
    double ray = sensor_->GetRange(i);
    if (ray < Range.range) Range.range = ray;
  }

  Range.range = std::min(Range.range + this->GaussianKernel(0,**gaussian_noise_), sensor_->GetMaxRange());
  publisher_.publish(Range);
}

////////////////////////////////////////////////////////////////////////////////
// Finalize the controller
void GazeboRosSonar::FiniChild()
{
  sensor_->SetActive(false);
  node_handle_->shutdown();
  delete node_handle_;
}

Crearemos un archivo llamado "gazebo_ros_sonar.h" situdo en la ruta "include/myrabot_gazebo_plugins/" dentro del package que hemos creado, con el siguiente contenido:

/*
 * Software License Agreement (Modified BSD License)
 *
 *  Copyright (c) 2012, PAL Robotics, S.L.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of PAL Robotics, S.L. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */


#include <ros/ros.h>

#include <gazebo.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/sensors/RaySensor.hh>

#include <ros/callback_queue.h>
#include <sensor_msgs/Range.h>


namespace gazebo
{

class GazeboRosSonar : public  gazebo::SensorPlugin
{
public:
  GazeboRosSonar();
  virtual ~GazeboRosSonar();

protected:
  virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
  virtual void Update();

  /// \brief Gaussian noise generator
private:
  double GaussianKernel(double mu,double sigma);

private:
  gazebo::sensors::RaySensorPtr sensor_;

  ros::NodeHandle* node_handle_;
  ros::Publisher publisher_;

  sensor_msgs::Range range_;

  std::string namespace_;
  std::string topic_name_;
  std::string frame_id_;
  std::string radiation_;
  double fov_;
  double gaussian_noise_;

  gazebo::physics::WorldPtr parent_;

  common::Time last_time;

  gazebo::event::ConnectionPtr updateConnection;


};

} // namespace gazebo

Añadiremos al archivo "CMakeLists.txt" del package la siguiente línea al final:

rosbuild_add_library(gazebo_ros_sonar src/gazebo_ros_sonar.cpp)

También añadiremos las siguientes líneas al archivo "manifest.xml" del package, antes de las etiqueta </package>:

  <export>
    <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib" />
    <gazebo plugin_path="${prefix}/lib" />
  </export>

Para compilar el nuevo plugin simplemente deberemos ejecutar en un terminal la siguiente secuencia de comandos:

roscd myrabot_gazebo_plugins
make

URDF

En el directorio "urdf" del package del robot vamos a crear un archivo llamado "ultrasonidos.xacro" con el siguiente contenido:

<?xml version="1.0"?>

<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
  
<xacro:macro name="ultrasonidos" params="id parent d_centro *origin">

  <joint name="ultrasonidos_${id}_joint_fisico" type="fixed">
    <parent link="${parent}"/>
    <child link="ultrasonidos_${id}_link_fisico"/>
    <insert_block name="origin" />
  </joint>
       
  <link name="ultrasonidos_${id}_link_fisico">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
	  <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>
    <visual>
      <origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
      <geometry>
        <cylinder radius="${0.015/2}" length="0.015" />
      </geometry>
      <material name="black">
		<color rgba="0 0 0 1" />
	  </material>
    </visual>
    <collision>
      <origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
      <geometry>
        <cylinder radius="${0.015/2}" length="0.015" />
      </geometry>
    </collision>
  </link>
 
<joint name="ultrasonidos_${id}_joint" type="fixed">
    <parent link="ultrasonidos_${id}_link_fisico"/>
    <child link="ultrasonidos_${id}_link"/>
    <origin rpy="0 0 -1.57" xyz="0 -${d_centro+(0.015/2)} ${0.015/2}"/> 
</joint> 
 
  <link name="ultrasonidos_${id}_link">

  </link> 
  
<gazebo reference="ultrasonidos_${id}_link_fisico">
  <material>myrabot_fer/Black</material>
  <mu1>0.5</mu1>
  <mu2>0.5</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff> 
</gazebo>  

    <gazebo reference="ultrasonidos_${id}_link">
      <sensor:ray name="ultrasonidos_${id}">
      
        <displayRays>false</displayRays>

        <rayCount>5</rayCount>
        <rangeCount>5</rangeCount>
        <verticalRayCount>1</verticalRayCount>
        <verticalRangeCount>1</verticalRangeCount>

        <minAngle>-16</minAngle>
        <verticalMinAngle>-16</verticalMinAngle>
        <maxAngle>16</maxAngle>
        <verticalMaxAngle>16</verticalMaxAngle>

        <minRange>0</minRange>
        <maxRange>7</maxRange>
        <resRange>0.01</resRange>

        <updateRate>20</updateRate>

        <controller:gazebo_ros_sonar name="gazebo_ros_ultrasonidos_${id}_controller" plugin="libgazebo_ros_sonar.so">
          <robotNamespace>/</robotNamespace>
          <gaussianNoise>0.005</gaussianNoise>
          <alwaysOn>true</alwaysOn>
          <updateRate>20</updateRate>
          <topicName>ultrasonidos_${id}</topicName>
          <frameId>ultrasonidos_${id}_link</frameId>
          <radiation>ultrasound</radiation>
          <fov>32</fov>
                  
        </controller:gazebo_ros_sonar>
      </sensor:ray>
    </gazebo> 
</xacro:macro>
    
</robot>

Ensamblado del modelo del MYRAbot

Para poder formar el modelo de nuestro robot tenemos que ensamblar las diferentes partes que lo componen, las que hemos descrito en los puntos anteriores y el brazo. Los componentes individuales se han definido como macros con los parámetros de link parent y origen, con lo que deberemos indicar el link y el origen del componente al que se une. Crearemos un archivo llamado "myrabot.urdf.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:

<?xml version="1.0"?>

<robot name="MYRAbot"
       xmlns:xi="http://www.w3.org/2001/XInclude"
       xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
       xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
       xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
       xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
       xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
       xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
       xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
       xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
	
	<include filename="$(find myrabot_fer_modelo)/urdf/roomba.xacro" />		
	
	<include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot.xacro" />
	
	<include filename="$(find myrabot_fer_modelo)/urdf/kinect.xacro" />	
	
	<include filename="$(find brazo_fer_modelo)/urdf/brazo.xacro" />	

	<include filename="$(find myrabot_fer_modelo)/urdf/webcam.xacro" />		
	
	<roomba />
	
	<estructura_myrabot parent="base_link">
		<origin rpy="0 0 1.57" xyz="0.135 -0.135 0.063"/>	
	</estructura_myrabot> 
	
	<kinect parent="e_base_kinect_link">
		<origin rpy="0 0 -1.57" xyz="0.135 0.090 0.112"/>
	</kinect>
	
	<brazo parent="e_base_brazo_1_link">
		<origin rpy="0 0 3.1416" xyz="0.135 0.0335 0.075"/>
	</brazo>
	
	<webcam parent="e_monitor_link">
		<origin rpy="0 0 0" xyz="0 0 ${0.185/2}"/>	
	</webcam>

	<ultrasonidos id="1" parent="e_base_2_link" d_centro="0.150">
		<origin rpy="0 0 1.57" xyz="0.135 0.135 0.02"/>	
	</ultrasonidos>	
	
	<ultrasonidos id="2" parent="e_base_2_link" d_centro="0.150">
		<origin rpy="0 0 2.36" xyz="0.135 0.135 0.02"/>	
	</ultrasonidos>	

	<ultrasonidos id="3" parent="e_base_2_link" d_centro="0.150">
		<origin rpy="0 0 3.14" xyz="0.135 0.135 0.02"/>	
	</ultrasonidos>	
	
	<ultrasonidos id="4" parent="e_base_2_link" d_centro="0.150">
		<origin rpy="0 0 3.93" xyz="0.135 0.135 0.02"/>
	</ultrasonidos>
	
	<ultrasonidos id="5" parent="e_base_2_link" d_centro="0.150">
		<origin rpy="0 0 4.71" xyz="0.135 0.135 0.02"/>
	</ultrasonidos>		

</robot>

Visualización del modelo en rviz

Para la comprobación y visualización del correcto ensamblado del modelo, así como la prueba de las uniones móviles, se ha creado un launcher para cargar el modelo en rviz. Crearemos un archivo llamado "myrabot_rviz.launch" dentro del directorio "launch" de nuestro package con el siguiente contenido:

<launch>

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

	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

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

	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

	<node name="rviz" pkg="rviz" type="rviz" />

</launch>

Para iniciar el launcher debemos ejecutar en un terminal el siguiente comando:

roslaunch myrabot_fer_modelo myrabot_rviz.launch

Si todo va bien, se ejecutará rviz y el interfaz del joint_state_publisher, sobre el cual al modificar los controles deslizables observaremos como se modifica la posición de las uniones móviles en el modelo 3D cargado en rviz. Para que se muestre correctamente nuestro modelo en rviz debemos seleccionar el elemento link que se va ha tomar como elemento fijo "fixed frame", en nuestro caso en "Global Options" el "Fixed Frame" será el link "/base_footprint". El resultado es el que se muestra a continuación:

Modelo MYRAbot en rviz e interfaz joint_state_publisher

Carga del modelo en gazebo

Uso de un único topic para joint states

Para poder tener en el topic "joint_states_myrabot" el estado de las uniones del brazo y de la base, hemos creado un pequeño programa que toma los valores de los topics "brazo/joint_states" y "joint_states", y los publica en este único topic. Crearemos un archivo llamado "remap_joint_states.cpp" dentro del directorio "src" de nuestro package:

  #include "ros/ros.h"
  #include "ros/time.h"  
  #include "sensor_msgs/JointState.h"
  
  sensor_msgs::JointState joint_states_todo;
  int cont_brazo = 0;
  int cont_roomba = 0;
  std::string name[9];
  float position[9];
  float velocity[9];
  float effort[9];

  void brazo(const sensor_msgs::JointState& brazo_states)
  {	
	int tam = brazo_states.name.size();
	
	if (::cont_brazo == 0)
	{	
		for (int i = 0; i < tam; i++)
		{
			::name[i] = brazo_states.name[i];
			::position[i] = brazo_states.position[i];
			::velocity[i] = brazo_states.velocity[i];
			::effort[i] = brazo_states.effort[i];
		}
		
		::cont_brazo = 1;
	}		  	  	  
  }
  
  void roomba(const sensor_msgs::JointState& roomba_states)
  {
	int tam = roomba_states.name.size();
	
	if (::cont_roomba == 0)
	{
		for (int i = 0; i < tam; i++)
		{
			::name[i+5] = roomba_states.name[i];
			::position[i+5] = roomba_states.position[i];
			::velocity[i+5] = roomba_states.velocity[i];
			::effort[i+5] = roomba_states.effort[i];
		}
		
		::cont_roomba = 1;
	}	  	  	  
  }    
 
  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "remap_joint_states");   
 
	ros::NodeHandle n;
  	
  	ros::Subscriber joint_states_brazo_sub_= n.subscribe("brazo/joint_states", 1, brazo);
  	ros::Subscriber joint_states_roomba_sub_= n.subscribe("joint_states", 1, roomba);   	 
  	
  	ros::Publisher joint_states_pub_=n.advertise<sensor_msgs::JointState>("joint_states_myrabot", 1);  	 
  	
        ros::Rate loop_rate(100);
    
	ros::Time ahora;
    
	::joint_states_todo.name = std::vector<std::string>(9);
	::joint_states_todo.position = std::vector<double>(9);
	::joint_states_todo.velocity = std::vector<double>(9);
	::joint_states_todo.effort = std::vector<double>(9);		  	      	

    while (ros::ok())
    {	
		
		if (::cont_brazo == 1 && ::cont_roomba == 1)
		{	
			for (int i = 0; i < 9; i++)
			{
				::joint_states_todo.name[i] = ::name[i];
				::joint_states_todo.position[i] = ::position[i];
				::joint_states_todo.velocity[i] = ::velocity[i];
				::joint_states_todo.effort[i] = ::effort[i];
			}
			
			ahora = ros::Time::now();
			
			::joint_states_todo.header.stamp = ahora; 			
			
			joint_states_pub_.publish(::joint_states_todo);
			
			::cont_brazo = 0;
			::cont_roomba = 0;
		}
		
			ros::spinOnce();
	
			loop_rate.sleep();			
	}   
   	   
 
	ros::spin();  
 
        return 0;
  }

Carga en gazebo

Para poder cargar el modelo en gazebo e iniciar los controladores de todos sus componentes, se ha creado un launcher. Crearemos un archivo llamado "myrabot_gazebo.launch" dentro del directorio "launch" de nuestro package con el siguiente contenido:

<launch>

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

  <node name="gazebo" pkg="gazebo" type="gazebo" args="-u $(find turtlebot_gazebo)/worlds/empty.world" respawn="false" output="screen"/>
  <node name="gazebo_gui" pkg="gazebo" type="gui" />
  
<group ns="brazo">
  
  <param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_robot_model)/urdf/myrabot.urdf.xacro'" />

  <node name="spawn_myrabot" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model MYRAbot" respawn="false" output="screen" /> 

  <rosparam file="$(find myrabot_arm_model)/controller.yaml" command="load"/>

  <node name="spawn_controller" pkg="pr2_controller_manager" type="spawner" args="base_pos_controller arti1_pos_controller arti2_pos_controller arti3_pos_controller pinza_pos_controller base_vel_controller arti1_vel_controller arti2_vel_controller arti3_vel_controller pinza_vel_controller" respawn="false" output="screen" />

</group>
  
  <node name="modelo_brazo" pkg="myrabot_arm_model" type="modelo_brazo" />
  
  <node name="remap_joint_states" pkg="myrabot_robot_model" type="remap_joint_states" />


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

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

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

  <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"/>
    <remap from="cloud" to="/cloud_throttled"/>
    <remap from="scan" to="/narrow_scan"/>
  </node>   
  
</launch>

Para lanzar el launcher ejecutaremos el siguiente comando en un terminal:

roslaunch myrabot_fer_modelo myrabot_gazebo.launch

Si todo va bien podremos ver en la ventana gráfica de gazebo el modelo del MYRAbot situado en un mundo vacío, tal como se muestra en la siguiente imagen:

Modelo MYRAbot en gazebo

Programa de prueba (Vuela al ruedo)

Se ha realizado un pequeño programa para probar el funcionamiento del modelo del MYRAbot, que simplemente hace girar en circulo al robot y gira el brazo de un lado a otro en posición vertical, cuando se acciona el pulsado de inicio del robot. Primero vamos a crear un package que va a contener nuestro programa y que llamaremos "myrabot_fer", con las siguientes dependencias: brazo_fer geometry_msgs std_msgs nav_msgs sensor_msgs image_transport roscpp.

Dentro de este package vamos a crear un archivo llamado "vuelta_al_ruedo.cpp" en el directorio "src" con el siguiente contenido:

  #include "ros/ros.h"
  #include "brazo_fer/Servos.h"
  #include "brazo_fer/WriteServos.h"  
  #include "brazo_fer/ReadServos.h"
  #include "geometry_msgs/Twist.h"
  #include "std_msgs/Bool.h"  

  int cont = 0;
  int cont1= 0;
  int encendido = 0;
  brazo_fer::Servos move, on, vel;
  

  void home()
  {
	if (cont1 == 0)
	{
		ros::NodeHandle n;

		ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
		ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);  		
		ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);   
		
		brazo_fer::WriteServos home;
		
		::move.base = 511;
		::move.arti1 = 511;
		::move.arti2 = 511;
		::move.arti3 = 511;
		::move.pinza = 511;
		
		::on.base = 1;
		::on.arti1 = 1;
		::on.arti2 = 1;
		::on.arti3 = 1;
		::on.pinza = 1;
		
		::vel.base = 400;
		::vel.arti1 = 400;
		::vel.arti2 = 400;
		::vel.arti3 = 400;
		::vel.pinza = 400;
		
		home.posicion = ::move;
		home.par = ::on;
		home.velocidad = ::vel;
		
		move_arm_pub_.publish(home);
		hand_pub_.publish(home);
		
		::cont1 = 1;
		
	}
  }
  
  void inicio(const std_msgs::Bool& inicio)
  {
	if (inicio.data == true && ::cont == 0)
	{
		::encendido = 1;
		::cont = 1;
	}
	else if (inicio.data == true && ::cont == 1)
	{
		::encendido = 0;
		::cont = 0;
	}
  }
  
  void posicion(const brazo_fer::ReadServos& posicion)
  {
	ros::NodeHandle n;

  	ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
  	ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);    	 	
	
	brazo_fer::Servos p = posicion.posicion;
	brazo_fer::WriteServos mover_brazo;
	
	home();
	
	if (::encendido == 1)
	{	
		geometry_msgs::Twist mover_base;
		
		mover_base.linear.x = 0.1;
		mover_base.angular.z = 0.3;
		
		move_base_pub_.publish(mover_base);		
		
		if (::cont1 ==  1)
		{
			::move.base = 718;
			::cont1 = 2;
			mover_brazo.posicion = ::move;
			mover_brazo.par = ::on;
			mover_brazo.velocidad = ::vel;
			move_arm_pub_.publish(mover_brazo);
		}
		else if ((p.base-110) < ::move.base && ::move.base < (p.base+110))		
		{
			if (::cont1 ==  2)
			{
				::move.base = 304;
				::cont1 = 3;
				mover_brazo.posicion = ::move;
				mover_brazo.par = ::on;
				mover_brazo.velocidad = ::vel;
				move_arm_pub_.publish(mover_brazo);				
			}
			else
			{
				::cont1 = 1;
			}
		}
	}
  }

  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "vuelta_al_ruedo");   
 
	ros::NodeHandle n;
 
 
  	ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion); 
  	ros::Subscriber button_sub_= n.subscribe("start_button", 1, inicio); 
 
  	ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);   
  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
  	ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);  	 	   
 
	ros::spin();  
 
        return 0;
  }

Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro package:

rosbuild_add_executable(vuelta_al_ruedo src/vuelta_al_ruedo.cpp)

Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal:

roscd myrabot_fer
make

Para lanzar gazebo con el modelo del robot en un mundo vacío, ejecutaremos el siguiente comando en un terminal:

roslaunch myrabot_fer_modelo myrabot_gazebo.launch

Para ejecutar el programa que hemos creado, en otro terminal ejecutaremos el siguiente comando:

rosrun myrabot_fer vuelta_al_ruedo

El modelo del robot permanecerá detenido hasta que publiquemos el valor "true" en el topic "start_button", que corresponde al botón de inicio del robot. Para simular la pulsación del botón de inicio vamos a ejecutar el siguiente comando en un nuevo terminal:

rostopic pub start_button std_msgs/Bool '{data: true}' --once

Si todo a funcionado bien el robot comenzara a desplazarse en círculo y girar el brazo de un lado a otro en posición vertical, como se muestra en el vídeo:

<videoflash>IY63zJgQYGI</videoflash> Ver vídeo en YouTube

Ejecución del programa (entre la cerveza y la cola, la plataforma) escoja

Para la ejecución del programa escoja en el simulador gazebo usando el modelo del MYRAbot, necesitamos crear previamente los modelos de los objetos a reconocer y una mesa, sobre la cual estarán situados.

Creación de modelos de objetos

Mesa

Primero vamos crear el modelo de la mesa donde vamos a situar los objetos. Se trata de una mesa con tablero de madera y patas metálicas negras de dimensiones 200x75x74 cm (largo x ancho x alto).

Modelo mesa en gazebo

Para contener los objetos vamos a crear un package que llamaremos "objetos_fer_modelos" con las siguientes dependencias: geometry_msgs gazebo urdf roscpp. Para el modelo de la mesa crearemos un archivo llamado "mesa.xacro" dentro del directorio "urdf" del package que acabamos de crear, con el siguiente contenido:

<?xml version="1.0"?>

<robot name="mesa" xmlns:xacro="http://www.ros.org/wiki/xacro">

<link name="world" />

<joint name="mesa" type="floating">
  <parent link="world"/>
  <child link="mesa_link"/>
  <origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="mesa_link">
  <visual>
    <geometry>
      <box size="2 0.75 0.03" /> 
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
      <material name="wood">
		<color rgba="${205/255} ${133/255} ${63/255} 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
    <geometry>
      <box size="2 0.75 0.03" /> 
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
    <mass value="5" />
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>  
  </inertial>
</link>


<xacro:macro name="pata" params="id refx refy">
<joint name="pata_${id}" type="fixed">
  <parent link="mesa_link"/>
  <child link="pata_${id}_link"/>
  <origin xyz="${refx*((2/2)-0.045)} ${refy*((0.75/2)-0.045)} 0" rpy="0 0 0" />
</joint>

<link name="pata_${id}_link">
  <visual>
    <geometry>
      <cylinder radius="0.025" length="0.71" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
    <material name="black">
      <color rgba="0 0 0 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
    <geometry>
      <cylinder radius="0.025" length="0.71" /> 
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
    <mass value="3" />
      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>  
  </inertial>
</link>
</xacro:macro>

<xacro:pata id="1" refx="1" refy="1"/>
<xacro:pata id="2" refx="1" refy="-1"/>
<xacro:pata id="3" refx="-1" refy="-1"/>
<xacro:pata id="4" refx="-1" refy="1"/>

<gazebo reference="mesa_link">
  <material>Gazebo/LightWood</material>
  <mu1>10</mu1>
  <mu2>10</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

<xacro:macro name="gazebo_pata" params="id">
<gazebo reference="pata_${id}_link">
  <material>PR2/Black</material>
  <mu1>0.9</mu1>
  <mu2>0.9</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>
</xacro:macro>

<xacro:gazebo_pata id="1"/>
<xacro:gazebo_pata id="2"/>
<xacro:gazebo_pata id="3"/>
<xacro:gazebo_pata id="4"/>

</robot>

Para el modelo de la mesa hemos empleado los materiales que vienen por defecto en gazebo, pero para las latas vamos a necesitar crear materiales nuevos con las texturas de las etiquetas.

Creación de nuevos materiales para gazebo

Comenzaremos creando dentro de nuestro package un directorio llamado "Media", dentro del cual crearemos otro directorio llamado "materials", y dentro de este crearemos otros dos directorios llamados "scripts" y "textures". También debemos añadir las siguientes líneas al archivo "manifest.xml" de nuestro package, para colocarlo en la ruta de gazebo:

 <export>
     <gazebo gazebo_media_path="${prefix}" />
 </export>

Los materiales se definen para el motor gráfico OGRE. Para las latas vamos a necesitar asociar una imagen al material para que aplique una textura a la geometría. La imagen que usaremos para la textura debe guardarse en la carpeta "textures" creada dentro de nuestro package. Crearemos un archivo llamado "myrabot_fer.material" dentro del directorio "scripts" de nuestro package con el contenido que se muestra a continuación, para la definición de un nuevo material con textura:

material material_fer/NOMBRE_DEL_NUEVO_MATERIAL
{
	technique
	{
		pass
		{
                        lighting off


                        ambient 1 1 1 1
			diffuse 1 1 1 1
			specular 0 0 0 0
			emissive 0 0 0


			alpha_rejection greater 128
			depth_write on

			texture_unit
			{
				texture NUESTRO_ARCHIVO_DE_IMAGEN
				tex_coord_set 0
				colour_op modulate
				rotate 180
				scale 1 1
			}
		}
	}
}

Latas

Lata de cerveza

Para el modelo simplificado de la lata crearemos un archivo llamado "lata_cerveza.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:

<?xml version="1.0"?>

<robot name="lata_cerveza" xmlns:xacro="http://www.ros.org/wiki/xacro">

<link name="world" />

<joint name="lata" type="floating">
  <parent link="world"/>
  <child link="lata_link"/>
  <origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_link">
  <visual>
    <geometry>
      <cylinder radius="0.0325" length="0.1115" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
    <material name="red">
      <color rgba="1 0 0 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
    <geometry>
      <cylinder radius="0.0325" length="0.1115" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
    <mass value="0.01" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>  
  </inertial>
</link>

<joint name="lata_top" type="fixed">
  <parent link="lata_link"/>
  <child link="lata_top_link"/>
  <origin xyz="0 0 0.1185" rpy="0 0 0" />
</joint>

<link name="lata_top_link">
  <visual>
    <geometry>
      <cylinder radius="0.0325" length="0.0015" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
    <material name="grey">
      <color rgba="0.8 0.8 0.8 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
    <geometry>
      <cylinder radius="0.0325" length="0.0015" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
    <mass value="0.001" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>  
  </inertial>
</link>

<joint name="lata_bottom" type="fixed">
  <parent link="lata_link"/>
  <child link="lata_bottom_link"/>
  <origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_bottom_link">
  <visual>
    <geometry>
      <cylinder radius="0.0325" length="0.007" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
    <material name="grey">
      <color rgba="0.8 0.8 0.8 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
    <geometry>
      <cylinder radius="0.0325" length="0.007" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
    <mass value="0.001" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>  
  </inertial>
</link>

<gazebo reference="lata_link">
  <material>material_fer/Amstel</material>
  <mu1>100</mu1>
  <mu2>100</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

<gazebo reference="lata_top_link">
  <material>PR2/Grey0</material>
  <mu1>100</mu1>
  <mu2>100</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

<gazebo reference="lata_bottom_link">
  <material>PR2/Grey0</material>
  <mu1>100</mu1>
  <mu2>100</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

</robot>

Ahora debemos añadir el material "Amstel" a gazebo. Copiaremos las siguientes líneas al final del archivo "myrabot_fer.material":

material material_fer/Amstel
{
	technique
	{
		pass
		{
                        lighting off


                        ambient 1 1 1 1
			diffuse 1 1 1 1
			specular 0 0 0 0
			emissive 0 0 0


			alpha_rejection greater 128
			depth_write on

			texture_unit
			{
				texture amstel.png
				tex_coord_set 0
				colour_op modulate
				rotate 180
				scale 1 1
			}
		}
	}
}

A continuación se muestra la imagen que se ha empleado para la textura y una captura del modelo en gazebo:

Textura lata de cerveza

Lata de cerveza en gazebo

Lata de cola

Para el modelo simplificado de la lata crearemos un archivo llamado "lata_cola.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:

<?xml version="1.0"?>

<robot name="lata_cola" xmlns:xacro="http://www.ros.org/wiki/xacro">

<link name="world" />

<joint name="lata" type="floating">
  <parent link="world"/>
  <child link="lata_link"/>
  <origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_link">
  <visual>
    <geometry>
      <cylinder radius="0.0325" length="0.1115" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
    <material name="red">
      <color rgba="1 0 0 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
    <geometry>
      <cylinder radius="0.0325" length="0.1115" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
    <mass value="0.01" />
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>  
  </inertial>
</link>

<joint name="lata_top" type="fixed">
  <parent link="lata_link"/>
  <child link="lata_top_link"/>
  <origin xyz="0 0 0.1185" rpy="0 0 0" />
</joint>

<link name="lata_top_link">
  <visual>
    <geometry>
      <cylinder radius="0.0325" length="0.0015" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
    <material name="grey">
      <color rgba="0.8 0.8 0.8 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
    <geometry>
      <cylinder radius="0.0325" length="0.0015" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
    <mass value="0.001" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>  
  </inertial>
</link>

<joint name="lata_bottom" type="fixed">
  <parent link="lata_link"/>
  <child link="lata_bottom_link"/>
  <origin xyz="0 0 0" rpy="0 0 0" />
</joint>

<link name="lata_bottom_link">
  <visual>
    <geometry>
      <cylinder radius="0.0325" length="0.007" />    
    </geometry>
    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
    <material name="grey">
      <color rgba="0.8 0.8 0.8 1" />
    </material>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
    <geometry>
      <cylinder radius="0.0325" length="0.007" />
    </geometry>
  </collision>
  <inertial>
    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
    <mass value="0.001" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>  
  </inertial>
</link>

<gazebo reference="lata_link">
  <material>material_fer/CocaCola</material>
  <mu1>10</mu1>
  <mu2>10</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

<gazebo reference="lata_top_link">
  <material>PR2/Grey0</material>
  <mu1>10</mu1>
  <mu2>10</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

<gazebo reference="lata_bottom_link">
  <material>PR2/Grey0</material>
  <mu1>10</mu1>
  <mu2>10</mu2>
  <selfCollide>true</selfCollide>
  <turnGravityOff>false</turnGravityOff>  
</gazebo>

</robot>

Ahora debemos añadir el material "CocaCola" a gazebo. Copiaremos las siguientes líneas al final del archivo "myrabot_fer.material":

material Gazebo/CocaCola
{
	technique
	{
		pass
		{
                        lighting off


                        ambient 1 1 1 1
			diffuse 1 1 1 1
			specular 0 0 0 0
			emissive 0 0 0


			alpha_rejection greater 128
			depth_write on

			texture_unit
			{
				texture coca_cola.png
				tex_coord_set 0
				colour_op modulate
				rotate 180
				scale 1 1
			}
		}
	}
}

A continuación se muestra la imagen que se ha empleado para la textura y una captura del modelo en gazebo:

Textura lata de cola

Lata de cola gazebo

Para poder cargar los modelo en gazebo debemos convertirlos a ".urdf", para extender las macros y calcular campos, de nuestros archivos ".xacro". Para esto simplemente deberemos ejecutar la siguiente secuencia de comandos en un terminal:

roscd objetos_fer_modelos
cd urdf
rosrun xacro xacro.py OBJETO.xacro > OBJETO.urdf

Ejecución

Para la carga de todos los modelos se ha creado un launcher, donde le indicamos a cada modelo sus coordenadas de inserción. Crearemos un archivo llamado "myrabot_latas.launch" dentro del directorio "launch" del package "myrabot_fer", con el sigiente contenido:

<launch>

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

  <node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>
  
  <node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0.55 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />    
  
  <node name="spawn_lata_coca_cola" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cola.urdf -urdf -x 0.38 -y -0.18 -z 0.74 -Y -1.57 -model lata_cocacola" respawn="false" output="screen" />  

  <node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.38 -y 0.17 -z 0.74 -Y -1.57 -model lata_amstel" respawn="false" output="screen" />  

  <include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>

</launch>

Para iniciar el launcher simplemente debemos ejecutar el siguiente comando en un terminal:

roslaunch myrabot_fer myrabot_latas.launch

En el siguiente vídeo se muestra al modelo del MYRAbot ejecutando el programa escoja, en el que el brazo señala el objeto que le indiquemos y pasado un breve período de tiempo regresa a su posición inicial. Para iniciar el programa ejecutaremos el siguiente comando en otro terminal:

roslaunch brazo_fer escoja.launch

Para seleccionar el objeto a señalar publicaremos en el topic "selected_object" su número de identificación, ejecutando en un terminal el siguiente comando:

rostopic pub selected_object std_msgs/Int16 1 --once

Si todo va bien el resultado será similar al que se muestra en el siguiente vídeo:

<videoflash>lp2FUFlsFV4</videoflash> Ver vídeo en YouTube

Ejecución del programa seguir objetos

Programa para mover objetos en gazebo usando el teclado

Para poder desplazar los modelos de los objetos dentro de gazebo hemos escrito un pequeño programa que publica en el topic "gazebo/set_model_state" la posición del modelo del objeto indicado, en este caso "Lata_amstel", usando el teclado. Crearemos un archivo llamado "mover_objetos.cpp" que guardaremos en el directorio "src" del package que hemos creado anteriormente para contener los modelos de los objetos, con el siguiente contenido:

  #include "ros/ros.h"
  #include "gazebo/ModelState.h"
  #include <signal.h>
  #include <termios.h>
  #include <stdio.h> 
 
  #define PI 3.14159265
  #define L1 104
  #define L2 104
  #define Lp 60
  
  #define KEYCODE_Xmas 0x43 
  #define KEYCODE_Xmenos 0x44
  #define KEYCODE_Ymas 0x41
  #define KEYCODE_Ymenos 0x42
  #define KEYCODE_Zmas 0x77
  #define KEYCODE_Zmenos 0x73
  #define KEYCODE_XgiroMas 0x61
  #define KEYCODE_XgiroMenos 0x64
  #define KEYCODE_YgiroMas 0x65
  #define KEYCODE_YgiroMenos 0x71  
  
struct termios cooked, raw; 

int cont = 0;

double retardo = 0.1;

gazebo::ModelState objeto;  
 
void quit(int sig)
{
  tcsetattr(0, TCSANOW, &cooked);
  ros::shutdown();
  exit(0);
}

void callback(const ros::TimerEvent&)
{
 
	ros::NodeHandle n;   	
  	
  	ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);

        std::string modelo_objeto;

        ros::NodeHandle nh("~");

        nh.param("modelo_objeto", modelo_objeto, std::string("Lata_amstel"));

	ros::Time anterior;

	if (::cont == 0)
        {
		anterior = ros::Time::now();
		
		::objeto.model_name = modelo_objeto;
  
		::objeto.reference_frame = "world";
		
		::objeto.pose.position.x = 0;
		
		::objeto.pose.position.y = 0.5;
		
		::objeto.pose.position.z = 0.74;
		
		::objeto.pose.orientation.x = 0;
		
		::objeto.pose.orientation.y = 0;
		
		::objeto.pose.orientation.z = 0;
		
		::objeto.pose.orientation.w = 1;												 
		
		::cont = 1;

		object_pub_.publish(::objeto);		
		
	}
	
  char c;

  // get the console in raw mode                                                              
  tcgetattr(0, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file                         
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(0, TCSANOW, &raw);

  puts(""); 
  puts("#####################################################");
  puts("                   CONTROLES OBJETO");
  puts("#####################################################");
  puts("");  
  puts("Flecha arriba:_______ Subir objeto");
  puts("Flecha abajo:________ Bajar objeto");
  puts("Flecha izquierda:____ Desplazar objeto a la izquierda"); 
  puts("Flecha derecha:______ Desplazar objeto a la derecha");
  puts("Tecla W:_____________ Desplazar objeto hacia delante");
  puts("Tecla S:_____________ Desplazar objeto hacia atrás");
  puts("Tecla A:_____________ Inclinar objeto en eje x mas");
  puts("Tecla D:_____________ Inclinar objeto en eje x menos");
  puts("Tecla Q:_____________ Inclinar objeto en eje y mas");
  puts("Tecla E:_____________ Inclinar objeto en eje y menos");                               

        
    while (ros::ok())
    {
		
			
    // get the next event from the keyboard  
    if(read(0, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }
	
      if (c == KEYCODE_Xmas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.position.x = ::objeto.pose.position.x + 0.005;
			
			anterior = ros::Time::now();
		}		
      }
      if (c == KEYCODE_Xmenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.position.x = ::objeto.pose.position.x - 0.005;

			anterior = ros::Time::now();
		}				
      }
      if (c == KEYCODE_Ymas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.position.z = ::objeto.pose.position.z + 0.005;
			
			anterior = ros::Time::now();
		}					    
      }
      if (c == KEYCODE_Ymenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.position.z = ::objeto.pose.position.z - 0.005;
			
			anterior = ros::Time::now();
		}						
      }
      if (c == KEYCODE_Zmas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.position.y = ::objeto.pose.position.y + 0.005;
			
			anterior = ros::Time::now();
		}					
      }
      if (c == KEYCODE_Zmenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.position.y = ::objeto.pose.position.y - 0.005;
			
			anterior = ros::Time::now();
		}				
      }
      if (c == KEYCODE_XgiroMas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.orientation.x = ::objeto.pose.orientation.x + 0.1;
			
			anterior = ros::Time::now();
		}	
      }
      if (c == KEYCODE_XgiroMenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.orientation.x = ::objeto.pose.orientation.x - 0.1;
			
			anterior = ros::Time::now();
		}	
      }
      if (c == KEYCODE_YgiroMas)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.orientation.y = ::objeto.pose.orientation.y + 0.1;
			
			anterior = ros::Time::now();
		}	
      }
      if (c == KEYCODE_YgiroMenos)
      {
		if (ros::Time::now() > anterior + ros::Duration(::retardo))
		{
			::objeto.pose.orientation.y = ::objeto.pose.orientation.y - 0.1;

			anterior = ros::Time::now();
		}	
      }
      
      object_pub_.publish(::objeto);

	}	
}
 
  int main(int argc, char **argv)
  {
 
	ros::init(argc, argv, "mover_objetos");   
 
	ros::NodeHandle n;
  	
  	ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);    	 	


	signal(SIGINT,quit); 
	
	ros::Timer timer = n.createTimer(ros::Duration(1), callback);
    
   	   
 
	ros::spin();  
 
        return 0;
  }

Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro package:

rosbuild_add_executable(mover_objetos src/mover_objetos.cpp)

Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal:

roscd objetos_fer_modelos
make

Ejecución

Para la carga de todos los modelos se ha creado un launcher, donde le indicamos a cada modelo sus coordenadas de inserción. Crearemos un archivo llamado "myrabot_lata.launch" dentro del directorio "launch" del package "myrabot_fer", con el sigiente contenido:

<launch>

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

  <node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>
  
  <node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0.55 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />     

  <node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.5 -y 0 -z 0.74 -Y -1.57 -model lata_amstel" respawn="false" output="screen" />  

  <include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>

</launch>

Para iniciar el launcher simplemente debemos ejecutar el siguiente comando en un terminal:

roslaunch myrabot_fer myrabot_lata.launch

En el siguiente vídeo se muestra al modelo del MYRAbot ejecutando el programa seguir objetos, en el que el brazo sigue el objeto que le indiquemos. Para iniciar el programa ejecutaremos el siguiente comando en otro terminal:

roslaunch brazo_fer seguir.launch

Para seleccionar el objeto a señalar publicaremos en el topic "selected_object" su número de identificación, ejecutando en un terminal el siguiente comando:

rostopic pub selected_object std_msgs/Int16 1 --once

Para poder mover la lata, ejecutaremos en un nuevo terminal el siguiente comando, donde indicamos en el parámetro "modelo_objeto" el nombre del modelo en gazebo que queremos mover:

rosrun objetos_fer_modelos mover_objetos _modelo_objeto:=lata_amstel

Si todo va bien el resultado será similar al que se muestra en el vídeo siguiente:

<videoflash>kLfgfHo8OnI</videoflash> Ver vídeo en YouTube

Artículos realcionados

Otros artículos

Control brazo MYRAbot (bioloid+arduino)
Detección y cálculo de posición de objetos (cámara web)
Modelo para simulación brazo MYRAbot (urdf+gazebo)
Integración de MYRAbot en moveIt! (gazebo+moveIt!)
Órdenes y confirmación mediante voz (sphinx+festival)
Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)

Control brazo y base CeRVaNTeS (maxon+epos2)
Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)
Modelo para simulación CeRVaNTeS (urdf+gazebo)
Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)
Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)

Programas de control para el robot Turtlebot sobre ROS

< volver a principal

Difference between revisions of "Modelo para simulación MYRAbot (urdf+gazebo)"

Latest revision as of 11:03, 29 September 2014

Contents

Componentes del modelo de MYRAbot

URDF estructura MYRAbot

URDF Roomba

URDF kinect

URDF cámara web

URDF sensor ultrasonidos

Creación de un plugin para gazebo

URDF

Ensamblado del modelo del MYRAbot

Visualización del modelo en rviz

Carga del modelo en gazebo

Uso de un único topic para joint states

Carga en gazebo

Programa de prueba (Vuela al ruedo)

Ejecución del programa (entre la cerveza y la cola, la plataforma) escoja

Creación de modelos de objetos

Mesa

Creación de nuevos materiales para gazebo

Latas

Lata de cerveza

Lata de cola

Ejecución

Ejecución del programa seguir objetos

Programa para mover objetos en gazebo usando el teclado

Ejecución

Navigation menu

Personal tools

Namespaces

Variants

Views

More

Search

Resources

Activities

Tools

@@ Line 1: / Line 1: @@
 [[Mobile manipulation | < volver a principal]]
-==Componentes del modelo del MYRAbot==
-Se ha comenzado realizado un modelo de la estructura del MYRAbot a le que se le ha añadido la base [http://store.irobot.com/family/index.jsp?categoryId=3358508&s=A-ProductAge Roomba], la cámara [http://www.xbox.com/es-ES/Kinect kinect], el [[Modelo para simulación brazo MYRAbot (urdf+gazebo)|brazo modelado]] y la cámara web [http://www.logitech.com/es-es/support/5867?crid=405 Logitech Webcam Pro 9000]. Hemos comenzado [[Fernando-TFM-ROS02#Creando un package|creando un package]] que va a contener el modelo de nuestro robot, que llamaremos "myrabot_fer_modelo", con la dependencia urdf.
+----
+{| style="border: solid 0px white; width: 100%"
+!Artículos realcionados
+----
+!Otros artículos
+----
+|-
+| valign="top" width="50%" |
-===URDF estructura MYRAbot===
+[[Control brazo MYRAbot (bioloid+arduino)]]<br/>
+[[Detección y cálculo de posición de objetos (cámara web)]]<br/>
+[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]]<br/>
+[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]]<br/>
+[[Órdenes y confirmación mediante voz (sphinx+festival)]]<br/>
+[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
+| valign="top" |
+[[Control brazo y base CeRVaNTeS (maxon+epos2)]]<br/>
+[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]]<br/>
+[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]]<br/>
+[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]<br/>
+[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
+----
+[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]
+|}
+----
+=Componentes del modelo de MYRAbot=
+Se ha comenzado realizado un modelo de la estructura del MYRAbot a la cual se le ha añadido la base [http://store.irobot.com/family/index.jsp?categoryId=3358508&s=A-ProductAge Roomba], la cámara [http://www.xbox.com/es-ES/Kinect kinect], el [[Modelo para simulación brazo MYRAbot (urdf+gazebo)|modelo del brazo]] y la cámara web [http://www.logitech.com/es-es/support/5867?crid=405 Logitech Webcam Pro 9000]. Hemos comenzado [[Fernando-TFM-ROS02#Creando un package|creando un package]] que va a contener el modelo de nuestro robot, que llamaremos "myrabot_fer_modelo", con las dependencias urdf gazebo sensor_msgs roscpp.
+==URDF estructura MYRAbot==
 Se ha realizado previamente un modelo 3D de la estructura con un programa [http://es.wikipedia.org/wiki/Dise%C3%B1o_asistido_por_computadora CAD], del que vamos a emplear las partes con una geometria singular, ya que el diseño empleando los elementos geométricos básicos sería complicado.
@@ Line 120: / Line 151: @@
      </collision>
    </link>
+<joint name="ultrasonidos" type="fixed">
+  <parent link="e_base_2_link"/>
+  <child link="e_ultrasonidos_link"/>
+  <origin xyz="0 0 0.02" rpy="0 0 0" />
+</joint>
+<link name="e_ultrasonidos_link">
+    <inertial>
+      <mass value="0.04"/>
+      <origin rpy="0 0 0" xyz="0.0 0.0 ${0.04/2}"/>
+      <default_inertia_e />
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.160 -0.135 0"/>
+      <geometry>
+        <mesh filename="package://myrabot_robot_model/meshes/e_ultrasonidos.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="wood" />
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.160 -0.135 0"/>
+      <geometry>
+        <mesh filename="package://myrabot_robot_model/meshes/e_ultrasonidos.stl" scale="0.001 0.001 0.001"/>
+      </geometry>
+    </collision>
+  </link>
 <joint name="botonera" type="fixed">
    <parent link="e_base_2_link"/>
@@ Line 517: / Line 575: @@
 Los archivos empleados, con los modelos de malla 3D ".stl", en la descrición de la estructura del robot se pueden descargar a continuación, deben guardarse en el directorio "meshes" de nuestro ''package'':
-* [http://www.fernando.casadogarcia.es/files/e_pilar.stl e_ e_pilar.stl]
+* [http://www.fernando.casadogarcia.es/files/e_pilar.stl e_pilar.stl]
 * [http://www.fernando.casadogarcia.es/files/e_base_kinect.stl e_base_kinect.stl]
-* [http://www.fernando.casadogarcia.es/files/e_base_brazo_1.stl e_ e_base_brazo_1.stl]
+* [http://www.fernando.casadogarcia.es/files/e_base_brazo_1.stl e_base_brazo_1.stl]
-* [http://www.fernando.casadogarcia.es/files/e_base_brazo_2.stl e_ e_base_brazo_2.stl]
+* [http://www.fernando.casadogarcia.es/files/e_base_brazo_2.stl e_base_brazo_2.stl]
-* [http://www.fernando.casadogarcia.es/files/e_soporte_1.stl e_ e_soporte_1.stl]
+* [http://www.fernando.casadogarcia.es/files/e_soporte_1.stl e_soporte_1.stl]
-* [http://www.fernando.casadogarcia.es/files/e_soporte_2.stl e_ e_soporte_2.stl]
+* [http://www.fernando.casadogarcia.es/files/e_soporte_2.stl e_soporte_2.stl]
-* [http://www.fernando.casadogarcia.es/files/e_monitor.stl e_ e_monitor.stl]
+* [http://www.fernando.casadogarcia.es/files/e_monitor.stl e_monitor.stl]
+* [http://www.fernando.casadogarcia.es/files/e_ultrasonidos.stl e_ultrasonidos.stl]
 Los macros que estamos usando en el archivo principal del [http://wiki.ros.org/urdf/Tutorials URDF]  son los que se muestran continuación, que guardaremos en un archivo llamado "estructura-myrabot-macros.xacro" en el directorio "urdf" de nuestro ''package'':
@@ Line 615: / Line 674: @@
 </syntaxhighlight>
-===URDF Roomba===
+==URDF Roomba==
 Para la base móvil [http://store.irobot.com/family/index.jsp?categoryId=3358508&s=A-ProductAge Roomba] se ha empleado el modelo del [http://www.turtlebot.com/ turtlebot], aunque equipado con una base móvil [http://store.irobot.com/family/index.jsp?categoryId=2591511&s=A-ProductAge create], esta tiene unas físicas y sensores similares a la nuestra. Para esto necesitamos instalar los ''stacks'' [http://wiki.ros.org/turtlebot turtlebot] y [http://wiki.ros.org/turtlebot_simulator turtlebot_simulator], que contienen el modelo y los ''plugins'' para [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo], respectivamente. Eliminando lo que no necesitamos del modelo del [http://www.turtlebot.com/ turtlebot] y modificacndo el elemento de la base, el contenido del archivo que guardaremos con el nombre "roomba.xacro" en el directorio "urdf" de nuestro ''package'', es el siguiente:
@@ Line 625: / Line 684: @@
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
-  <property name="M_PI" value="3.14159"/>
    <include filename="$(find myrabot_fer_modelo)/urdf/roomba-gazebo.xacro"/>
-  <property name="base_x" value="0.33" />
-  <property name="base_y" value="0.33" />
-  <xacro:macro name="create">
+<xacro:macro name="roomba">
      <material name="Green">
        <color rgba="0.0 0.8 0.0 1.0"/>
@@ Line 908: / Line 965: @@
    <joint name="laser_joint" type="fixed">
-     <origin xyz="-0.065 0 0.075" rpy="0 0 0" />
+     <origin xyz="-0.065 0 0.57" rpy="0 0 0" />
      <parent link="base_link" />
      <child link="laser" />
    </joint>
-   <link name="laser">
+   <link name="laser" />
-    <visual>
-      <geometry>
+<gaezebo>
-        <box size="0.02 0.035 0.002" />
-      </geometry>
-      <material name="Green" />
-    </visual>
-    <inertial>
-      <mass value="0.001" />
-      <origin xyz="0 0 0" />
-      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
-        iyy="0.0001" iyz="0.0"
-        izz="0.0001" />
-    </inertial>
-  </link>
-<gaezebo>
    <material>Gazebo/White</material>
 </gaezebo>
-   <turtlebot_sim_imu/>
+   <myrabot_sim_imu/>
-   <turtlebot_sim_laser/>
+   <myrabot_sim_laser/>
-   <turtlebot_sim_create/>
+   <myrabot_sim_roomba/>
-   <turtlebot_sim_wall_sensors/>
+   <myrabot_sim_wall_sensors/>
 </xacro:macro>
@@ Line 951: / Line 994: @@
 <robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
+<xacro:macro name="myrabot_sim_imu">
-<xacro:macro name="turtlebot_sim_imu">
    <gazebo>
      <controller:gazebo_ros_imu name="imu_controller" plugin="libgazebo_ros_imu.so">
+      <robotNamespace>/</robotNamespace>
        <alwaysOn>true</alwaysOn>
        <updateRate>30</updateRate>
@@ Line 968: / Line 1,012: @@
 </xacro:macro>
-<xacro:macro name="turtlebot_sim_laser">
+<xacro:macro name="myrabot_sim_laser">
    <gazebo reference="laser">
      <sensor:ray name="laser">
@@ Line 978: / Line 1,022: @@
        <displayRays>false</displayRays>
-       <minAngle>-130</minAngle>
+       <minAngle>-${60/2}</minAngle>
-       <maxAngle>130</maxAngle>
+       <maxAngle>${60/2}</maxAngle>
        <minRange>0.08</minRange>
-       <maxRange>10.0</maxRange>
+       <maxRange>5.0</maxRange>
        <resRange>0.01</resRange>
        <updateRate>20</updateRate>
        <controller:gazebo_ros_laser name="gazebo_ros_laser_controller" plugin="libgazebo_ros_laser.so">
+	<robotNamespace>/</robotNamespace>
 	<gaussianNoise>0.005</gaussianNoise>
 	<alwaysOn>true</alwaysOn>
@@ Line 997: / Line 1,042: @@
 </xacro:macro>
-<xacro:macro name="turtlebot_sim_create">
+<xacro:macro name="myrabot_sim_roomba">
    <gazebo>
-     <controller:gazebo_ros_roomba name="roomba_controller" plugin="libgazebo_ros_roomba.so">
+     <controller:gazebo_ros_roomba name="roomba_controller" plugin="libgazebo_ros_create.so">
        <alwaysOn>true</alwaysOn>
-       <node_namespace>turtlebot_node</node_namespace>
+       <node_namespace>myrabot_node</node_namespace>
        <left_wheel_joint>left_wheel_joint</left_wheel_joint>
        <right_wheel_joint>right_wheel_joint</right_wheel_joint>
        <front_castor_joint>front_castor_joint</front_castor_joint>
        <rear_castor_joint>rear_castor_joint</rear_castor_joint>
-       <wheel_separation>.260</wheel_separation>
+       <wheel_separation>.230</wheel_separation>
-       <wheel_diameter>0.066</wheel_diameter>
+       <wheel_diameter>0.070</wheel_diameter>
-       <base_geom>base_link_geom_base_link</base_geom>
+       <base_geom>base_roomba_link</base_geom>
        <updateRate>40</updateRate>
        <torque>1.0</torque>
@@ Line 1,015: / Line 1,060: @@
 </xacro:macro>
-<xacro:macro name="turtlebot_sim_wall_sensors">
+<xacro:macro name="myrabot_sim_wall_sensors">
    <gazebo reference="wall_sensor_link">
      <sensor:ray name="wall_sensor">
@@ Line 1,093: / Line 1,138: @@
        <minRange>0.01</minRange>
        <maxRange>0.04</maxRange>
-       <displayRays>fan</displayRays>
+       <displayRays>false</displayRays>
      </sensor:ray>
    </gazebo>
@@ Line 1,132: / Line 1,177: @@
 </syntaxhighlight>
-====Edición del ''plugin'' del tutlebot====
+==URDF kinect==
-===URDF kinect===
 Para la cámara [http://www.xbox.com/es-ES/Kinect kinect] incluiremos el modelo y el ''plugin'' para el simulador en un solo archivo. Crearemos un archivo llamado "kinect.xacro" dentro del directorio "urdf" de nuestro ''package'' con el siguiente contenido:
@@ Line 1,249: / Line 1,292: @@
        <hfov>60</hfov>
        <nearClip>0.05</nearClip>
-       <farClip>3</farClip>
+       <farClip>5</farClip>
        <updateRate>20</updateRate>
        <baseline>0.1</baseline>
@@ Line 1,255: / Line 1,298: @@
          <alwaysOn>true</alwaysOn>
          <updateRate>20</updateRate>
+        <robotNamespace>/</robotNamespace>
          <imageTopicName>/camera/image_raw</imageTopicName>
          <pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
@@ Line 1,274: / Line 1,318: @@
 </syntaxhighlight>
-===URDF cámara web===
+Los archivos empleados, con el modelo de malla 3D ".stl", en la descrición de la cámara se pueden descargar a continuación, deben guardarse en el directorio "meshes" de nuestro ''package'':
+* [http://www.fernando.casadogarcia.es/files/kinect.dae kinect.dae]
+* [http://www.fernando.casadogarcia.es/images/kinect.jpg kinect.jpg]
+==URDF cámara web==
 Para la cámara web vamos a emplear el ''plugin'' de la cámara [http://www.xbox.com/es-ES/Kinect kinect], pero sin usar la nube de puntos (''point cloud''). La cámara web empleada es una [http://www.logitech.com/es-es/support/5867?crid=405 Logitech Webcam Pro 9000]. Crearemos un archivo llamado "webcam.xacro" dentro del directorio "urdf" de nuestro ''package'' con el siguiente contenido:
@@ Line 1,302: / Line 1,351: @@
        <origin xyz=" 0 0 0 " rpy="0 0 0" />
        <geometry>
-         <mesh filename="package://myrabot_fer_modelo/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
+         <mesh filename="package://myrabot_robot_model/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
        </geometry>
        <material name="black">
@@ Line 1,311: / Line 1,360: @@
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
        <geometry>
-         <mesh filename="package://myrabot_fer_modelo/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
+         <mesh filename="package://myrabot_robot_model/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
        </geometry>
      </collision>
@@ Line 1,331: / Line 1,380: @@
        <origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
        <geometry>
-         <mesh filename="package://myrabot_fer_modelo/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
+         <mesh filename="package://myrabot_robot_model/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
        </geometry>
        <material name="black" />
@@ Line 1,338: / Line 1,387: @@
        <origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
        <geometry>
-         <mesh filename="package://myrabot_fer_modelo/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
+         <mesh filename="package://myrabot_robot_model/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
        </geometry>
      </collision>
@@ Line 1,373: / Line 1,422: @@
 <gazebo reference="e_webcam_1_link">
-   <material>Gazebo/Black</material>
+   <material>myrabot_fer/Black</material>
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
@@ Line 1,381: / Line 1,430: @@
 <gazebo reference="e_webcam_2_link">
-   <material>Gazebo/Black</material>
+   <material>myrabot_fer/Black</material>
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
@@ Line 1,389: / Line 1,438: @@
 <gazebo reference="e_webcam_optica_link">
-   <material>Gazebo/White</material>
+   <material>myrabot_fer/White</material>
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
@@ Line 1,400: / Line 1,449: @@
        <imageFormat>R8G8B8</imageFormat>
        <imageSize>640 480</imageSize>
-       <hfov>60</hfov>
+       <hfov>63.09</hfov>
        <nearClip>0.02</nearClip>
        <farClip>3</farClip>
@@ Line 1,408: / Line 1,457: @@
          <alwaysOn>true</alwaysOn>
          <updateRate>20</updateRate>
-         <imageTopicName>/image_raw</imageTopicName>
+        <robotNamespace>/</robotNamespace>
-         <cameraInfoTopicName>/camera_info</cameraInfoTopicName>
+         <imageTopicName>/webcam/image_raw</imageTopicName>
+        <pointCloudTopicName>/webcam/depth/points</pointCloudTopicName>
+         <cameraInfoTopicName>/webcam/camera_info</cameraInfoTopicName>
          <frameName>e_webcam_optica_link</frameName>
          <distortion_k1>0.0</distortion_k1>
@@ Line 1,418: / Line 1,469: @@
        </controller:gazebo_ros_openni_kinect>
      </sensor:camera>
    </gazebo>
 </xacro:macro>
@@ Line 1,431: / Line 1,482: @@
 * [http://www.fernando.casadogarcia.es/files/webcam_2.stl webcam_2.stl]
-==Ensamblado del modelo del MYRAbot==
+==URDF sensor ultrasonidos==
+===Creación de un plugin para gazebo===
+Primero [[Fernando-TFM-ROS02#Creando un package|crearemos un package]] que va a contener los ''plugins'' de nuestro robot, que llamaremos "myrabot_gazebo_plugins", con las dependencias gazebo sensor_msgs roscpp.. Dentro de la carpeta "src" del ''package'' creado para los ''plugins'' crearemos un archivo llamado "gazebo_ros_sonar.cpp" con el siguiente contenido:
+<syntaxhighlight lang="cpp" enclose="div">
+/*
+ * Software License Agreement (Modified BSD License)
+ *
+ *  Copyright (c) 2012, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PAL Robotics, S.L. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
-Para poder formar el modelo de nuestro robot tenemos que ensamblar las diferentes partes que lo componen, y que hemos modelado previamente. Los componentes individuales se han definido como ''macros'' con los parámetros de ''link parent'' y origen, con lo que deberemos indicar el ''link'' y el origen al que tienen que unirse de los otros componentes. Crearemos un archivo llamado "myrabot.urdf.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:
+#include <gazebo/Sensor.hh>
+#include <gazebo/Global.hh>
+#include <gazebo/XMLConfig.hh>
+#include <gazebo/Simulator.hh>
+#include <gazebo/gazebo.h>
+#include <gazebo/GazeboError.hh>
+#include <gazebo/ControllerFactory.hh>
+#include <limits>
+#include <myrabot_gazebo_plugin/gazebo_ros_sonar.h>
-<syntaxhighlight lang="xml" enclose="div">
+using namespace gazebo;
-<?xml version="1.0"?>
+GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_sonar", GazeboRosSonar)
-<robot name="MYRAbot"
+GazeboRosSonar::GazeboRosSonar(Entity *parent) : Controller(parent)
-       xmlns:xi="http://www.w3.org/2001/XInclude"
+{
-       xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+  sensor_ = dynamic_cast<RaySensor*>(parent);
-       xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+  if (!sensor_) gzthrow("GazeboRosSonar controller requires a RaySensor as its parent");
-       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
-       xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+  Param::Begin(&parameters);
-       xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+  topic_param_ = new ParamT<std::string>("topicName", "sonar", false);
-       xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+  frame_id_param_ = new ParamT<std::string>("frameId", "", false);
-       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+  radiation_param_ = new ParamT<std::string>("radiation","ultrasound",false);
-       xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+  fov_param_ = new ParamT<double>("fov", 0.05, false);
-       xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+  gaussian_noise_ = new ParamT<double>("gaussianNoise", 0.0, 0);
-       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+  Param::End();
-       xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
+}
-	<include filename="$(find myrabot_fer_modelo)/urdf/roomba.xacro" />
+////////////////////////////////////////////////////////////////////////////////
+// Destructor
-	<include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot.xacro" />
+GazeboRosSonar::~GazeboRosSonar()
+{
-	<include filename="$(find myrabot_fer_modelo)/urdf/kinect.xacro" />
+  delete topic_param_;
+  delete frame_id_param_;
-	<include filename="$(find brazo_fer_modelo)/urdf/brazo.xacro" />
+  delete radiation_param_;
+  delete fov_param_;
+}
+////////////////////////////////////////////////////////////////////////////////
+// Load the controller
+void GazeboRosSonar::LoadChild(XMLConfigNode *node)
+{
+  ROS_INFO("INFO: gazebo_ros_ir plugin loading" );
+  topic_param_->Load(node);
+  frame_id_param_->Load(node);
+  radiation_param_->Load(node);
+  fov_param_->Load(node);
+  gaussian_noise_->Load(node);
+}
-	<include filename="$(find myrabot_fer_modelo)/urdf/webcam.xacro" />
+///////////////////////////////////////////////////////
+// Initialize the controller
-	<roomba />
+void GazeboRosSonar::InitChild()
+{
-	<estructura_myrabot parent="base_link">
+  Range.header.frame_id = **frame_id_param_;
-		<origin rpy="0 0 1.57" xyz="0.135 -0.135 0.063"/>
-	</estructura_myrabot>
-	<kinect parent="e_base_kinect_link">
-		<origin rpy="0 0 -1.57" xyz="0.135 0.090 0.112"/>
-	</kinect>
-	<brazo parent="e_base_brazo_1_link">
-		<origin rpy="0 0 3.1416" xyz="0.135 0.0335 0.075"/>
-	</brazo>
-	<webcam parent="e_monitor_link">
-		<origin rpy="0 0 0" xyz="0 0 ${0.185/2}"/>
-	</webcam>
-</robot>
+  if (**radiation_param_==std::string("ultrasound"))
+      Range.radiation_type = sensor_msgs::Range::ULTRASOUND;
+  else
+      Range.radiation_type = sensor_msgs::Range::INFRARED;
-</syntaxhighlight>
+  Range.field_of_view = **fov_param_;
+  Range.max_range = sensor_->GetMaxRange();
+  Range.min_range = sensor_->GetMinRange();
-===Visualización del modelo en rviz===
+  sensor_->SetActive(false);
+  node_handle_ = new ros::NodeHandle("");
+  publisher_ = node_handle_->advertise<sensor_msgs::Range>(**topic_param_, 10);
+}
-Para la comprobación y visualización del correcto ensamblado del modelo, así como la prueba de las uniones móviles, se ha creado un ''launcher'' para cargar el modelo en [http://wiki.ros.org/rviz rviz]. Crearemos un archivo llamado "myrabot_rviz.launch" dentro del directorio "launch" de nuestro ''package'' con el siguiente contenido:
+////////////////////////////////////////////////////////////////////////////////
+// Utility for adding noise
+double GazeboRosSonar::GaussianKernel(double mu,double sigma)
+{
+    // using Box-Muller transform to generate two independent standard normally disbributed normal variables
+    // see wikipedia
+    double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
+    double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
+    double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
+    //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
+    // we'll just use X
+    // scale to our mu and sigma
+    X = sigma * X + mu;
+    return X;
+}
-<syntaxhighlight lang="xml" enclose="div">
+////////////////////////////////////////////////////////////////////////////////
+// Update the controller
+void GazeboRosSonar::UpdateChild()
+{
+  if (!sensor_->IsActive()) sensor_->SetActive(true);
-<launch>
+  Range.header.stamp.sec  = (Simulator::Instance()->GetSimTime()).sec;
+  Range.header.stamp.nsec = (Simulator::Instance()->GetSimTime()).nsec;
+  Range.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();
-	<param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_fer_modelo)/urdf/myrabot.urdf.xacro'" />
+  for(int i = 0; i < sensor_->GetRangeCount(); ++i) {
+    double ray = sensor_->GetRange(i);
+    if (ray < Range.range) Range.range = ray;
+  }
-	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
+  Range.range = std::min(Range.range + this->GaussianKernel(0,**gaussian_noise_), sensor_->GetMaxRange());
+  publisher_.publish(Range);
+}
-	<param name="use_gui" value="true"/>
+////////////////////////////////////////////////////////////////////////////////
+// Finalize the controller
+void GazeboRosSonar::FiniChild()
+{
+  sensor_->SetActive(false);
+  node_handle_->shutdown();
+  delete node_handle_;
+}
-	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+</syntaxhighlight>
-	<node name="rviz" pkg="rviz" type="rviz" />
+Crearemos un archivo llamado "gazebo_ros_sonar.h" situdo en la ruta "include/myrabot_gazebo_plugins/" dentro del ''package'' que hemos creado, con el siguiente contenido:
-</launch>
+<syntaxhighlight lang="cpp" enclose="div">
-</syntaxhighlight>
+/*
+ * Software License Agreement (Modified BSD License)
+ *
+ *  Copyright (c) 2012, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PAL Robotics, S.L. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
-Para iniciar el ''launcher'' debemos ejecutar en un terminal el siguiente comando:
-<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_rviz.launch</syntaxhighlight>
+#include <ros/ros.h>
+#include <gazebo.hh>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/sensors/RaySensor.hh>
+#include <ros/callback_queue.h>
+#include <sensor_msgs/Range.h>
-Si todo va bien, se ejecutará [http://wiki.ros.org/rviz rviz] y el interfaz del [http://wiki.ros.org/joint_state_publisher joint_state_publisher], sobre el cual al modificar los controles deslizables observaremos como se modifica la posición de las uniones móviles en el modelo 3D cargado en [http://wiki.ros.org/rviz rviz]. Para que se muestre correctamente nuestro modelo en [http://wiki.ros.org/rviz rviz] debemos seleccionar el elemento ''link'' que se va ha tomar como elemento fijo "fixed frame", en nuestro caso en "Global Options" el "Fixed Frame" será el ''link'' "/base_footprint". El resultado es el que se muestra a continuación:
-[[file:modelo_MYRAbot_rviz.png|thumb|500px|center|Modelo MYRAbot en [http://wiki.ros.org/rviz rviz] e interfaz [http://wiki.ros.org/joint_state_publisher joint_state_publisher]]]
+namespace gazebo
+{
-==Carga del modelo en gazebo==
+class GazeboRosSonar : public  gazebo::SensorPlugin
+{
+public:
+  GazeboRosSonar();
+  virtual ~GazeboRosSonar();
-Para poder cargar el modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] e iniciar los controladores de todos sus componentes, se ha creado un ''launcher''. Crearemos un archivo llamado "myrabot_gazebo.launch" dentro del directorio "launch" de nuestro ''package'' con el siguiente contenido:
+protected:
+  virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
+  virtual void Update();
-<syntaxhighlight lang="xml" enclose="div">
+  /// \brief Gaussian noise generator
+private:
+  double GaussianKernel(double mu,double sigma);
-<launch>
+private:
+  gazebo::sensors::RaySensorPtr sensor_;
-   <include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
+   ros::NodeHandle* node_handle_;
+  ros::Publisher publisher_;
-   <param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_fer_modelo)/urdf/myrabot.urdf.xacro'" />
+   sensor_msgs::Range range_;
-   <node name="spawn_brazo" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model MYRAbot" respawn="false" output="screen" />
+   std::string namespace_;
+  std::string topic_name_;
+  std::string frame_id_;
+  std::string radiation_;
+  double fov_;
+  double gaussian_noise_;
-   <rosparam file="$(find brazo_fer_modelo)/controller.yaml" command="load"/>
+   gazebo::physics::WorldPtr parent_;
-   <node name="spawn_controller" pkg="pr2_controller_manager" type="spawner" args="base_pos_controller arti1_pos_controller arti2_pos_controller arti3_pos_controller pinza_pos_controller base_vel_controller arti1_vel_controller arti2_vel_controller arti3_vel_controller pinza_vel_controller" respawn="false" output="screen" />
+   common::Time last_time;
-  <node name="modelo_brazo" pkg="brazo_fer_modelo" type="modelo_brazo" />
-   <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
+   gazebo::event::ConnectionPtr updateConnection;
-    <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>
-  <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>
-  <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"/>
-    <param name="min_height" value="-0.15"/>
-    <param name="max_height" value="0.15"/>
-    <remap from="cloud" to="/cloud_throttled"/>
-  </node>
-  <node pkg="nodelet" type="nodelet" name="kinect_laser_narrow" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
+} // namespace gazebo
-    <param name="output_frame_id" value="/camera_depth_frame"/>
-    <param name="min_height" value="-0.025"/>
-    <param name="max_height" value="0.025"/>
-    <remap from="cloud" to="/cloud_throttled"/>
-    <remap from="scan" to="/narrow_scan"/>
-  </node>
-</launch>
 </syntaxhighlight>
-Para lanzar el ''launcher'' ejecutaremos el siguiente comando en un terminal:
+Añadiremos al archivo "CMakeLists.txt" del ''package'' la siguiente línea al final:
+<syntaxhighlight>rosbuild_add_library(gazebo_ros_sonar src/gazebo_ros_sonar.cpp)</syntaxhighlight>
+También añadiremos las siguientes líneas al archivo "manifest.xml" del ''package'', antes de las etiqueta </package>:
-<syntaxhighlight>roslaunch brazo_fer_modelo brazo.gazebo.launch</syntaxhighlight>
+<syntaxhighlight lang="xml">
-Si todo va bien podremos ver en la ventana gráfica de [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] el modelo del MYRAbot  situado en un mundo vacío, tal como se muestra en la siguiente imagen:
+  <export>
+    <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib" />
+    <gazebo plugin_path="${prefix}/lib" />
+  </export>
-[[file:modelo_MYRAbot_gazebo.png|thumb|500px|center|Modelo MYRAbot en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
+</syntaxhighlight>
-===Programa de prueba (Vuela al ruedo)===
+Para compilar el nuevo ''plugin'' simplemente deberemos ejecutar en un terminal la siguiente secuencia de comandos:
-Se ha realizado un pequeño programa para probar el funcionamiento del modelo del MYRAbot, que simplemente hace girar en circulo al robot y gira el brazo de un lado a otro en posición vertical, cuando se acciona el pulsado de inicio del robot. Primero vamos a [[Fernando-TFM-ROS02#Creando un package| crear un package]] que va a contener nuestro programa y que llamaremos "myrabot_fer", con las siguientes dependencias: brazo_fer geometry_msgs std_msgs nav_msgs sensor_msgs image_transport roscpp.
+<syntaxhighlight>
-Dentro de este ''package'' vamos a crear un archivo llamado "vuelta_al_ruedo.cpp" en el directorio "src" con el siguiente contenido:
+roscd myrabot_gazebo_plugins
+make
-<syntaxhighlight lang="cpp" anclose="div">
+</syntaxhighlight>
-  #include "ros/ros.h"
+===URDF===
-  #include "brazo_fer/Servos.h"
-  #include "brazo_fer/WriteServos.h"
-  #include "brazo_fer/ReadServos.h"
-  #include "geometry_msgs/Twist.h"
-  #include "std_msgs/Bool.h"
-  int cont = 0;
+En el directorio "urdf" del ''package'' del robot vamos a crear un archivo llamado "ultrasonidos.xacro" con el siguiente contenido:
-  int cont1= 0;
-  int encendido = 0;
-  brazo_fer::Servos move, on, vel;
-  void home()
+<syntaxhighlight lang="xml" enclose="div">
-  {
-	if (cont1 == 0)
+<?xml version="1.0"?>
-	{
-		ros::NodeHandle n;
+<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
+<xacro:macro name="ultrasonidos" params="id parent d_centro *origin">
-		ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
+  <joint name="ultrasonidos_${id}_joint_fisico" type="fixed">
-		ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
+    <parent link="${parent}"/>
-		ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+    <child link="ultrasonidos_${id}_link_fisico"/>
+    <insert_block name="origin" />
-		brazo_fer::WriteServos home;
+  </joint>
-		::move.base = 511;
+  <link name="ultrasonidos_${id}_link_fisico">
-		::move.arti1 = 511;
+    <inertial>
-		::move.arti2 = 511;
+      <mass value="0.001"/>
-		::move.arti3 = 511;
+      <origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
-		::move.pinza = 511;
+	  <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+    </inertial>
-		::on.base = 1;
+    <visual>
-		::on.arti1 = 1;
+      <origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
-		::on.arti2 = 1;
+      <geometry>
-		::on.arti3 = 1;
+        <cylinder radius="${0.015/2}" length="0.015" />
-		::on.pinza = 1;
+      </geometry>
+      <material name="black">
-		::vel.base = 400;
+		<color rgba="0 0 0 1" />
-		::vel.arti1 = 400;
+	  </material>
-		::vel.arti2 = 400;
+    </visual>
-		::vel.arti3 = 400;
+    <collision>
-		::vel.pinza = 400;
+      <origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
+      <geometry>
-		home.posicion = ::move;
+        <cylinder radius="${0.015/2}" length="0.015" />
-		home.par = ::on;
+      </geometry>
-		home.velocidad = ::vel;
+    </collision>
+  </link>
-		move_arm_pub_.publish(home);
-		hand_pub_.publish(home);
+<joint name="ultrasonidos_${id}_joint" type="fixed">
+    <parent link="ultrasonidos_${id}_link_fisico"/>
-		::cont1 = 1;
+    <child link="ultrasonidos_${id}_link"/>
+    <origin rpy="0 0 -1.57" xyz="0 -${d_centro+(0.015/2)} ${0.015/2}"/>
-	}
+</joint>
-   }
+  <link name="ultrasonidos_${id}_link">
+   </link>
-   void inicio(const std_msgs::Bool& inicio)
+<gazebo reference="ultrasonidos_${id}_link_fisico">
-   {
+   <material>myrabot_fer/Black</material>
-	if (inicio.data == true && ::cont == 0)
+   <mu1>0.5</mu1>
-	{
+  <mu2>0.5</mu2>
-		::encendido = 1;
+  <selfCollide>true</selfCollide>
-		::cont = 1;
+  <turnGravityOff>false</turnGravityOff>
-	}
+</gazebo>
-	else if (inicio.data == true && ::cont == 1)
-	{
+    <gazebo reference="ultrasonidos_${id}_link">
-		::encendido = 0;
+      <sensor:ray name="ultrasonidos_${id}">
-		::cont = 0;
-	}
+        <displayRays>false</displayRays>
-  }
+        <rayCount>5</rayCount>
-  void posicion(const brazo_fer::ReadServos& posicion)
+        <rangeCount>5</rangeCount>
-  {
+        <verticalRayCount>1</verticalRayCount>
-	ros::NodeHandle n;
+        <verticalRangeCount>1</verticalRangeCount>
+        <minAngle>-16</minAngle>
+        <verticalMinAngle>-16</verticalMinAngle>
+        <maxAngle>16</maxAngle>
+        <verticalMaxAngle>16</verticalMaxAngle>
+        <minRange>0</minRange>
+        <maxRange>7</maxRange>
+        <resRange>0.01</resRange>
+        <updateRate>20</updateRate>
-  	ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
+        <controller:gazebo_ros_sonar name="gazebo_ros_ultrasonidos_${id}_controller" plugin="libgazebo_ros_sonar.so">
-  	ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+          <robotNamespace>/</robotNamespace>
+          <gaussianNoise>0.005</gaussianNoise>
-	brazo_fer::Servos p = posicion.posicion;
+          <alwaysOn>true</alwaysOn>
-	brazo_fer::WriteServos mover_brazo;
+          <updateRate>20</updateRate>
+          <topicName>ultrasonidos_${id}</topicName>
-	home();
+          <frameId>ultrasonidos_${id}_link</frameId>
+          <radiation>ultrasound</radiation>
+          <fov>32</fov>
+        </controller:gazebo_ros_sonar>
+      </sensor:ray>
+    </gazebo>
+</xacro:macro>
+</robot>
+</syntaxhighlight>
+=Ensamblado del modelo del MYRAbot=
+Para poder formar el modelo de nuestro robot tenemos que ensamblar las diferentes partes que lo componen, las que hemos descrito en los puntos anteriores y el [[Control brazo MYRAbot (bioloid+arduino)|brazo]]. Los componentes individuales se han definido como ''macros'' con los parámetros de ''link parent'' y origen, con lo que deberemos indicar el ''link'' y el origen del componente al que se une. Crearemos un archivo llamado "myrabot.urdf.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:
+<syntaxhighlight lang="xml" enclose="div">
+<?xml version="1.0"?>
+<robot name="MYRAbot"
+       xmlns:xi="http://www.w3.org/2001/XInclude"
+       xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+       xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+       xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+       xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+       xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+       xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+       xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+       xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
+	<include filename="$(find myrabot_fer_modelo)/urdf/roomba.xacro" />
+	<include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot.xacro" />
-	if (::encendido == 1)
+	<include filename="$(find myrabot_fer_modelo)/urdf/kinect.xacro" />
-	{
-		geometry_msgs::Twist mover_base;
+	<include filename="$(find brazo_fer_modelo)/urdf/brazo.xacro" />
-		mover_base.linear.x = 0.1;
+	<include filename="$(find myrabot_fer_modelo)/urdf/webcam.xacro" />
-		mover_base.angular.z = 0.3;
+	<roomba />
-		move_base_pub_.publish(mover_base);
+	<estructura_myrabot parent="base_link">
-		if (::cont1 ==  1)
+		<origin rpy="0 0 1.57" xyz="0.135 -0.135 0.063"/>
-		{
+	</estructura_myrabot>
-			::move.base = 718;
-			::cont1 = 2;
+	<kinect parent="e_base_kinect_link">
-			mover_brazo.posicion = ::move;
+		<origin rpy="0 0 -1.57" xyz="0.135 0.090 0.112"/>
-			mover_brazo.par = ::on;
+	</kinect>
-			mover_brazo.velocidad = ::vel;
-			move_arm_pub_.publish(mover_brazo);
+	<brazo parent="e_base_brazo_1_link">
-		}
+		<origin rpy="0 0 3.1416" xyz="0.135 0.0335 0.075"/>
-		else if ((p.base-110) < ::move.base && ::move.base < (p.base+110))
+	</brazo>
-		{
-			if (::cont1 ==  2)
+	<webcam parent="e_monitor_link">
-			{
+		<origin rpy="0 0 0" xyz="0 0 ${0.185/2}"/>
-				::move.base = 304;
+	</webcam>
-				::cont1 = 3;
-				mover_brazo.posicion = ::move;
+	<ultrasonidos id="1" parent="e_base_2_link" d_centro="0.150">
-				mover_brazo.par = ::on;
+		<origin rpy="0 0 1.57" xyz="0.135 0.135 0.02"/>
-				mover_brazo.velocidad = ::vel;
+	</ultrasonidos>
-				move_arm_pub_.publish(mover_brazo);
-			}
+	<ultrasonidos id="2" parent="e_base_2_link" d_centro="0.150">
-			else
+		<origin rpy="0 0 2.36" xyz="0.135 0.135 0.02"/>
-			{
+	</ultrasonidos>
-				::cont1 = 1;
-			}
+	<ultrasonidos id="3" parent="e_base_2_link" d_centro="0.150">
-		}
+		<origin rpy="0 0 3.14" xyz="0.135 0.135 0.02"/>
-	}
+	</ultrasonidos>
-  }
+	<ultrasonidos id="4" parent="e_base_2_link" d_centro="0.150">
+		<origin rpy="0 0 3.93" xyz="0.135 0.135 0.02"/>
+	</ultrasonidos>
+	<ultrasonidos id="5" parent="e_base_2_link" d_centro="0.150">
+		<origin rpy="0 0 4.71" xyz="0.135 0.135 0.02"/>
+	</ultrasonidos>
+</robot>
-  int main(int argc, char **argv)
+</syntaxhighlight>
-  {
-	ros::init(argc, argv, "vuelta_al_ruedo");
-	ros::NodeHandle n;
-  	ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);
-  	ros::Subscriber button_sub_= n.subscribe("start_button", 1, inicio);
-  	ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
-  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
-  	ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
-	ros::spin();
-        return 0;
-  }
-</syntaxhighlight>
+==Visualización del modelo en rviz==
-Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro ''package'':
+Para la comprobación y visualización del correcto ensamblado del modelo, así como la prueba de las uniones móviles, se ha creado un ''launcher'' para cargar el modelo en [http://wiki.ros.org/rviz rviz]. Crearemos un archivo llamado "myrabot_rviz.launch" dentro del directorio "launch" de nuestro ''package'' con el siguiente contenido:
-<syntaxhighlight>rosbuild_add_executable(vuelta_al_ruedo src/vuelta_al_ruedo.cpp)</syntaxhighlight>
+<syntaxhighlight lang="xml" enclose="div">
-Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal:
+<launch>
-<syntaxhighlight>
+	<param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_fer_modelo)/urdf/myrabot.urdf.xacro'" />
-roscd myrabot_fer
-make
-</syntaxhighlight>
-Para lanzar [http://wiki.ros.org/simulator_gazebo gazebo] con el modelo del robot en un mundo vacío, ejecutaremos el siguiente comando en un terminal:
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
-<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_gazebo.launch</syntaxhighlight>
+	<param name="use_gui" value="true"/>
-Para ejecutar el programa que hemos creado, en otro terminal ejecutaremos el siguiente comando:
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
-<syntaxhighlight>rosrun myrabot_fer vuelta_al_ruedo</syntaxhighlight>
+	<node name="rviz" pkg="rviz" type="rviz" />
-El modelo del robot permanecerá detenido hasta que publiquemos el valor "true" en el ''topic'' "start_button", que corresponde al botón de inicio del robot. Para simular la pulsación del botón de inicio vamos a ejecutar el siguiente comando en un nuevo terminal:
+</launch>
-<syntaxhighlight>rostopic pub start_button std_msgs/Bool '{data: true}' --once</syntaxhighlight>
+</syntaxhighlight>
-Si todo a funcionado bien el robot comenzara a desplazarse en círculo y girar el brazo de un lado a otro en posición vertical, como se muestra en el vídeo:
+Para iniciar el ''launcher'' debemos ejecutar en un terminal el siguiente comando:
-<center><videoflash>IY63zJgQYGI</videoflash></center>
+<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_rviz.launch</syntaxhighlight>
-<center>[http://www.youtube.com/watch?v=IY63zJgQYGI Ver vídeo en YouTube]</center>
+Si todo va bien, se ejecutará [http://wiki.ros.org/rviz rviz] y el interfaz del [http://wiki.ros.org/joint_state_publisher joint_state_publisher], sobre el cual al modificar los controles deslizables observaremos como se modifica la posición de las uniones móviles en el modelo 3D cargado en [http://wiki.ros.org/rviz rviz]. Para que se muestre correctamente nuestro modelo en [http://wiki.ros.org/rviz rviz] debemos seleccionar el elemento ''link'' que se va ha tomar como elemento fijo "fixed frame", en nuestro caso en "Global Options" el "Fixed Frame" será el ''link'' "/base_footprint". El resultado es el que se muestra a continuación:
-==Ejecución del programa (entre la cerveza y la cola, la plataforma) escoja==
+[[file:modelo_MYRAbot_rviz.png|thumb|500px|center|Modelo MYRAbot en [http://wiki.ros.org/rviz rviz] e interfaz [http://wiki.ros.org/joint_state_publisher joint_state_publisher]]]
-Para la ejecución del programa [[Detección y cálculo de posición de objetos (cámara web)#Señalar objetos (Escoja)|escoja]] en el simulador [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] usando el modelo del MYRAbot, necesitamos crear previamente los modelos de los objetos a reconocer y una mesa, sobre la cual estarán situados.
+=Carga del modelo en gazebo=
-===Creación de modelos de objetos===
+==Uso de un único ''topic'' para joint states==
-====Mesa====
+Para poder tener en el ''topic'' "joint_states_myrabot" el estado de las uniones del brazo y de la base, hemos creado un pequeño programa que toma los valores de los ''topics'' "brazo/joint_states" y "joint_states", y los publica en este único ''topic''. Crearemos un archivo llamado "remap_joint_states.cpp" dentro del directorio "src" de nuestro ''package'':
-Primero vamos crear el modelo de la mesa donde vamos a situar los objetos. Se trata de una mesa con tablero de madera y patas metálicas negras de dimensiones 200x75x74 cm (largo x ancho x alto).
+<syntaxhighlight lang="cpp" enclose="div">
-[[file:modelo_mesa.png|thumb|200px|center|Modelo mesa en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
+  #include "ros/ros.h"
+  #include "ros/time.h"
+  #include "sensor_msgs/JointState.h"
+  sensor_msgs::JointState joint_states_todo;
+  int cont_brazo = 0;
+  int cont_roomba = 0;
+  std::string name[9];
+  float position[9];
+  float velocity[9];
+  float effort[9];
-Para contener los objetos vamos a [[Fernando-TFM-ROS02#Creando un package|crear un ''package'']] que llamaremos "objetos_fer_modelos" con dependencia "urdf". Para el modelo de la mesa crearemos un archivo llamado "mesa.xacro" dentro del directorio "urdf" del ''package'' que acabamos de crear, con el siguiente contenido:
+  void brazo(const sensor_msgs::JointState& brazo_states)
+  {
-<syntaxhighlight lang="xml" enclose="div">
+	int tam = brazo_states.name.size();
-<?xml version="1.0"?>
+	if (::cont_brazo == 0)
+	{
-<robot name="mesa" xmlns:xacro="http://www.ros.org/wiki/xacro">
+		for (int i = 0; i < tam; i++)
+		{
-<link name="world" />
+			::name[i] = brazo_states.name[i];
+			::position[i] = brazo_states.position[i];
+			::velocity[i] = brazo_states.velocity[i];
+			::effort[i] = brazo_states.effort[i];
+		}
+		::cont_brazo = 1;
+	}
+  }
+  void roomba(const sensor_msgs::JointState& roomba_states)
+  {
+	int tam = roomba_states.name.size();
+	if (::cont_roomba == 0)
+	{
+		for (int i = 0; i < tam; i++)
+		{
+			::name[i+5] = roomba_states.name[i];
+			::position[i+5] = roomba_states.position[i];
+			::velocity[i+5] = roomba_states.velocity[i];
+			::effort[i+5] = roomba_states.effort[i];
+		}
+		::cont_roomba = 1;
+	}
+  }
+  int main(int argc, char **argv)
+  {
+	ros::init(argc, argv, "remap_joint_states");
+	ros::NodeHandle n;
+  	ros::Subscriber joint_states_brazo_sub_= n.subscribe("brazo/joint_states", 1, brazo);
+  	ros::Subscriber joint_states_roomba_sub_= n.subscribe("joint_states", 1, roomba);
+  	ros::Publisher joint_states_pub_=n.advertise<sensor_msgs::JointState>("joint_states_myrabot", 1);
+        ros::Rate loop_rate(100);
+	ros::Time ahora;
+	::joint_states_todo.name = std::vector<std::string>(9);
+	::joint_states_todo.position = std::vector<double>(9);
+	::joint_states_todo.velocity = std::vector<double>(9);
+	::joint_states_todo.effort = std::vector<double>(9);
-<joint name="mesa" type="floating">
+    while (ros::ok())
-  <parent link="world"/>
+    {
-  <child link="mesa_link"/>
-  <origin xyz="0 0 0" rpy="0 0 0" />
+		if (::cont_brazo == 1 && ::cont_roomba == 1)
-</joint>
+		{
+			for (int i = 0; i < 9; i++)
+			{
+				::joint_states_todo.name[i] = ::name[i];
+				::joint_states_todo.position[i] = ::position[i];
+				::joint_states_todo.velocity[i] = ::velocity[i];
+				::joint_states_todo.effort[i] = ::effort[i];
+			}
+			ahora = ros::Time::now();
+			::joint_states_todo.header.stamp = ahora;
+			joint_states_pub_.publish(::joint_states_todo);
+			::cont_brazo = 0;
+			::cont_roomba = 0;
+		}
+			ros::spinOnce();
+			loop_rate.sleep();
+	}
+	ros::spin();
+        return 0;
+  }
+</syntaxhighlight>
+==Carga en gazebo==
-<link name="mesa_link">
+Para poder cargar el modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] e iniciar los controladores de todos sus componentes, se ha creado un ''launcher''. Crearemos un archivo llamado "myrabot_gazebo.launch" dentro del directorio "launch" de nuestro ''package'' con el siguiente contenido:
-  <visual>
-    <geometry>
+<syntaxhighlight lang="xml" enclose="div">
-      <box size="2 0.75 0.03" />
-    </geometry>
-    <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
-      <material name="wood">
-		<color rgba="${205/255} ${133/255} ${63/255} 1" />
-    </material>
-  </visual>
-  <collision>
-    <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
-    <geometry>
-      <box size="2 0.75 0.03" />
-    </geometry>
-  </collision>
-  <inertial>
-    <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
-    <mass value="5" />
-      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
-  </inertial>
-</link>
+<launch>
-<xacro:macro name="pata" params="id refx refy">
+  <param name="/use_sim_time" value="true" />
-<joint name="pata_${id}" type="fixed">
-  <parent link="mesa_link"/>
-  <child link="pata_${id}_link"/>
-  <origin xyz="${refx*((2/2)-0.045)} ${refy*((0.75/2)-0.045)} 0" rpy="0 0 0" />
-</joint>
-<link name="pata_${id}_link">
+  <node name="gazebo" pkg="gazebo" type="gazebo" args="-u $(find turtlebot_gazebo)/worlds/empty.world" respawn="false" output="screen"/>
-  <visual>
+  <node name="gazebo_gui" pkg="gazebo" type="gui" />
-    <geometry>
-      <cylinder radius="0.025" length="0.71" />
+<group ns="brazo">
-    </geometry>
-    <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
+   <param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_robot_model)/urdf/myrabot.urdf.xacro'" />
-    <material name="black">
-      <color rgba="0 0 0 1" />
-    </material>
-  </visual>
-  <collision>
-    <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
-    <geometry>
-      <cylinder radius="0.025" length="0.71" />
-    </geometry>
-   </collision>
-   <inertial>
-    <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
-    <mass value="3" />
-      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
-  </inertial>
-</link>
-</xacro:macro>
-<xacro:pata id="1" refx="1" refy="1"/>
+  <node name="spawn_myrabot" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model MYRAbot" respawn="false" output="screen" />
-<xacro:pata id="2" refx="1" refy="-1"/>
-<xacro:pata id="3" refx="-1" refy="-1"/>
-<xacro:pata id="4" refx="-1" refy="1"/>
-<gazebo reference="mesa_link">
+  <rosparam file="$(find myrabot_arm_model)/controller.yaml" command="load"/>
-  <material>Gazebo/LightWood</material>
-  <mu1>10</mu1>
-  <mu2>10</mu2>
-  <selfCollide>true</selfCollide>
-  <turnGravityOff>false</turnGravityOff>
-</gazebo>
-<xacro:macro name="gazebo_pata" params="id">
+  <node name="spawn_controller" pkg="pr2_controller_manager" type="spawner" args="base_pos_controller arti1_pos_controller arti2_pos_controller arti3_pos_controller pinza_pos_controller base_vel_controller arti1_vel_controller arti2_vel_controller arti3_vel_controller pinza_vel_controller" respawn="false" output="screen" />
-<gazebo reference="pata_${id}_link">
-  <material>PR2/Black</material>
-  <mu1>0.9</mu1>
-  <mu2>0.9</mu2>
-  <selfCollide>true</selfCollide>
-  <turnGravityOff>false</turnGravityOff>
-</gazebo>
-</xacro:macro>
-<xacro:gazebo_pata id="1"/>
+</group>
-<xacro:gazebo_pata id="2"/>
-<xacro:gazebo_pata id="3"/>
+  <node name="modelo_brazo" pkg="myrabot_arm_model" type="modelo_brazo" />
-<xacro:gazebo_pata id="4"/>
+  <node name="remap_joint_states" pkg="myrabot_robot_model" type="remap_joint_states" />
-</robot>
-</syntaxhighlight>
+  <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>
-Para poder cargar el modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] debemos convertirlo a ".urdf", para extender las macros y calcular campos, de nuestro archivo ".xacro". Para esto simplemente deberemos ejecutar la siguiente secuencia de comandos en un terminal:
+  <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>
+  <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>
+  <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"/>
+    <remap from="cloud" to="/cloud_throttled"/>
+    <remap from="scan" to="/narrow_scan"/>
+  </node>
+</launch>
-<syntaxhighlight>
-roscd objetos_fer_modelos
-cd urdf
-rosrun xacro xacro.py mesa.xacro > mesa.urdf
 </syntaxhighlight>
-Para el modelo de la mesa hemos empleado los materiales que vienen por defecto en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo], pero para las latas vamos a necesitar crear materiales nuevos con las texturas de las etiquetas.
+Para lanzar el ''launcher'' ejecutaremos el siguiente comando en un terminal:
-====Creación de nuevos materiales en gazebo====
+<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_gazebo.launch</syntaxhighlight>
-Para añadir nuevos materiales simplemente debemos editar el archivo "Gazebo.material". Los materiales se definen para el motor gráfico [http://www.ogre3d.org/ OGRE]. Para las latas vamos a necesitar asociar una imagen al material para que aplique una textura a la geometría. La imagen que usaremos para la textura debe guardarse en la carpeta "textures" dentro del directorio de [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]. La definición de un nuevo material con textura sería la que se muestra a continuación:
+Si todo va bien podremos ver en la ventana gráfica de [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] el modelo del MYRAbot  situado en un mundo vacío, tal como se muestra en la siguiente imagen:
-<syntaxhighlight>
+[[file:modelo_MYRAbot_gazebo.png|thumb|500px|center|Modelo MYRAbot en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
-material Gazebo/NOMBRE_DEL_NUEVO_MATERIAL
+==Programa de prueba (Vuela al ruedo)==
-{
-	technique
-	{
-		pass
-		{
-                        lighting off
+Se ha realizado un pequeño programa para probar el funcionamiento del modelo del MYRAbot, que simplemente hace girar en circulo al robot y gira el brazo de un lado a otro en posición vertical, cuando se acciona el pulsado de inicio del robot. Primero vamos a [[Fernando-TFM-ROS02#Creando un package| crear un package]] que va a contener nuestro programa y que llamaremos "myrabot_fer", con las siguientes dependencias: brazo_fer geometry_msgs std_msgs nav_msgs sensor_msgs image_transport roscpp.
-                        ambient 1 1 1 1
+Dentro de este ''package'' vamos a crear un archivo llamado "vuelta_al_ruedo.cpp" en el directorio "src" con el siguiente contenido:
-			diffuse 1 1 1 1
-			specular 0 0 0 0
-			emissive 0 0 0
+<syntaxhighlight lang="cpp" anclose="div">
-			alpha_rejection greater 128
+  #include "ros/ros.h"
-			depth_write on
+  #include "brazo_fer/Servos.h"
+  #include "brazo_fer/WriteServos.h"
+  #include "brazo_fer/ReadServos.h"
+  #include "geometry_msgs/Twist.h"
+  #include "std_msgs/Bool.h"
-			texture_unit
+  int cont = 0;
-			{
+  int cont1= 0;
-				texture NUESTRO_ARCHIVO_DE_IMAGEN
+  int encendido = 0;
-				tex_coord_set 0
+  brazo_fer::Servos move, on, vel;
-				colour_op modulate
-				rotate 180
-				scale 1 1
-			}
-		}
-	}
-}
-</syntaxhighlight>
+  void home()
+  {
+	if (cont1 == 0)
+	{
+		ros::NodeHandle n;
-====Latas====
+		ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
+		ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
-* Lata de cerveza
+		ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
-Para el modelo simplificado de la lata crearemos un archivo llamado "lata_cerveza.xacro" dentro del directorio "urdf" de nuestro ''package'' con el siguiente contenido:
+		brazo_fer::WriteServos home;
-<syntaxhighlight lang="xml" enclose="div">
+		::move.base = 511;
+		::move.arti1 = 511;
-<?xml version="1.0"?>
+		::move.arti2 = 511;
+		::move.arti3 = 511;
-<robot name="lata_cerveza" xmlns:xacro="http://www.ros.org/wiki/xacro">
+		::move.pinza = 511;
-<link name="world" />
+		::on.base = 1;
+		::on.arti1 = 1;
-<joint name="lata" type="floating">
+		::on.arti2 = 1;
-  <parent link="world"/>
+		::on.arti3 = 1;
-  <child link="lata_link"/>
+		::on.pinza = 1;
-  <origin xyz="0 0 0" rpy="0 0 0" />
-</joint>
+		::vel.base = 400;
+		::vel.arti1 = 400;
-<link name="lata_link">
+		::vel.arti2 = 400;
-  <visual>
+		::vel.arti3 = 400;
-    <geometry>
+		::vel.pinza = 400;
-      <cylinder radius="0.0325" length="0.1115" />
-    </geometry>
+		home.posicion = ::move;
-    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
+		home.par = ::on;
-    <material name="red">
+		home.velocidad = ::vel;
-      <color rgba="1 0 0 1" />
-    </material>
+		move_arm_pub_.publish(home);
-   </visual>
+		hand_pub_.publish(home);
-   <collision>
-    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
+		::cont1 = 1;
-    <geometry>
-      <cylinder radius="0.0325" length="0.1115" />
+	}
-    </geometry>
+   }
-  </collision>
-  <inertial>
+  void inicio(const std_msgs::Bool& inicio)
-    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
+  {
-    <mass value="0.01" />
+	if (inicio.data == true && ::cont == 0)
-      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+	{
-   </inertial>
+		::encendido = 1;
-</link>
+		::cont = 1;
+	}
+	else if (inicio.data == true && ::cont == 1)
+	{
+		::encendido = 0;
+		::cont = 0;
+	}
+  }
+  void posicion(const brazo_fer::ReadServos& posicion)
+   {
+	ros::NodeHandle n;
-<joint name="lata_top" type="fixed">
+  	ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
-  <parent link="lata_link"/>
+  	ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
-  <child link="lata_top_link"/>
-  <origin xyz="0 0 0.1185" rpy="0 0 0" />
+	brazo_fer::Servos p = posicion.posicion;
-</joint>
+	brazo_fer::WriteServos mover_brazo;
-<link name="lata_top_link">
+	home();
-  <visual>
-    <geometry>
+	if (::encendido == 1)
-      <cylinder radius="0.0325" length="0.0015" />
+	{
-    </geometry>
+		geometry_msgs::Twist mover_base;
-    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
-    <material name="grey">
+		mover_base.linear.x = 0.1;
-      <color rgba="0.8 0.8 0.8 1" />
+		mover_base.angular.z = 0.3;
-    </material>
-  </visual>
+		move_base_pub_.publish(mover_base);
-  <collision>
-    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
+		if (::cont1 ==  1)
-    <geometry>
+		{
-      <cylinder radius="0.0325" length="0.0015" />
+			::move.base = 718;
-    </geometry>
+			::cont1 = 2;
-  </collision>
+			mover_brazo.posicion = ::move;
-  <inertial>
+			mover_brazo.par = ::on;
-    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
+			mover_brazo.velocidad = ::vel;
-    <mass value="0.001" />
+			move_arm_pub_.publish(mover_brazo);
-      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+		}
-   </inertial>
+		else if ((p.base-110) < ::move.base && ::move.base < (p.base+110))
-</link>
+		{
+			if (::cont1 ==  2)
+			{
+				::move.base = 304;
+				::cont1 = 3;
+				mover_brazo.posicion = ::move;
+				mover_brazo.par = ::on;
+				mover_brazo.velocidad = ::vel;
+				move_arm_pub_.publish(mover_brazo);
+			}
+			else
+			{
+				::cont1 = 1;
+			}
+		}
+	}
+   }
-<joint name="lata_bottom" type="fixed">
+  int main(int argc, char **argv)
-  <parent link="lata_link"/>
+  {
-  <child link="lata_bottom_link"/>
-  <origin xyz="0 0 0" rpy="0 0 0" />
+	ros::init(argc, argv, "vuelta_al_ruedo");
-</joint>
+	ros::NodeHandle n;
+  	ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);
+  	ros::Subscriber button_sub_= n.subscribe("start_button", 1, inicio);
+  	ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
+  	ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
+  	ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+	ros::spin();
+        return 0;
+  }
+</syntaxhighlight>
+Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro ''package'':
-<link name="lata_bottom_link">
+<syntaxhighlight>rosbuild_add_executable(vuelta_al_ruedo src/vuelta_al_ruedo.cpp)</syntaxhighlight>
-  <visual>
-    <geometry>
-      <cylinder radius="0.0325" length="0.007" />
-    </geometry>
-    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
-    <material name="grey">
-      <color rgba="0.8 0.8 0.8 1" />
-    </material>
-  </visual>
-  <collision>
-    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
-    <geometry>
-      <cylinder radius="0.0325" length="0.007" />
-    </geometry>
-  </collision>
-  <inertial>
-    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
-    <mass value="0.001" />
-      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
-  </inertial>
-</link>
-<gazebo reference="lata_link">
+Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal:
-  <material>Gazebo/Amstel</material>
-  <mu1>10</mu1>
-  <mu2>10</mu2>
-  <selfCollide>true</selfCollide>
-  <turnGravityOff>false</turnGravityOff>
-</gazebo>
-<gazebo reference="lata_top_link">
+<syntaxhighlight>
-  <material>PR2/Grey0</material>
+roscd myrabot_fer
-  <mu1>10</mu1>
+make
-  <mu2>10</mu2>
+</syntaxhighlight>
-  <selfCollide>true</selfCollide>
-  <turnGravityOff>false</turnGravityOff>
-</gazebo>
-<gazebo reference="lata_bottom_link">
+Para lanzar [http://wiki.ros.org/simulator_gazebo gazebo] con el modelo del robot en un mundo vacío, ejecutaremos el siguiente comando en un terminal:
-  <material>PR2/Grey0</material>
-  <mu1>10</mu1>
-  <mu2>10</mu2>
-  <selfCollide>true</selfCollide>
-  <turnGravityOff>false</turnGravityOff>
-</gazebo>
-</robot>
+<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_gazebo.launch</syntaxhighlight>
-</syntaxhighlight>
+Para ejecutar el programa que hemos creado, en otro terminal ejecutaremos el siguiente comando:
-Ahora debemos añadir el material "Amstel" a [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]. Copiaremos las siguientes líneas al final del archivo "Gazebo.material":
+<syntaxhighlight>rosrun myrabot_fer vuelta_al_ruedo</syntaxhighlight>
-<syntaxhighlight>
+El modelo del robot permanecerá detenido hasta que publiquemos el valor "true" en el ''topic'' "start_button", que corresponde al botón de inicio del robot. Para simular la pulsación del botón de inicio vamos a ejecutar el siguiente comando en un nuevo terminal:
+<syntaxhighlight>rostopic pub start_button std_msgs/Bool '{data: true}' --once</syntaxhighlight>
-material Gazebo/Amstel
+Si todo a funcionado bien el robot comenzara a desplazarse en círculo y girar el brazo de un lado a otro en posición vertical, como se muestra en el vídeo:
-{
-	technique
-	{
-		pass
-		{
-                        lighting off
+<center><videoflash>IY63zJgQYGI</videoflash></center>
-                        ambient 1 1 1 1
+<center>[http://www.youtube.com/watch?v=IY63zJgQYGI Ver vídeo en YouTube]</center>
-			diffuse 1 1 1 1
-			specular 0 0 0 0
-			emissive 0 0 0
+=Ejecución del programa (entre la cerveza y la cola, la plataforma) escoja=
-			alpha_rejection greater 128
+Para la ejecución del programa [[Detección y cálculo de posición de objetos (cámara web)#Señalar objetos (Escoja)|escoja]] en el simulador [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] usando el modelo del MYRAbot, necesitamos crear previamente los modelos de los objetos a reconocer y una mesa, sobre la cual estarán situados.
-			depth_write on
-			texture_unit
+==Creación de modelos de objetos==
-			{
-				texture amstel.png
-				tex_coord_set 0
-				colour_op modulate
-				rotate 180
-				scale 1 1
-			}
-		}
-	}
-}
-</syntaxhighlight>
+===Mesa===
-A continuación se muestra la imagen que se ha empleado para la textura y una captura del modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]:
+Primero vamos crear el modelo de la mesa donde vamos a situar los objetos. Se trata de una mesa con tablero de madera y patas metálicas negras de dimensiones 200x75x74 cm (largo x ancho x alto).
-[[file:amstel.png|thumb|200px|center|Textura lata de cerveza]]
+[[file:modelo_mesa.png|thumb|200px|center|Modelo mesa en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
-[[file:modelo_lata_cerveza.png|thumb|200px|center|Lata de cerveza en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
-* Lata de cola
-Para el modelo simplificado de la lata crearemos un archivo llamado "lata_cola.xacro" dentro del directorio "urdf" de nuestro ''package'' con el siguiente contenido:
+Para contener los objetos vamos a [[Fernando-TFM-ROS02#Creando un package|crear un ''package'']] que llamaremos "objetos_fer_modelos" con las siguientes dependencias: geometry_msgs gazebo urdf roscpp. Para el modelo de la mesa crearemos un archivo llamado "mesa.xacro" dentro del directorio "urdf" del ''package'' que acabamos de crear, con el siguiente contenido:
 <syntaxhighlight lang="xml" enclose="div">
@@ Line 2,097: / Line 2,344: @@
 <?xml version="1.0"?>
-<robot name="lata_cola" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<robot name="mesa" xmlns:xacro="http://www.ros.org/wiki/xacro">
 <link name="world" />
-<joint name="lata" type="floating">
+<joint name="mesa" type="floating">
    <parent link="world"/>
-   <child link="lata_link"/>
+   <child link="mesa_link"/>
    <origin xyz="0 0 0" rpy="0 0 0" />
 </joint>
-<link name="lata_link">
+<link name="mesa_link">
    <visual>
      <geometry>
-       <cylinder radius="0.0325" length="0.1115" />
+       <box size="2 0.75 0.03" />
      </geometry>
-     <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
+     <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
-    <material name="red">
+      <material name="wood">
-      <color rgba="1 0 0 1" />
+		<color rgba="${205/255} ${133/255} ${63/255} 1" />
      </material>
    </visual>
    <collision>
-     <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
+     <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
      <geometry>
-       <cylinder radius="0.0325" length="0.1115" />
+       <box size="2 0.75 0.03" />
      </geometry>
    </collision>
    <inertial>
-     <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
+     <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
-     <mass value="0.01" />
+     <mass value="5" />
-       <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+       <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
 </link>
-<joint name="lata_top" type="fixed">
-   <parent link="lata_link"/>
+<xacro:macro name="pata" params="id refx refy">
-   <child link="lata_top_link"/>
+<joint name="pata_${id}" type="fixed">
-   <origin xyz="0 0 0.1185" rpy="0 0 0" />
+   <parent link="mesa_link"/>
+   <child link="pata_${id}_link"/>
+   <origin xyz="${refx*((2/2)-0.045)} ${refy*((0.75/2)-0.045)} 0" rpy="0 0 0" />
 </joint>
-<link name="lata_top_link">
+<link name="pata_${id}_link">
    <visual>
      <geometry>
-       <cylinder radius="0.0325" length="0.0015" />
+       <cylinder radius="0.025" length="0.71" />
      </geometry>
-     <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
+     <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
-     <material name="grey">
+     <material name="black">
-       <color rgba="0.8 0.8 0.8 1" />
+       <color rgba="0 0 0 1" />
      </material>
    </visual>
    <collision>
-     <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
+     <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
      <geometry>
-       <cylinder radius="0.0325" length="0.0015" />
+       <cylinder radius="0.025" length="0.71" />
      </geometry>
    </collision>
    <inertial>
-     <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
+     <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
-     <mass value="0.001" />
+     <mass value="3" />
-       <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+       <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
 </link>
+</xacro:macro>
-<joint name="lata_bottom" type="fixed">
+<xacro:pata id="1" refx="1" refy="1"/>
-  <parent link="lata_link"/>
+<xacro:pata id="2" refx="1" refy="-1"/>
-  <child link="lata_bottom_link"/>
+<xacro:pata id="3" refx="-1" refy="-1"/>
-  <origin xyz="0 0 0" rpy="0 0 0" />
+<xacro:pata id="4" refx="-1" refy="1"/>
-</joint>
-<link name="lata_bottom_link">
+<gazebo reference="mesa_link">
-  <visual>
+   <material>Gazebo/LightWood</material>
-    <geometry>
+   <mu1>10</mu1>
-      <cylinder radius="0.0325" length="0.007" />
+   <mu2>10</mu2>
-    </geometry>
+   <selfCollide>true</selfCollide>
-    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
-    <material name="grey">
-      <color rgba="0.8 0.8 0.8 1" />
-    </material>
-  </visual>
-  <collision>
-    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
-    <geometry>
-      <cylinder radius="0.0325" length="0.007" />
-    </geometry>
-  </collision>
-  <inertial>
-    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
-    <mass value="0.001" />
-      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
-  </inertial>
-</link>
-<gazebo reference="lata_link">
-   <material>Gazebo/CocaCola</material>
-   <mu1>10</mu1>
-   <mu2>10</mu2>
-   <selfCollide>true</selfCollide>
    <turnGravityOff>false</turnGravityOff>
 </gazebo>
-<gazebo reference="lata_top_link">
+<xacro:macro name="gazebo_pata" params="id">
-   <material>PR2/Grey0</material>
+<gazebo reference="pata_${id}_link">
-   <mu1>10</mu1>
+   <material>PR2/Black</material>
-   <mu2>10</mu2>
+   <mu1>0.9</mu1>
+   <mu2>0.9</mu2>
    <selfCollide>true</selfCollide>
    <turnGravityOff>false</turnGravityOff>
 </gazebo>
+</xacro:macro>
-<gazebo reference="lata_bottom_link">
+<xacro:gazebo_pata id="1"/>
-  <material>PR2/Grey0</material>
+<xacro:gazebo_pata id="2"/>
-  <mu1>10</mu1>
+<xacro:gazebo_pata id="3"/>
-  <mu2>10</mu2>
+<xacro:gazebo_pata id="4"/>
-  <selfCollide>true</selfCollide>
-  <turnGravityOff>false</turnGravityOff>
-</gazebo>
 </robot>
@@ Line 2,216: / Line 2,441: @@
 </syntaxhighlight>
-Ahora debemos añadir el material "CocaCola" a [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]. Copiaremos las siguientes líneas al final del archivo "Gazebo.material":
+Para el modelo de la mesa hemos empleado los materiales que vienen por defecto en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo], pero para las latas vamos a necesitar crear materiales nuevos con las texturas de las etiquetas.
+===Creación de nuevos materiales para gazebo===
+Comenzaremos creando dentro de nuestro ''package'' un directorio llamado "Media", dentro del cual crearemos otro directorio llamado "materials", y dentro de este crearemos otros dos directorios llamados "scripts" y "textures". También debemos añadir las siguientes líneas al archivo "manifest.xml" de nuestro ''package'', para colocarlo en la ruta de [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]:
+<syntaxhighlight lang="xml">
+ <export>
+     <gazebo gazebo_media_path="${prefix}" />
+ </export>
+</syntaxhighlight>
+Los materiales se definen para el motor gráfico [http://www.ogre3d.org/ OGRE]. Para las latas vamos a necesitar asociar una imagen al material para que aplique una textura a la geometría. La imagen que usaremos para la textura debe guardarse en la carpeta "textures" creada dentro de nuestro ''package''. Crearemos un archivo llamado "myrabot_fer.material" dentro del directorio "scripts" de nuestro ''package'' con el contenido que se muestra a continuación, para la definición de un nuevo material con textura:
 <syntaxhighlight>
-material Gazebo/CocaCola
+material material_fer/NOMBRE_DEL_NUEVO_MATERIAL
 {
 	technique
@@ Line 2,240: / Line 2,481: @@
 			texture_unit
 			{
-				texture coca_cola.png
+				texture NUESTRO_ARCHIVO_DE_IMAGEN
+				tex_coord_set 0
+				colour_op modulate
+				rotate 180
+				scale 1 1
+			}
+		}
+	}
+}
+</syntaxhighlight>
+===Latas===
+====Lata de cerveza====
+Para el modelo simplificado de la lata crearemos un archivo llamado "lata_cerveza.xacro" dentro del directorio "urdf" de nuestro ''package'' con el siguiente contenido:
+<syntaxhighlight lang="xml" enclose="div">
+<?xml version="1.0"?>
+<robot name="lata_cerveza" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<link name="world" />
+<joint name="lata" type="floating">
+  <parent link="world"/>
+  <child link="lata_link"/>
+  <origin xyz="0 0 0" rpy="0 0 0" />
+</joint>
+<link name="lata_link">
+  <visual>
+    <geometry>
+      <cylinder radius="0.0325" length="0.1115" />
+    </geometry>
+    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
+    <material name="red">
+      <color rgba="1 0 0 1" />
+    </material>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
+    <geometry>
+      <cylinder radius="0.0325" length="0.1115" />
+    </geometry>
+  </collision>
+  <inertial>
+    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
+    <mass value="0.01" />
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+  </inertial>
+</link>
+<joint name="lata_top" type="fixed">
+  <parent link="lata_link"/>
+  <child link="lata_top_link"/>
+  <origin xyz="0 0 0.1185" rpy="0 0 0" />
+</joint>
+<link name="lata_top_link">
+  <visual>
+    <geometry>
+      <cylinder radius="0.0325" length="0.0015" />
+    </geometry>
+    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
+    <material name="grey">
+      <color rgba="0.8 0.8 0.8 1" />
+    </material>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
+    <geometry>
+      <cylinder radius="0.0325" length="0.0015" />
+    </geometry>
+  </collision>
+  <inertial>
+    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
+    <mass value="0.001" />
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+  </inertial>
+</link>
+<joint name="lata_bottom" type="fixed">
+  <parent link="lata_link"/>
+  <child link="lata_bottom_link"/>
+  <origin xyz="0 0 0" rpy="0 0 0" />
+</joint>
+<link name="lata_bottom_link">
+  <visual>
+    <geometry>
+      <cylinder radius="0.0325" length="0.007" />
+    </geometry>
+    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
+    <material name="grey">
+      <color rgba="0.8 0.8 0.8 1" />
+    </material>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
+    <geometry>
+      <cylinder radius="0.0325" length="0.007" />
+    </geometry>
+  </collision>
+  <inertial>
+    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
+    <mass value="0.001" />
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+  </inertial>
+</link>
+<gazebo reference="lata_link">
+  <material>material_fer/Amstel</material>
+  <mu1>100</mu1>
+  <mu2>100</mu2>
+  <selfCollide>true</selfCollide>
+  <turnGravityOff>false</turnGravityOff>
+</gazebo>
+<gazebo reference="lata_top_link">
+  <material>PR2/Grey0</material>
+  <mu1>100</mu1>
+  <mu2>100</mu2>
+  <selfCollide>true</selfCollide>
+  <turnGravityOff>false</turnGravityOff>
+</gazebo>
+<gazebo reference="lata_bottom_link">
+  <material>PR2/Grey0</material>
+  <mu1>100</mu1>
+  <mu2>100</mu2>
+  <selfCollide>true</selfCollide>
+  <turnGravityOff>false</turnGravityOff>
+</gazebo>
+</robot>
+</syntaxhighlight>
+Ahora debemos añadir el material "Amstel" a [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]. Copiaremos las siguientes líneas al final del archivo "myrabot_fer.material":
+<syntaxhighlight>
+material material_fer/Amstel
+{
+	technique
+	{
+		pass
+		{
+                        lighting off
+                        ambient 1 1 1 1
+			diffuse 1 1 1 1
+			specular 0 0 0 0
+			emissive 0 0 0
+			alpha_rejection greater 128
+			depth_write on
+			texture_unit
+			{
+				texture amstel.png
 				tex_coord_set 0
 				colour_op modulate
@@ Line 2,254: / Line 2,660: @@
 A continuación se muestra la imagen que se ha empleado para la textura y una captura del modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]:
+[[file:amstel.png|thumb|200px|center|Textura lata de cerveza]]
+[[file:modelo_lata_cerveza.png|thumb|200px|center|Lata de cerveza en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
+====Lata de cola====
+Para el modelo simplificado de la lata crearemos un archivo llamado "lata_cola.xacro" dentro del directorio "urdf" de nuestro ''package'' con el siguiente contenido:
+<syntaxhighlight lang="xml" enclose="div">
+<?xml version="1.0"?>
+<robot name="lata_cola" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<link name="world" />
+<joint name="lata" type="floating">
+  <parent link="world"/>
+  <child link="lata_link"/>
+  <origin xyz="0 0 0" rpy="0 0 0" />
+</joint>
+<link name="lata_link">
+  <visual>
+    <geometry>
+      <cylinder radius="0.0325" length="0.1115" />
+    </geometry>
+    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
+    <material name="red">
+      <color rgba="1 0 0 1" />
+    </material>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
+    <geometry>
+      <cylinder radius="0.0325" length="0.1115" />
+    </geometry>
+  </collision>
+  <inertial>
+    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
+    <mass value="0.01" />
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+  </inertial>
+</link>
+<joint name="lata_top" type="fixed">
+  <parent link="lata_link"/>
+  <child link="lata_top_link"/>
+  <origin xyz="0 0 0.1185" rpy="0 0 0" />
+</joint>
+<link name="lata_top_link">
+  <visual>
+    <geometry>
+      <cylinder radius="0.0325" length="0.0015" />
+    </geometry>
+    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
+    <material name="grey">
+      <color rgba="0.8 0.8 0.8 1" />
+    </material>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
+    <geometry>
+      <cylinder radius="0.0325" length="0.0015" />
+    </geometry>
+  </collision>
+  <inertial>
+    <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
+    <mass value="0.001" />
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+  </inertial>
+</link>
+<joint name="lata_bottom" type="fixed">
+  <parent link="lata_link"/>
+  <child link="lata_bottom_link"/>
+  <origin xyz="0 0 0" rpy="0 0 0" />
+</joint>
+<link name="lata_bottom_link">
+  <visual>
+    <geometry>
+      <cylinder radius="0.0325" length="0.007" />
+    </geometry>
+    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
+    <material name="grey">
+      <color rgba="0.8 0.8 0.8 1" />
+    </material>
+  </visual>
+  <collision>
+    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
+    <geometry>
+      <cylinder radius="0.0325" length="0.007" />
+    </geometry>
+  </collision>
+  <inertial>
+    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
+    <mass value="0.001" />
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+  </inertial>
+</link>
+<gazebo reference="lata_link">
+  <material>material_fer/CocaCola</material>
+  <mu1>10</mu1>
+  <mu2>10</mu2>
+  <selfCollide>true</selfCollide>
+  <turnGravityOff>false</turnGravityOff>
+</gazebo>
+<gazebo reference="lata_top_link">
+  <material>PR2/Grey0</material>
+  <mu1>10</mu1>
+  <mu2>10</mu2>
+  <selfCollide>true</selfCollide>
+  <turnGravityOff>false</turnGravityOff>
+</gazebo>
+<gazebo reference="lata_bottom_link">
+  <material>PR2/Grey0</material>
+  <mu1>10</mu1>
+  <mu2>10</mu2>
+  <selfCollide>true</selfCollide>
+  <turnGravityOff>false</turnGravityOff>
+</gazebo>
+</robot>
+</syntaxhighlight>
+Ahora debemos añadir el material "CocaCola" a [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]. Copiaremos las siguientes líneas al final del archivo "myrabot_fer.material":
+<syntaxhighlight>
+material Gazebo/CocaCola
+{
+	technique
+	{
+		pass
+		{
+                        lighting off
+                        ambient 1 1 1 1
+			diffuse 1 1 1 1
+			specular 0 0 0 0
+			emissive 0 0 0
+			alpha_rejection greater 128
+			depth_write on
+			texture_unit
+			{
+				texture coca_cola.png
+				tex_coord_set 0
+				colour_op modulate
+				rotate 180
+				scale 1 1
+			}
+		}
+	}
+}
+</syntaxhighlight>
+A continuación se muestra la imagen que se ha empleado para la textura y una captura del modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]:
+[[file:coca_cola.png|thumb|200px|center|Textura lata de cola]]
+[[file:modelo_lata_cola.png|thumb|200px|center|Lata de cola [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
+Para poder cargar los modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] debemos convertirlos a ".urdf", para extender las macros y calcular campos, de nuestros archivos ".xacro". Para esto simplemente deberemos ejecutar la siguiente secuencia de comandos en un terminal:
+<syntaxhighlight>
+roscd objetos_fer_modelos
+cd urdf
+rosrun xacro xacro.py OBJETO.xacro > OBJETO.urdf
+</syntaxhighlight>
+==Ejecución==
+Para la carga de todos los modelos se ha creado un ''launcher'', donde le indicamos a cada modelo sus coordenadas de inserción. Crearemos un archivo llamado "myrabot_latas.launch" dentro del directorio "launch" del ''package'' "myrabot_fer", con el sigiente contenido:
+<syntaxhighlight lang="xml" enclose="div">
+<launch>
+  <param name="/use_sim_time" value="true" />
+  <node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>
+  <node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0.55 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />
+  <node name="spawn_lata_coca_cola" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cola.urdf -urdf -x 0.38 -y -0.18 -z 0.74 -Y -1.57 -model lata_cocacola" respawn="false" output="screen" />
+  <node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.38 -y 0.17 -z 0.74 -Y -1.57 -model lata_amstel" respawn="false" output="screen" />
+  <include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>
+</launch>
+</syntaxhighlight>
+Para iniciar el ''launcher'' simplemente debemos ejecutar el siguiente comando en un terminal:
+<syntaxhighlight>roslaunch myrabot_fer myrabot_latas.launch</syntaxhighlight>
+En el siguiente vídeo se muestra al modelo del MYRAbot ejecutando el programa [[Detección y cálculo de posición de objetos (cámara web)#Señalar objetos (Escoja)|escoja]], en el que el brazo señala el objeto que le indiquemos y pasado un breve período de tiempo regresa a su posición inicial. Para iniciar el programa ejecutaremos el siguiente comando en otro terminal:
+<syntaxhighlight>roslaunch brazo_fer escoja.launch</syntaxhighlight>
+Para seleccionar el objeto a señalar publicaremos en el ''topic'' "selected_object" su número de identificación, ejecutando en un terminal el siguiente comando:
+<syntaxhighlight>rostopic pub selected_object std_msgs/Int16 1 --once</syntaxhighlight>
+Si todo va bien el resultado será similar al que se muestra en el siguiente vídeo:
+<center><videoflash>lp2FUFlsFV4</videoflash></center>
+<center>[http://www.youtube.com/watch?v=lp2FUFlsFV4 Ver vídeo en YouTube]</center>
+=Ejecución del programa seguir objetos=
+==Programa para mover objetos en gazebo usando el teclado==
+Para poder desplazar los modelos de los objetos dentro de [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] hemos escrito un pequeño programa que publica en el ''topic'' "gazebo/set_model_state" la posición del modelo del objeto indicado, en este caso "Lata_amstel", usando el teclado. Crearemos un archivo llamado "mover_objetos.cpp" que guardaremos en el directorio "src" del ''package'' que hemos creado anteriormente para contener los modelos de los objetos, con el siguiente contenido:
+<syntaxhighlight lang="cpp" enclose="div">
+  #include "ros/ros.h"
+  #include "gazebo/ModelState.h"
+  #include <signal.h>
+  #include <termios.h>
+  #include <stdio.h>
+  #define PI 3.14159265
+  #define L1 104
+  #define L2 104
+  #define Lp 60
+  #define KEYCODE_Xmas 0x43
+  #define KEYCODE_Xmenos 0x44
+  #define KEYCODE_Ymas 0x41
+  #define KEYCODE_Ymenos 0x42
+  #define KEYCODE_Zmas 0x77
+  #define KEYCODE_Zmenos 0x73
+  #define KEYCODE_XgiroMas 0x61
+  #define KEYCODE_XgiroMenos 0x64
+  #define KEYCODE_YgiroMas 0x65
+  #define KEYCODE_YgiroMenos 0x71
+struct termios cooked, raw;
+int cont = 0;
+double retardo = 0.1;
+gazebo::ModelState objeto;
+void quit(int sig)
+{
+  tcsetattr(0, TCSANOW, &cooked);
+  ros::shutdown();
+  exit(0);
+}
+void callback(const ros::TimerEvent&)
+{
+	ros::NodeHandle n;
+  	ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);
+        std::string modelo_objeto;
+        ros::NodeHandle nh("~");
+        nh.param("modelo_objeto", modelo_objeto, std::string("Lata_amstel"));
+	ros::Time anterior;
+	if (::cont == 0)
+        {
+		anterior = ros::Time::now();
+		::objeto.model_name = modelo_objeto;
+		::objeto.reference_frame = "world";
+		::objeto.pose.position.x = 0;
+		::objeto.pose.position.y = 0.5;
+		::objeto.pose.position.z = 0.74;
+		::objeto.pose.orientation.x = 0;
+		::objeto.pose.orientation.y = 0;
+		::objeto.pose.orientation.z = 0;
+		::objeto.pose.orientation.w = 1;
+		::cont = 1;
+		object_pub_.publish(::objeto);
+	}
+  char c;
+  // get the console in raw mode
+  tcgetattr(0, &cooked);
+  memcpy(&raw, &cooked, sizeof(struct termios));
+  raw.c_lflag &=~ (ICANON | ECHO);
+  // Setting a new line, then end of file
+  raw.c_cc[VEOL] = 1;
+  raw.c_cc[VEOF] = 2;
+  tcsetattr(0, TCSANOW, &raw);
+  puts("");
+  puts("#####################################################");
+  puts("                   CONTROLES OBJETO");
+  puts("#####################################################");
+  puts("");
+  puts("Flecha arriba:_______ Subir objeto");
+  puts("Flecha abajo:________ Bajar objeto");
+  puts("Flecha izquierda:____ Desplazar objeto a la izquierda");
+  puts("Flecha derecha:______ Desplazar objeto a la derecha");
+  puts("Tecla W:_____________ Desplazar objeto hacia delante");
+  puts("Tecla S:_____________ Desplazar objeto hacia atrás");
+  puts("Tecla A:_____________ Inclinar objeto en eje x mas");
+  puts("Tecla D:_____________ Inclinar objeto en eje x menos");
+  puts("Tecla Q:_____________ Inclinar objeto en eje y mas");
+  puts("Tecla E:_____________ Inclinar objeto en eje y menos");
+    while (ros::ok())
+    {
+    // get the next event from the keyboard
+    if(read(0, &c, 1) < 0)
+    {
+      perror("read():");
+      exit(-1);
+    }
+      if (c == KEYCODE_Xmas)
+      {
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
+		{
+			::objeto.pose.position.x = ::objeto.pose.position.x + 0.005;
+			anterior = ros::Time::now();
+		}
+      }
+      if (c == KEYCODE_Xmenos)
+      {
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
+		{
+			::objeto.pose.position.x = ::objeto.pose.position.x - 0.005;
+			anterior = ros::Time::now();
+		}
+      }
+      if (c == KEYCODE_Ymas)
+      {
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
+		{
+			::objeto.pose.position.z = ::objeto.pose.position.z + 0.005;
+			anterior = ros::Time::now();
+		}
+      }
+      if (c == KEYCODE_Ymenos)
+      {
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
+		{
+			::objeto.pose.position.z = ::objeto.pose.position.z - 0.005;
+			anterior = ros::Time::now();
+		}
+      }
+      if (c == KEYCODE_Zmas)
+      {
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
+		{
+			::objeto.pose.position.y = ::objeto.pose.position.y + 0.005;
+			anterior = ros::Time::now();
+		}
+      }
+      if (c == KEYCODE_Zmenos)
+      {
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
+		{
+			::objeto.pose.position.y = ::objeto.pose.position.y - 0.005;
+			anterior = ros::Time::now();
+		}
+      }
+      if (c == KEYCODE_XgiroMas)
+      {
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
+		{
+			::objeto.pose.orientation.x = ::objeto.pose.orientation.x + 0.1;
+			anterior = ros::Time::now();
+		}
+      }
+      if (c == KEYCODE_XgiroMenos)
+      {
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
+		{
+			::objeto.pose.orientation.x = ::objeto.pose.orientation.x - 0.1;
+			anterior = ros::Time::now();
+		}
+      }
+      if (c == KEYCODE_YgiroMas)
+      {
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
+		{
+			::objeto.pose.orientation.y = ::objeto.pose.orientation.y + 0.1;
+			anterior = ros::Time::now();
+		}
+      }
+      if (c == KEYCODE_YgiroMenos)
+      {
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
+		{
+			::objeto.pose.orientation.y = ::objeto.pose.orientation.y - 0.1;
+			anterior = ros::Time::now();
+		}
+      }
+      object_pub_.publish(::objeto);
+	}
+}
+  int main(int argc, char **argv)
+  {
+	ros::init(argc, argv, "mover_objetos");
+	ros::NodeHandle n;
+  	ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);
+	signal(SIGINT,quit);
+	ros::Timer timer = n.createTimer(ros::Duration(1), callback);
+	ros::spin();
+        return 0;
+  }
+</syntaxhighlight>
+Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro ''package'':
+<syntaxhighlight>rosbuild_add_executable(mover_objetos src/mover_objetos.cpp)</syntaxhighlight>
+Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal:
+<syntaxhighlight>
+roscd objetos_fer_modelos
+make
+</syntaxhighlight>
+==Ejecución==
+Para la carga de todos los modelos se ha creado un ''launcher'', donde le indicamos a cada modelo sus coordenadas de inserción. Crearemos un archivo llamado "myrabot_lata.launch" dentro del directorio "launch" del ''package'' "myrabot_fer", con el sigiente contenido:
+<syntaxhighlight lang="xml" enclose="div">
+<launch>
+  <param name="/use_sim_time" value="true" />
+  <node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>
+  <node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0.55 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />
+  <node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.5 -y 0 -z 0.74 -Y -1.57 -model lata_amstel" respawn="false" output="screen" />
+  <include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>
+</launch>
+</syntaxhighlight>
+Para iniciar el ''launcher'' simplemente debemos ejecutar el siguiente comando en un terminal:
+<syntaxhighlight>roslaunch myrabot_fer myrabot_lata.launch</syntaxhighlight>
+En el siguiente vídeo se muestra al modelo del MYRAbot ejecutando el programa [[Detección y cálculo de posición de objetos (cámara web)#Seguir objetos|seguir objetos]], en el que el brazo sigue el objeto que le indiquemos. Para iniciar el programa ejecutaremos el siguiente comando en otro terminal:
+<syntaxhighlight>roslaunch brazo_fer seguir.launch</syntaxhighlight>
+Para seleccionar el objeto a señalar publicaremos en el ''topic'' "selected_object" su número de identificación, ejecutando en un terminal el siguiente comando:
+<syntaxhighlight>rostopic pub selected_object std_msgs/Int16 1 --once</syntaxhighlight>
+Para poder mover la lata, ejecutaremos en un nuevo terminal el siguiente comando, donde indicamos en el parámetro "modelo_objeto" el nombre del modelo en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] que queremos mover:
+<syntaxhighlight>rosrun objetos_fer_modelos mover_objetos _modelo_objeto:=lata_amstel</syntaxhighlight>
+Si todo va bien el resultado será similar al que se muestra en el vídeo siguiente:
+<center><videoflash>kLfgfHo8OnI</videoflash></center>
-[[file:coca_cola.png|thumb|200px|center|Textura lata de cola]]
+<center>[http://www.youtube.com/watch?v=kLfgfHo8OnI Ver vídeo en YouTube]</center>
-[[file:modelo_lata_cola.png|thumb|200px|center|Lata de cola [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
-===Carga de los modelos de los objetos y del robot en gazebo===
+----
+{| style="border: solid 0px white; width: 100%"
+!Artículos realcionados
+----
+!Otros artículos
+----
+|-
+| valign="top" width="50%" |
-Para la carga de todos los modelos se ha creado un ''launcher'', donde le indicamos a cada modelo sus coordenadas de inserción. Crearemos un archivo llamado "myrabot_latas.launch" dentro del directorio "launch" del ''package'' "myrabot_fer", con el sigiente contenido:
+[[Control brazo MYRAbot (bioloid+arduino)]]<br/>
+[[Detección y cálculo de posición de objetos (cámara web)]]<br/>
+[[Modelo para simulación brazo MYRAbot (urdf+gazebo)]]<br/>
+[[Integración de MYRAbot en moveIt! (gazebo+moveIt!)]]<br/>
+[[Órdenes y confirmación mediante voz (sphinx+festival)]]<br/>
+[[Teleoperación de MYRAbot con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
-<syntaxhighlight lang="xml" enclose="div">
+| valign="top" |
-<launch>
+[[Control brazo y base CeRVaNTeS (maxon+epos2)]]<br/>
+[[Modelo para simulación brazo CeRVaNTeS (urdf+gazebo)]]<br/>
+[[Modelo para simulación CeRVaNTeS (urdf+gazebo)]]<br/>
+[[Integración de CeRVaNTeS en moveIt! (gazebo+moveIt!)]]<br/>
+[[Teleoperación de CeRVaNTeS con controlador inalámbrico xbox360 (controlador xbox360+joy)]]
+----
+[[Fernando-TFM-ROS02|Programas de control para el robot Turtlebot sobre ROS ]]
-  <!-- start gazebo with an simple office -->
+|}
-  <param name="/use_sim_time" value="true" />
+----
-  <node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>
-  <node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0 -y 0.55 -z 0 -model mesa" respawn="false" output="screen" />
-  <node name="spawn_lata_coca_cola" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cola.urdf -urdf -x -0.18 -y 0.38 -z 0.74 -model lata_CocaCola" respawn="false" output="screen" />
-  <node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.17 -y 0.38 -z 0.74 -model lata_Amstel" respawn="false" output="screen" />
-  <include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>
-</launch>
-</syntaxhighlight>
-Para iniciar el ''launcher'' simplemente debemos ejecutar el siguiente comando en un terminal:
-<syntaxhighlight>roslaunch myrabot_fer myrabot_latas.launch</syntaxhighlight>
-En el siguiente vídeo se muestra al modelo del MYRAbot ejecutando el Programa [[Detección y cálculo de posición de objetos (cámara web)#Señalar objetos (Escoja)|escoja]], en el que el brazo señala el objeto que le indiquemos y pasado un breve período de tiempo regresa a su posición inicial.
-Para la iniciar el programa ejecutaremos el siguiente comando en otro terminal:
-<syntaxhighlight>roslaunch brazo_fer escoja.launch</syntaxhighlight>
-Para seleccionar el objeto a señalar publicaremos en el ''topic'' "selected_object" su número de identificación, ejecutando en un terminal el siguiente comando:
-<syntaxhighlight>rostopic pub selected_object std_msgs/Int16 1 --once</syntaxhighlight>
-Si todo va bien el resultado será similar al que se muestra en el vídeo siguiente:
-<center><videoflash>lp2FUFlsFV4</videoflash></center>
-<center>[http://www.youtube.com/watch?v=lp2FUFlsFV4 Ver vídeo en YouTube]</center>
 [[Mobile manipulation | < volver a principal]]