Difference between revisions of "MYRAbot model for simulation (urdf+gazebo)"

Revision as of 16:26, 12 February 2014

Components of MYRAbot model

We have started creating a model of the MYRAbot's structure and we have added the mobile base Roomba, the kinect camera, the arm model and the webcam Logitech Webcam Pro 9000.

First, we will create a new package with the necessary dependences for our programs (urdf std_msgs sensor_msgs tf roscpp). We will execute the next commands in a terminal:

cd ~/ros_workspace
roscreate-pkg myrabot_fer_modelo urdf std_msgs sensor_msgs tf roscpp

URDF MYRAbot's structure

We have made the 3D model of the structure with a CAD software from which we use the parts with a complex geometry.

3D CAD design of MYRAbot's structure

We have divided the model in two files, main file and macros file. For the main file, we will create a file named "estructura-myrabot.xacro" within the folder "urdf" of the created package with the content that is shown below:

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

We will add the next files within the folder "meshes" of our package:

For the macros file, we will create a file named "estructura-myrabot-macros.xacro" within the folder "urdf" of our package with the content that is shown below:

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

We have used the model of the mobile base create of the robot turtlebot, because the vacuum Roomba has similar sensors and physics. We have to install the stacks turtlebot and turtlebot_simulator that contain the robot model and the plugins for gazebo. We have divided the model in two files, main file and plugins file. For the main file, we will create a file named "roomba.xacro" within the folder "urdf" of the created package with the content that is shown below:

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

For the plugins file, we will create a file named "roomba-gazebo.xacro" within the folder "urdf" of our package with the content that is shown below:

<?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_roomba.so">
      <alwaysOn>true</alwaysOn>
      <node_namespace>turtlebot_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_diameter>0.066</wheel_diameter>
      <base_geom>base_link_geom_base_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>fan</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

   <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"/>
   <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:

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

-->

< go back to main

Difference between revisions of "MYRAbot model for simulation (urdf+gazebo)"

Revision as of 16:26, 12 February 2014

Contents

Components of MYRAbot model

URDF MYRAbot's structure

URDF Roomba

URDF kinect

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,152: / Line 1,152: @@
 </syntaxhighlight>
-====Edition of the ''plugin'' for create of the turtlebot====
+===URDF kinect===
-We have to rename the ''topic'' "joint_states" where the ''plugin'' of the mobile base [http://www.irobot.com/us/learn/Educators/Create.aspx create]  publishes the joint states by "joint_states_roomba" in order to avoid the overlap with the ''topic'' "joint_states" where the controller of the [[MYRAbot's arm model for simulation (urdf+gazebo)|arm]] publishes the joint states. First, we
 <!--
+===URDF kinect===
-Para evitar que el ''plugin'' de la base [http://www.irobot.com/us/learn/Educators/Create.aspx create] publique el estado de la uniones en el ''topic'' "joint_states", el mismo empleado por el ''plugin'' para el control de las uniones del [[Modelo para simulación brazo MYRAbot (urdf+gazebo)|brazo]], vamos a modificar el ''topic'' en el que publica para evitar sobrescribir datos. Lo primero debemos copiar el ''package'' "turtlebot_gazebo_plugins", situado en el ''stack'' "turtlebot_simulator", pegarlo en nuestro espacio de trabajo y renombrarlo como "myrabot_gazebo_plugins". Dentro de la carpeta "src" del ''package'' copiado encontraremos el archivo "gazebo_ros_create.cpp", que renombraremos como "gazebo_ros_roomba.cpp". En este archivo tenemos que editar las siguientes líneas como se muestra a continuación:
+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:
-<syntaxhighlight lang="cpp" enclose="div">
+<syntaxhighlight lang="xml" enclose="div">
-GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_create", GazeboRosCreate);
+<?xml version="1.0"?>
-por
+<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"/>
-GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_roomba", GazeboRosCreate);
+<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>
-y
+  <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>
-joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("/joint_states", 1);
+    <visual>
+      <origin xyz=" 0 0 0 " rpy="0 0 0" />
-por
+      <geometry>
+        <mesh filename="package://myrabot_fer_modelo/meshes/kinect.dae"/>
-joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("/joint_states_roomba", 1);
+      </geometry>
+    </visual>
-</syntaxhighlight>
+    <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>
-Tambien hay que editar el archivo "CMakeLists.txt" del ''package'' modificando la línea que se muestra a continuación:
+  </link>
-<syntaxhighlight lang="cpp" enclose="div">
+  <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>
-rosbuild_add_library(gazebo_ros_create src/gazebo_ros_create.cpp)
+  <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>
-por
+  <joint name="camera_depth_optical_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
-rosbuild_add_library(gazebo_ros_roomba src/gazebo_ros_roomba.cpp)
+    <parent link="camera_depth_frame" />
+    <child link="camera_depth_optical_frame" />
+  </joint>
-</syntaxhighlight>
+  <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>
-Para compilar el ''plugin'' editado simplemente deberemos ejecutar en un terminal la siguiente secuencia de comandos:
+  <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>
-<syntaxhighlight>
+  <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>
-rosmake myrabot_gazebo_plugins
+  <joint name="camera_rgb_optical_joint" type="fixed">
-roscd myrabot_gazebo_plugins
+    <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
-make
+     <parent link="camera_rgb_frame" />
+     <child link="camera_rgb_optical_frame" />
-</syntaxhighlight>
-===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:
-<syntaxhighlight lang="xml" enclose="div">
-<?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">
+   <link name="camera_rgb_optical_frame">
      <inertial>
-       <mass value="0.01" />
+       <mass value="0.001" />
        <origin xyz="0 0 0" />
-       <inertia ixx="0.001" ixy="0.0" ixz="0.0"
+       <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
-         iyy="0.001" iyz="0.0"
+         iyy="0.0001" iyz="0.0"
-         izz="0.001" />
+         izz="0.0001" />
      </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">
+   <gazebo reference="camera_link">
-    <origin xyz="0 0.018 0" rpy="0 0 0" />
+    <sensor:camera name="camera">
-     <parent link="camera_link" />
+      <imageFormat>R8G8B8</imageFormat>
-    <child link="camera_depth_frame" />
+      <imageSize>640 480</imageSize>
-   </joint>
+      <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>
-  <link name="camera_depth_frame">
+</syntaxhighlight>
-    <inertial>
-      <mass value="0.01" />
+===URDF cámara web===
-      <origin xyz="0 0 0" />
-      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
+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:
-        iyy="0.001" iyz="0.0"
-        izz="0.001" />
-    </inertial>
-  </link>
-  <joint name="camera_depth_optical_joint" type="fixed">
+<syntaxhighlight lang="xml" enclose="div">
-    <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">
+<?xml version="1.0"?>
-    <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">
+<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">
-    <origin xyz="0 -0.005 0" rpy="0 0 0" />
-     <parent link="camera_link" />
-     <child link="camera_rgb_frame" />
+<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="camera_rgb_frame">
+   <link name="e_webcam_1_link">
      <inertial>
-       <mass value="0.001" />
+       <mass value="0.01" />
        <origin xyz="0 0 0" />
-       <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
+       <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
-        iyy="0.0001" iyz="0.0"
-        izz="0.0001" />
      </inertial>
-   </link>
+    <visual>
+      <origin xyz=" 0 0 0 " rpy="0 0 0" />
-   <joint name="camera_rgb_optical_joint" type="fixed">
+      <geometry>
-     <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
+        <mesh filename="package://myrabot_robot_model/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
-     <parent link="camera_rgb_frame" />
+      </geometry>
-     <child link="camera_rgb_optical_frame" />
+      <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="camera_rgb_optical_frame">
+   <link name="e_webcam_2_link">
      <inertial>
-       <mass value="0.001" />
+       <mass value="0.01" />
-       <origin xyz="0 0 0" />
+       <origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
-       <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
+       <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
-        iyy="0.0001" iyz="0.0"
-        izz="0.0001" />
      </inertial>
-   </link>
+    <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>
-   <gazebo reference="camera_link">
+   <link name="e_webcam_optica_link">
-     <sensor:camera name="camera">
+     <inertial>
-       <imageFormat>R8G8B8</imageFormat>
+       <mass value="0.001" />
-       <imageSize>640 480</imageSize>
+       <origin xyz="0 0 0" rpy="0 0 0" />
-       <hfov>60</hfov>
+       <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001" />
-      <nearClip>0.05</nearClip>
+    </inertial>
-      <farClip>5</farClip>
+    <visual>
-      <updateRate>20</updateRate>
+       <origin xyz="0 0 0" rpy="0 1.57 0" />
-       <baseline>0.1</baseline>
+       <geometry>
-       <controller:gazebo_ros_openni_kinect name="kinect_camera_controller" plugin="libgazebo_ros_openni_kinect.so">
+        <cylinder radius="0.017" length="0.004" />
-        <alwaysOn>true</alwaysOn>
+      </geometry>
-        <updateRate>20</updateRate>
+      <material name="white">
-         <imageTopicName>/camera/image_raw</imageTopicName>
+         <color rgba="1 1 1 1"/>
-        <pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
+      </material>
-        <cameraInfoTopicName>/camera/camera_info</cameraInfoTopicName>
+    </visual>
-        <frameName>camera_depth_optical_frame</frameName>
+    <collision>
-        <distortion_k1>0.0</distortion_k1>
+      <origin xyz="0 0 0" rpy="0 1.57 0" />
-        <distortion_k2>0.0</distortion_k2>
+      <geometry>
-        <distortion_k3>0.0</distortion_k3>
+         <cylinder radius="0.017" length="0.004" />
-         <distortion_t1>0.0</distortion_t1>
+       </geometry>
-        <distortion_t2>0.0</distortion_t2>
+     </collision>
-       </controller:gazebo_ros_openni_kinect>
+   </link>
-     </sensor:camera>
-   </gazebo>
-</xacro:macro>
-</robot>
+<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>
-</syntaxhighlight>
+<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>
-===URDF cámara web===
+<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>
-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:
+  <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>
-<syntaxhighlight lang="xml" enclose="div">
+</robot>
-<?xml version="1.0"?>
+</syntaxhighlight>
-<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">
+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:
-<xacro:macro name="webcam" params="parent *origin">
+* [http://www.fernando.casadogarcia.es/files/webcam_1.stl webcam_1.stl]
+* [http://www.fernando.casadogarcia.es/files/webcam_2.stl webcam_2.stl]
-  <joint name="webcam_1" type="fixed">
-	<insert_block name="origin" />
+===URDF sensor ultrasonidos===
-    <parent link="${parent}" />
-    <child link="e_webcam_1_link" />
+Para la detección de acercamientos por la parte trasera y laterales del robot se van ha emplear sensores de ultrasonidos. Lo primero vamos a crear el ''plugin'' que vamos a emplear para este sensor. 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:
-  </joint>
+<syntaxhighlight lang="cpp" enclose="div">
-  <link name="e_webcam_1_link">
+/*
-    <inertial>
+ * Software License Agreement (Modified BSD License)
-      <mass value="0.01" />
+ *
-      <origin xyz="0 0 0" />
+ *  Copyright (c) 2012, PAL Robotics, S.L.
-      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
+ *  All rights reserved.
-    </inertial>
+ *
-    <visual>
+ *  Redistribution and use in source and binary forms, with or without
-      <origin xyz=" 0 0 0 " rpy="0 0 0" />
+ *  modification, are permitted provided that the following conditions
-      <geometry>
+ *  are met:
-        <mesh filename="package://myrabot_fer_modelo/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
+ *
-      </geometry>
+ *   * Redistributions of source code must retain the above copyright
-      <material name="black">
+ *     notice, this list of conditions and the following disclaimer.
-	    <color rgba="0 0 0 1" />
+ *   * Redistributions in binary form must reproduce the above
-      </material>
+ *     copyright notice, this list of conditions and the following
-     </visual>
+ *     disclaimer in the documentation and/or other materials provided
-     <collision>
+ *     with the distribution.
-      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
+ *   * Neither the name of PAL Robotics, S.L. nor the names of its
-      <geometry>
+ *     contributors may be used to endorse or promote products derived
-        <mesh filename="package://myrabot_fer_modelo/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
+ *     from this software without specific prior written permission.
-      </geometry>
+ *
-     </collision>
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-  </link>
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-  <joint name="webcam_2" type="fixed">
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-    <origin xyz="0 ${0.135-0.04-(0.02-0.009)} ${(0.185/2)+0.0045}" rpy="0.82 0 0" />
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-    <parent link="e_webcam_1_link" />
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-    <child link="e_webcam_2_link" />
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-  </joint>
+ *  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.
+ */
-  <link name="e_webcam_2_link">
+#include <gazebo/Sensor.hh>
-    <inertial>
+#include <gazebo/Global.hh>
-      <mass value="0.01" />
+#include <gazebo/XMLConfig.hh>
-      <origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
+#include <gazebo/Simulator.hh>
-      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
+#include <gazebo/gazebo.h>
-    </inertial>
+#include <gazebo/GazeboError.hh>
-    <visual>
+#include <gazebo/ControllerFactory.hh>
-      <origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
+#include <limits>
-      <geometry>
+#include <range_gazebo_plugin/gazebo_ros_sonar.h>
-        <mesh filename="package://myrabot_fer_modelo/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
+using namespace gazebo;
-      <material name="black" />
-    </visual>
+GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_sonar", GazeboRosSonar)
-    <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_fer_modelo/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">
+GazeboRosSonar::GazeboRosSonar(Entity *parent) : Controller(parent)
-    <inertial>
+{
-      <mass value="0.001" />
+  sensor_ = dynamic_cast<RaySensor*>(parent);
-      <origin xyz="0 0 0" rpy="0 0 0" />
+  if (!sensor_) gzthrow("GazeboRosSonar controller requires a RaySensor as its parent");
-      <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">
+  Param::Begin(&parameters);
-   <material>Gazebo/Black</material>
+  topic_param_ = new ParamT<std::string>("topicName", "sonar", false);
-   <mu1>0.5</mu1>
+   frame_id_param_ = new ParamT<std::string>("frameId", "", false);
-   <mu2>0.5</mu2>
+  radiation_param_ = new ParamT<std::string>("radiation","ultrasound",false);
-   <selfCollide>true</selfCollide>
+   fov_param_ = new ParamT<double>("fov", 0.05, false);
-  <turnGravityOff>false</turnGravityOff>
+   gaussian_noise_ = new ParamT<double>("gaussianNoise", 0.0, 0);
-</gazebo>
+   Param::End();
+}
-<gazebo reference="e_webcam_2_link">
+////////////////////////////////////////////////////////////////////////////////
-  <material>Gazebo/Black</material>
+// Destructor
-   <mu1>0.5</mu1>
+GazeboRosSonar::~GazeboRosSonar()
-   <mu2>0.5</mu2>
+{
-   <selfCollide>true</selfCollide>
+   delete topic_param_;
-   <turnGravityOff>false</turnGravityOff>
+   delete frame_id_param_;
-</gazebo>
+   delete radiation_param_;
+   delete fov_param_;
+}
-<gazebo reference="e_webcam_optica_link">
+////////////////////////////////////////////////////////////////////////////////
-   <material>Gazebo/White</material>
+// Load the controller
-   <mu1>0.5</mu1>
+void GazeboRosSonar::LoadChild(XMLConfigNode *node)
-   <mu2>0.5</mu2>
+{
-   <selfCollide>true</selfCollide>
+  ROS_INFO("INFO: gazebo_ros_ir plugin loading" );
-   <turnGravityOff>false</turnGravityOff>
+   topic_param_->Load(node);
-</gazebo>
+   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;
-   <gazebo reference="e_webcam_optica_link">
+   Range.field_of_view = **fov_param_;
-    <sensor:camera name="webcam">
+  Range.max_range = sensor_->GetMaxRange();
-      <imageFormat>R8G8B8</imageFormat>
+  Range.min_range = sensor_->GetMinRange();
-      <imageSize>640 480</imageSize>
-      <hfov>60</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>
-        <imageTopicName>/image_raw</imageTopicName>
-        <cameraInfoTopicName>/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>
+  sensor_->SetActive(false);
+  node_handle_ = new ros::NodeHandle("");
+  publisher_ = node_handle_->advertise<sensor_msgs::Range>(**topic_param_, 10);
+}
-</syntaxhighlight>
+////////////////////////////////////////////////////////////////////////////////
+// Utility for adding noise
-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:
+double GazeboRosSonar::GaussianKernel(double mu,double sigma)
+{
-* [http://www.fernando.casadogarcia.es/files/webcam_1.stl webcam_1.stl]
+    // using Box-Muller transform to generate two independent standard normally disbributed normal variables
-* [http://www.fernando.casadogarcia.es/files/webcam_2.stl webcam_2.stl]
+    // 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;
+}
-===URDF sensor ultrasonidos===
+////////////////////////////////////////////////////////////////////////////////
+// Update the controller
+void GazeboRosSonar::UpdateChild()
+{
+  if (!sensor_->IsActive()) sensor_->SetActive(true);
-Para la detección de acercamientos por la parte trasera y laterales del robot se van ha emplear sensores de ultrasonidos. Lo primero vamos a crear el ''plugin'' que vamos a emplear para este sensor. Dentro de la carpeta "src" del [[Modelo para simulación MYRAbot (urdf+gazebo)#Edición del plugin del turtlebot|''package'' creado para los ''plugin'']] crearemos un archivo llamado "gazebo_ros_sonar.cpp" con el siguiente contenido:
+  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();
-<syntaxhighlight lang="cpp" enclose="div">
+  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());
- * Software License Agreement (Modified BSD License)
+  publisher_.publish(Range);
- *
+}
- *  Copyright (c) 2012, PAL Robotics, S.L.
- *  All rights reserved.
+////////////////////////////////////////////////////////////////////////////////
- *
+// Finalize the controller
- *  Redistribution and use in source and binary forms, with or without
+void GazeboRosSonar::FiniChild()
- *  modification, are permitted provided that the following conditions
+{
- *  are met:
+   sensor_->SetActive(false);
- *
+   node_handle_->shutdown();
- *   * Redistributions of source code must retain the above copyright
+   delete node_handle_;
- *     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
+</syntaxhighlight>
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
+Añadiremos al archivo "CMakeLists.txt" del ''package'' la siguiente línea al final:
- *   * Neither the name of PAL Robotics, S.L. nor the names of its
- *     contributors may be used to endorse or promote products derived
+<syntaxhighlight>rosbuild_add_library(gazebo_ros_sonar src/gazebo_ros_sonar.cpp)</syntaxhighlight>
- *     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>
+También añadiremos las siguientes líneas al archivo "manifest.xml" del ''package'', antes de las etiqueta </package>:
-#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 <range_gazebo_plugin/gazebo_ros_sonar.h>
-using namespace gazebo;
+<syntaxhighlight lang="xml">
-GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_sonar", GazeboRosSonar)
+  <export>
+    <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib" />
+    <gazebo plugin_path="${prefix}/lib" />
+  </export>
-GazeboRosSonar::GazeboRosSonar(Entity *parent) : Controller(parent)
+</syntaxhighlight>
-{
-  sensor_ = dynamic_cast<RaySensor*>(parent);
+Para compilar el nuevo ''plugin'' simplemente deberemos ejecutar en un terminal la siguiente secuencia de comandos:
-  if (!sensor_) gzthrow("GazeboRosSonar controller requires a RaySensor as its parent");
-  Param::Begin(&parameters);
+<syntaxhighlight>
-  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();
-}
-////////////////////////////////////////////////////////////////////////////////
+roscd myrabot_gazebo_plugins
-// Destructor
+make
-GazeboRosSonar::~GazeboRosSonar()
-{
-  delete topic_param_;
-  delete frame_id_param_;
-  delete radiation_param_;
-  delete fov_param_;
-}
-////////////////////////////////////////////////////////////////////////////////
+</syntaxhighlight>
-// 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);
-}
-///////////////////////////////////////////////////////
+En el directorio "urdf" del ''package'' del robot vamos a crear un archivo llamado "ultrasonidos.xacro" con el siguiente contenido:
-// Initialize the controller
-void GazeboRosSonar::InitChild()
-{
-  Range.header.frame_id = **frame_id_param_;
-  if (**radiation_param_==std::string("ultrasound"))
+<syntaxhighlight lang="xml" enclose="div">
-      Range.radiation_type = sensor_msgs::Range::ULTRASOUND;
-  else
-      Range.radiation_type = sensor_msgs::Range::INFRARED;
-  Range.field_of_view = **fov_param_;
+<?xml version="1.0"?>
-  Range.max_range = sensor_->GetMaxRange();
-  Range.min_range = sensor_->GetMinRange();
-  sensor_->SetActive(false);
+<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">
-  node_handle_ = new ros::NodeHandle("");
-   publisher_ = node_handle_->advertise<sensor_msgs::Range>(**topic_param_, 10);
+<xacro:macro name="ultrasonidos" params="id parent d_centro *origin">
-}
-////////////////////////////////////////////////////////////////////////////////
+  <joint name="ultrasonidos_${id}_joint_fisico" type="fixed">
-// Utility for adding noise
+    <parent link="${parent}"/>
-double GazeboRosSonar::GaussianKernel(double mu,double sigma)
+    <child link="ultrasonidos_${id}_link_fisico"/>
-{
+    <insert_block name="origin" />
-    // using Box-Muller transform to generate two independent standard normally disbributed normal variables
+  </joint>
-     // see wikipedia
-     double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
+  <link name="ultrasonidos_${id}_link_fisico">
-     double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
+    <inertial>
-     double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
+      <mass value="0.001"/>
-    //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
+      <origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
-    // we'll just use X
+	  <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
-    // scale to our mu and sigma
+    </inertial>
-    X = sigma * X + mu;
+    <visual>
-    return X;
+      <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>
-// Update the controller
-void GazeboRosSonar::UpdateChild()
+<gazebo reference="ultrasonidos_${id}_link_fisico">
-{
+  <material>myrabot_fer/Black</material>
-   if (!sensor_->IsActive()) sensor_->SetActive(true);
+  <mu1>0.5</mu1>
+  <mu2>0.5</mu2>
+  <selfCollide>true</selfCollide>
+   <turnGravityOff>false</turnGravityOff>
+</gazebo>
-  Range.header.stamp.sec  = (Simulator::Instance()->GetSimTime()).sec;
+    <gazebo reference="ultrasonidos_${id}_link">
-  Range.header.stamp.nsec = (Simulator::Instance()->GetSimTime()).nsec;
+      <sensor:ray name="ultrasonidos_${id}">
-  Range.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();
+        <displayRays>false</displayRays>
-  for(int i = 0; i < sensor_->GetRangeCount(); ++i) {
+        <rayCount>5</rayCount>
-    double ray = sensor_->GetRange(i);
+        <rangeCount>5</rangeCount>
-    if (ray < Range.range) Range.range = ray;
+        <verticalRayCount>1</verticalRayCount>
-  }
+        <verticalRangeCount>1</verticalRangeCount>
-  Range.range = std::min(Range.range + this->GaussianKernel(0,**gaussian_noise_), sensor_->GetMaxRange());
+        <minAngle>-16</minAngle>
-  publisher_.publish(Range);
+        <verticalMinAngle>-16</verticalMinAngle>
-}
+        <maxAngle>16</maxAngle>
+        <verticalMaxAngle>16</verticalMaxAngle>
-////////////////////////////////////////////////////////////////////////////////
+        <minRange>0</minRange>
-// Finalize the controller
+        <maxRange>7</maxRange>
-void GazeboRosSonar::FiniChild()
+        <resRange>0.01</resRange>
-{
-  sensor_->SetActive(false);
-  node_handle_->shutdown();
-  delete node_handle_;
-}
-</syntaxhighlight>
+        <updateRate>20</updateRate>
-Añadiremos al archivo "CMakeLists.txt" del ''package'' la siguiente línea, debajo de la línea de los ''plugin'' existentes:
+        <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>
-<syntaxhighlight>rosbuild_add_library(gazebo_ros_sonar src/gazebo_ros_sonar.cpp)</syntaxhighlight>
+</syntaxhighlight>
-Para compilar el nuevo ''plugin'' simplemente deberemos ejecutar en un terminal la siguiente secuencia de comandos:
+==Ensamblado del modelo del MYRAbot==
-<syntaxhighlight>
+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:
-roscd myrabot_gazebo_plugins
+<syntaxhighlight lang="xml" enclose="div">
-make
-</syntaxhighlight>
-En el directorio "urdf" del ''package'' del robot vamos a crear un archivo llamado "ultrasonidos.xacro" con el siguiente contenido:
-<syntaxhighlight lang="xml" enclose="div">
 <?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">
+<robot name="MYRAbot"
+       xmlns:xi="http://www.w3.org/2001/XInclude"
-<xacro:macro name="ultrasonidos" params="id parent d_centro *origin">
+       xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+       xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
-  <joint name="ultrasonidos_${id}_joint_fisico" type="fixed">
+       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
-    <parent link="${parent}"/>
+       xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
-    <child link="ultrasonidos_${id}_link_fisico"/>
+       xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
-    <insert_block name="origin" />
+       xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
-  </joint>
+       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+       xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
-  <link name="ultrasonidos_${id}_link_fisico">
+       xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
-    <inertial>
+       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
-      <mass value="0.001"/>
+       xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
-      <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"/>
+	<include filename="$(find myrabot_fer_modelo)/urdf/roomba.xacro" />
-    </inertial>
-    <visual>
+	<include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot.xacro" />
-      <origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
-      <geometry>
+	<include filename="$(find myrabot_fer_modelo)/urdf/kinect.xacro" />
-        <cylinder radius="${0.015/2}" length="0.015" />
-      </geometry>
+	<include filename="$(find brazo_fer_modelo)/urdf/brazo.xacro" />
-      <material name="black">
-		<color rgba="0 0 0 1" />
+	<include filename="$(find myrabot_fer_modelo)/urdf/webcam.xacro" />
-	  </material>
-    </visual>
+	<roomba />
-    <collision>
-      <origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
+	<estructura_myrabot parent="base_link">
-      <geometry>
+		<origin rpy="0 0 1.57" xyz="0.135 -0.135 0.063"/>
-        <cylinder radius="${0.015/2}" length="0.015" />
+	</estructura_myrabot>
-      </geometry>
-    </collision>
+	<kinect parent="e_base_kinect_link">
-  </link>
+		<origin rpy="0 0 -1.57" xyz="0.135 0.090 0.112"/>
+	</kinect>
-<joint name="ultrasonidos_${id}_joint" type="fixed">
-    <parent link="ultrasonidos_${id}_link_fisico"/>
+	<brazo parent="e_base_brazo_1_link">
-    <child link="ultrasonidos_${id}_link"/>
+		<origin rpy="0 0 3.1416" xyz="0.135 0.0335 0.075"/>
-    <origin rpy="0 0 -1.57" xyz="0 -${d_centro+(0.015/2)} ${0.015/2}"/>
+	</brazo>
-</joint>
+	<webcam parent="e_monitor_link">
-  <link name="ultrasonidos_${id}_link">
+		<origin rpy="0 0 0" xyz="0 0 ${0.185/2}"/>
+	</webcam>
-  </link>
+	<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"/>
-<gazebo reference="ultrasonidos_${id}_link_fisico">
+	</ultrasonidos>
-  <material>Gazebo/Black</material>
-  <mu1>0.5</mu1>
+	<ultrasonidos id="2" parent="e_base_2_link" d_centro="0.150">
-  <mu2>0.5</mu2>
+		<origin rpy="0 0 2.36" xyz="0.135 0.135 0.02"/>
-  <selfCollide>true</selfCollide>
+	</ultrasonidos>
-  <turnGravityOff>false</turnGravityOff>
-</gazebo>
-    <gazebo reference="ultrasonidos_${id}_link">
+	<ultrasonidos id="3" parent="e_base_2_link" d_centro="0.150">
-      <sensor:ray name="ultrasonidos_${id}">
+		<origin rpy="0 0 3.14" xyz="0.135 0.135 0.02"/>
+	</ultrasonidos>
-        <displayRays>false</displayRays>
+	<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>
-        <rayCount>5</rayCount>
+</syntaxhighlight>
-        <rangeCount>5</rangeCount>
-        <verticalRayCount>1</verticalRayCount>
-        <verticalRangeCount>1</verticalRangeCount>
-        <minAngle>-16</minAngle>
+===Visualización del modelo en rviz===
-        <verticalMinAngle>-16</verticalMinAngle>
-        <maxAngle>16</maxAngle>
-        <verticalMaxAngle>16</verticalMaxAngle>
-        <minRange>0</minRange>
+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:
-        <maxRange>7</maxRange>
-        <resRange>0.01</resRange>
-        <updateRate>20</updateRate>
+<syntaxhighlight lang="xml" enclose="div">
-        <controller:gazebo_ros_sonar name="gazebo_ros_ultrasonidos_${id}_controller" plugin="libgazebo_ros_sonar.so">
+<launch>
-          <gaussianNoise>0.005</gaussianNoise>
-          <alwaysOn>true</alwaysOn>
+	<param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_fer_modelo)/urdf/myrabot.urdf.xacro'" />
-          <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>
-</syntaxhighlight>
+	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
-==Ensamblado del modelo del MYRAbot==
+	<param name="use_gui" value="true"/>
-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:
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
-<syntaxhighlight lang="xml" enclose="div">
+	<node name="rviz" pkg="rviz" type="rviz" />
-<?xml version="1.0"?>
+</launch>
-<robot name="MYRAbot"
+</syntaxhighlight>
-       xmlns:xi="http://www.w3.org/2001/XInclude"
-       xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+Para iniciar el ''launcher'' debemos ejecutar en un terminal el siguiente comando:
-       xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
-       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_rviz.launch</syntaxhighlight>
-       xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
-       xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+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:
-       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" />
+[[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]]]
-	<roomba />
+==Carga del modelo en gazebo==
-	<estructura_myrabot parent="base_link">
+===Uso de un único ''topic'' "Joint_states_myrabot"===
-		<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">
+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'':
-		<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">
+<syntaxhighlight lang="cpp" enclose="div">
-		<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>
+  #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];
-</syntaxhighlight>
+  void brazo(const sensor_msgs::JointState& brazo_states)
+  {
-===Visualización del modelo en rviz===
+	int tam = brazo_states.name.size();
-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:
+	if (::cont_brazo == 0)
+	{
-<syntaxhighlight lang="xml" enclose="div">
+		for (int i = 0; i < tam; i++)
+		{
-<launch>
+			::name[i] = brazo_states.name[i];
+			::position[i] = brazo_states.position[i];
-	<param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_fer_modelo)/urdf/myrabot.urdf.xacro'" />
+			::velocity[i] = brazo_states.velocity[i];
+			::effort[i] = brazo_states.effort[i];
-	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
+		}
-	<param name="use_gui" value="true"/>
+		::cont_brazo = 1;
+	}
-	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+  }
-	<node name="rviz" pkg="rviz" type="rviz" />
+  void roomba(const sensor_msgs::JointState& roomba_states)
+  {
-</launch>
+	int tam = roomba_states.name.size();
-</syntaxhighlight>
+	if (::cont_roomba == 0)
+	{
-Para iniciar el ''launcher'' debemos ejecutar en un terminal el siguiente comando:
+		for (int i = 0; i < tam; i++)
+		{
-<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_rviz.launch</syntaxhighlight>
+			::name[i+5] = roomba_states.name[i];
+			::position[i+5] = roomba_states.position[i];
-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:
+			::velocity[i+5] = roomba_states.velocity[i];
+			::effort[i+5] = roomba_states.effort[i];
-[[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]]]
+		}
-==Carga del modelo en gazebo==
+		::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);
-===Edición del ''plugin'' del brazo===
+    while (ros::ok())
+    {
-Tenemos que modificar una línea del archivo "controller_manager.cpp", situado en el directorio "src" del ''package'' "pr2_controller_manager", como se muestra a continuación:
+		if (::cont_brazo == 1 && ::cont_roomba == 1)
-<syntaxhighlight lang="cpp">
+		{
+			for (int i = 0; i < 9; i++)
-pub_joint_state_(nh, "joint_states", 1),
+			{
+				::joint_states_todo.name[i] = ::name[i];
-por
+				::joint_states_todo.position[i] = ::position[i];
+				::joint_states_todo.velocity[i] = ::velocity[i];
-pub_joint_state_(nh, "joint_states_brazo", 1),
+				::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>
-Compilaremos para generar el nuevo archivo. Con esto conseguimos que los estados de las uniones del brazo se publiquen en el ''topic'' "joint_states_brazo", en ver de en el ''topic'' "joint_states", que emplearemos para la publicación de los estados de las uniones del brazo y la base.
+===Carga en gazebo===
-===Uso de un único ''topic'' "Joint_states"===
+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:
+<syntaxhighlight lang="xml" enclose="div">
-Para poder tener en el ''topic'' "joint_states" el estado de las uniones del brazo y de la base, hemos creado un pequeño programa que toma los valores de los ''topics'' "joint_states_brazo" y "joint_states_roomba", y los publica en este único ''topic''. Crearemos un archivo llamado "remap_joint_states.cpp" dentro del directorio "src" de nuestro ''package'':
+<launch>
-<syntaxhighlight lang="cpp" enclose="div">
+  <param name="/use_sim_time" value="true" />
-   #include "ros/ros.h"
+   <node name="gazebo" pkg="gazebo" type="gazebo" args="-u $(find turtlebot_gazebo)/worlds/empty.world" respawn="false" output="screen"/>
-   #include "ros/time.h"
+   <node name="gazebo_gui" pkg="gazebo" type="gui" />
-   #include "sensor_msgs/JointState.h"
+<group ns="brazo">
-   sensor_msgs::JointState joint_states_todo;
+   <param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_robot_model)/urdf/myrabot.urdf.xacro'" />
-   int cont_brazo = 0;
-  int cont_roomba = 0;
+   <node name="spawn_myrabot" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model MYRAbot" respawn="false" output="screen" />
-  std::string name[9];
-   float position[9];
+   <rosparam file="$(find myrabot_arm_model)/controller.yaml" command="load"/>
-  float velocity[9];
-   float effort[9];
+   <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" />
-   void brazo(const sensor_msgs::JointState& brazo_states)
+</group>
-   {
-	int tam = brazo_states.name.size();
+   <node name="modelo_brazo" pkg="myrabot_arm_model" type="modelo_brazo" />
-	if (::cont_brazo == 0)
+  <node name="remap_joint_states" pkg="myrabot_robot_model" type="remap_joint_states" />
-	{
-		for (int i = 0; i < tam; i++)
-		{
+  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
-			::name[i] = brazo_states.name[i];
+    <param name="freq" value="30.0"/>
-			::position[i] = brazo_states.position[i];
+    <param name="sensor_timeout" value="1.0"/>
-			::velocity[i] = brazo_states.velocity[i];
+    <param name="publish_tf" value="true"/>
-			::effort[i] = brazo_states.effort[i];
+    <param name="odom_used" value="true"/>
-		}
+    <param name="imu_used" value="false"/>
+    <param name="vo_used" value="false"/>
-		::cont_brazo = 1;
+		<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">
-   void roomba(const sensor_msgs::JointState& roomba_states)
+    <param name="max_rate" value="20.0"/>
-  {
+    <remap from="cloud_in" to="/camera/depth/points"/>
-	int tam = roomba_states.name.size();
+    <remap from="cloud_out" to="cloud_throttled"/>
+  </node>
-	if (::cont_roomba == 0)
-	{
+  <node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
-		for (int i = 0; i < tam; i++)
+    <param name="output_frame_id" value="/camera_depth_frame"/>
-		{
+    <!-- heights are in the (optical?) frame of the kinect -->
-			::name[i+5] = roomba_states.name[i];
+    <param name="min_height" value="-0.15"/>
-			::position[i+5] = roomba_states.position[i];
+    <param name="max_height" value="0.15"/>
-			::velocity[i+5] = roomba_states.velocity[i];
+    <remap from="cloud" to="/cloud_throttled"/>
-			::effort[i+5] = roomba_states.effort[i];
+   </node>
-		}
+   <node pkg="nodelet" type="nodelet" name="kinect_laser_narrow" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
-		::cont_roomba = 1;
+    <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"/>
-   int main(int argc, char **argv)
+    <remap from="cloud" to="/cloud_throttled"/>
-  {
+    <remap from="scan" to="/narrow_scan"/>
+  </node>
-	ros::init(argc, argv, "remap_joint_states");
+</launch>
-	ros::NodeHandle n;
+</syntaxhighlight>
-  	ros::Subscriber joint_states_brazo_sub_= n.subscribe("joint_states_brazo", 1, brazo);
-  	ros::Subscriber joint_states_roomba_sub_= n.subscribe("joint_states_roomba", 1, roomba);
+Para lanzar el ''launcher'' ejecutaremos el siguiente comando en un terminal:
-  	ros::Publisher joint_states_pub_=n.advertise<sensor_msgs::JointState>("joint_states", 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())
+<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_gazebo.launch</syntaxhighlight>
-    {
+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:
-		if (::cont_brazo == 1 && ::cont_roomba == 1)
-		{
+[[file:modelo_MYRAbot_gazebo.png|thumb|500px|center|Modelo MYRAbot en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
-			for (int i = 0; i < 9; i++)
-			{
+===Programa de prueba (Vuela al ruedo)===
-				::joint_states_todo.name[i] = ::name[i];
-				::joint_states_todo.position[i] = ::position[i];
+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.
-				::joint_states_todo.velocity[i] = ::velocity[i];
-				::joint_states_todo.effort[i] = ::effort[i];
+Dentro de este ''package'' vamos a crear un archivo llamado "vuelta_al_ruedo.cpp" en el directorio "src" con el siguiente contenido:
-			}
+<syntaxhighlight lang="cpp" anclose="div">
-			ahora = ros::Time::now();
+  #include "ros/ros.h"
-			::joint_states_todo.header.stamp = ahora;
+  #include "brazo_fer/Servos.h"
+   #include "brazo_fer/WriteServos.h"
-			joint_states_pub_.publish(::joint_states_todo);
+  #include "brazo_fer/ReadServos.h"
+  #include "geometry_msgs/Twist.h"
-			::cont_brazo = 0;
+   #include "std_msgs/Bool.h"
-			::cont_roomba = 0;
-		}
-			ros::spinOnce();
-			loop_rate.sleep();
-	}
-	ros::spin();
-        return 0;
-   }
-</syntaxhighlight>
+  int cont = 0;
+  int cont1= 0;
+  int encendido = 0;
+  brazo_fer::Servos move, on, vel;
-===Carga en gazebo===
+  void home()
+  {
+	if (cont1 == 0)
+	{
+		ros::NodeHandle n;
-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:
+		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);
-<syntaxhighlight lang="xml" enclose="div">
+		ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
-<launch>
+		brazo_fer::WriteServos home;
-  <include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
+		::move.base = 511;
+		::move.arti1 = 511;
-  <param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_fer_modelo)/urdf/myrabot.urdf.xacro'" />
+		::move.arti2 = 511;
+		::move.arti3 = 511;
-  <node name="remap_joint_states" pkg="myrabot_fer_modelo" type="remap_joint_states" />
+		::move.pinza = 511;
-  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+		::on.base = 1;
+		::on.arti1 = 1;
-  <node name="spawn_brazo" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model MYRAbot" respawn="false" output="screen" />
+		::on.arti2 = 1;
+		::on.arti3 = 1;
-  <rosparam file="$(find brazo_fer_modelo)/controller.yaml" command="load"/>
+		::on.pinza = 1;
-   <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" />
+		::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;
+	}
+   }
-   <node name="modelo_brazo" pkg="brazo_fer_modelo" type="modelo_brazo" />
+   void inicio(const std_msgs::Bool& inicio)
+  {
-  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
+	if (inicio.data == true && ::cont == 0)
-    <param name="freq" value="30.0"/>
+	{
-    <param name="sensor_timeout" value="1.0"/>
+		::encendido = 1;
-    <param name="publish_tf" value="true"/>
+		::cont = 1;
-    <param name="odom_used" value="true"/>
+	}
-    <param name="imu_used" value="false"/>
+	else if (inicio.data == true && ::cont == 1)
-    <param name="vo_used" value="false"/>
+	{
-    <param name="output_frame" value="odom"/>
+		::encendido = 0;
-   </node>
+		::cont = 0;
+	}
+  }
+  void posicion(const brazo_fer::ReadServos& posicion)
+   {
+	ros::NodeHandle n;
-  <node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager" respawn="true">
+  	ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
-    <param name="max_rate" value="20.0"/>
+  	ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
-    <remap from="cloud_in" to="/camera/depth/points"/>
-    <remap from="cloud_out" to="cloud_throttled"/>
+	brazo_fer::Servos p = posicion.posicion;
-  </node>
+	brazo_fer::WriteServos mover_brazo;
-  <node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
+	home();
-    <param name="output_frame_id" value="/camera_depth_frame"/>
-    <param name="min_height" value="-0.15"/>
+	if (::encendido == 1)
-    <param name="max_height" value="0.15"/>
+	{
-    <remap from="cloud" to="/cloud_throttled"/>
+		geometry_msgs::Twist mover_base;
-  </node>
+		mover_base.linear.x = 0.1;
-  <node pkg="nodelet" type="nodelet" name="kinect_laser_narrow" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
+		mover_base.angular.z = 0.3;
-    <param name="output_frame_id" value="/camera_depth_frame"/>
-    <param name="min_height" value="-0.025"/>
+		move_base_pub_.publish(mover_base);
-    <param name="max_height" value="0.025"/>
-    <remap from="cloud" to="/cloud_throttled"/>
+		if (::cont1 ==  1)
-    <remap from="scan" to="/narrow_scan"/>
+		{
-  </node>
+			::move.base = 718;
+			::cont1 = 2;
-</launch>
+			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;
+			}
+		}
+	}
+   }
-</syntaxhighlight>
+  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 lanzar el ''launcher'' ejecutaremos el siguiente comando en un terminal:
+</syntaxhighlight>
-<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_gazebo.launch</syntaxhighlight>
+Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro ''package'':
-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>rosbuild_add_executable(vuelta_al_ruedo src/vuelta_al_ruedo.cpp)</syntaxhighlight>
-[[file:modelo_MYRAbot_gazebo.png|thumb|500px|center|Modelo MYRAbot en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
+Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal:
-===Programa de prueba (Vuela al ruedo)===
+<syntaxhighlight>
+roscd myrabot_fer
-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.
+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:
-Dentro de este ''package'' vamos a crear un archivo llamado "vuelta_al_ruedo.cpp" en el directorio "src" con el siguiente contenido:
+<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_gazebo.launch</syntaxhighlight>
-<syntaxhighlight lang="cpp" anclose="div">
+Para ejecutar el programa que hemos creado, en otro terminal ejecutaremos el siguiente comando:
-  #include "ros/ros.h"
+<syntaxhighlight>rosrun myrabot_fer vuelta_al_ruedo</syntaxhighlight>
-  #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;
+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:
-  int cont1= 0;
-  int encendido = 0;
-  brazo_fer::Servos move, on, vel;
-  void home()
+<syntaxhighlight>rostopic pub start_button std_msgs/Bool '{data: true}' --once</syntaxhighlight>
-  {
-	if (cont1 == 0)
-	{
-		ros::NodeHandle n;
-		ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
+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:
-		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);
+<center><videoflash>IY63zJgQYGI</videoflash></center>
-		brazo_fer::WriteServos home;
+<center>[http://www.youtube.com/watch?v=IY63zJgQYGI Ver vídeo en YouTube]</center>
-		::move.base = 511;
+==Ejecución del programa (entre la cerveza y la cola, la plataforma) escoja==
-		::move.arti1 = 511;
-		::move.arti2 = 511;
+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.
-		::move.arti3 = 511;
-		::move.pinza = 511;
+===Creación de modelos de objetos===
-		::on.base = 1;
+====Mesa====
-		::on.arti1 = 1;
-		::on.arti2 = 1;
+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).
-		::on.arti3 = 1;
-		::on.pinza = 1;
+[[file:modelo_mesa.png|thumb|200px|center|Modelo mesa en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
-		::vel.base = 400;
+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:
-		::vel.arti1 = 400;
-		::vel.arti2 = 400;
+<syntaxhighlight lang="xml" enclose="div">
-		::vel.arti3 = 400;
-		::vel.pinza = 400;
+<?xml version="1.0"?>
-		home.posicion = ::move;
+<robot name="mesa" xmlns:xacro="http://www.ros.org/wiki/xacro">
-		home.par = ::on;
-		home.velocidad = ::vel;
+<link name="world" />
-		move_arm_pub_.publish(home);
+<joint name="mesa" type="floating">
-		hand_pub_.publish(home);
+   <parent link="world"/>
+   <child link="mesa_link"/>
-		::cont1 = 1;
+   <origin xyz="0 0 0" rpy="0 0 0" />
+</joint>
-	}
-  }
-  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);
+<link name="mesa_link">
-  	ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+  <visual>
+    <geometry>
-	brazo_fer::Servos p = posicion.posicion;
+      <box size="2 0.75 0.03" />
-	brazo_fer::WriteServos mover_brazo;
+    </geometry>
+    <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
-	home();
+      <material name="wood">
+		<color rgba="${205/255} ${133/255} ${63/255} 1" />
-	if (::encendido == 1)
+    </material>
-	{
+  </visual>
-		geometry_msgs::Twist mover_base;
+  <collision>
+    <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
-		mover_base.linear.x = 0.1;
+    <geometry>
-		mover_base.angular.z = 0.3;
+      <box size="2 0.75 0.03" />
+    </geometry>
-		move_base_pub_.publish(mover_base);
+  </collision>
+  <inertial>
-		if (::cont1 ==  1)
+    <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
-		{
+    <mass value="5" />
-			::move.base = 718;
+      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
-			::cont1 = 2;
+  </inertial>
-			mover_brazo.posicion = ::move;
+</link>
-			mover_brazo.par = ::on;
-			mover_brazo.velocidad = ::vel;
-			move_arm_pub_.publish(mover_brazo);
+<xacro:macro name="pata" params="id refx refy">
-		}
+<joint name="pata_${id}" type="fixed">
-		else if ((p.base-110) < ::move.base && ::move.base < (p.base+110))
+  <parent link="mesa_link"/>
-		{
+  <child link="pata_${id}_link"/>
-			if (::cont1 ==  2)
+  <origin xyz="${refx*((2/2)-0.045)} ${refy*((0.75/2)-0.045)} 0" rpy="0 0 0" />
-			{
+</joint>
-				::move.base = 304;
-				::cont1 = 3;
+<link name="pata_${id}_link">
-				mover_brazo.posicion = ::move;
+  <visual>
-				mover_brazo.par = ::on;
+    <geometry>
-				mover_brazo.velocidad = ::vel;
+      <cylinder radius="0.025" length="0.71" />
-				move_arm_pub_.publish(mover_brazo);
+    </geometry>
-			}
+    <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
-			else
+    <material name="black">
-			{
+      <color rgba="0 0 0 1" />
-				::cont1 = 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"/>
-  int main(int argc, char **argv)
+<gazebo reference="mesa_link">
-  {
+  <material>Gazebo/LightWood</material>
+  <mu1>10</mu1>
-	ros::init(argc, argv, "vuelta_al_ruedo");
+  <mu2>10</mu2>
+   <selfCollide>true</selfCollide>
-	ros::NodeHandle n;
+  <turnGravityOff>false</turnGravityOff>
+</gazebo>
-  	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>
+<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>
-Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro ''package'':
+<xacro:gazebo_pata id="1"/>
+<xacro:gazebo_pata id="2"/>
+<xacro:gazebo_pata id="3"/>
+<xacro:gazebo_pata id="4"/>
-<syntaxhighlight>rosbuild_add_executable(vuelta_al_ruedo src/vuelta_al_ruedo.cpp)</syntaxhighlight>
+</robot>
-Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal:
+</syntaxhighlight>
-<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.
-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:
+====Creación de nuevos materiales para gazebo====
-<syntaxhighlight>roslaunch myrabot_fer_modelo myrabot_gazebo.launch</syntaxhighlight>
+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]:
-Para ejecutar el programa que hemos creado, en otro terminal ejecutaremos el siguiente comando:
+<syntaxhighlight lang="xml">
-<syntaxhighlight>rosrun myrabot_fer vuelta_al_ruedo</syntaxhighlight>
+ <export>
+     <gazebo gazebo_media_path="${prefix}" />
+ </export>
-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>
-<syntaxhighlight>rostopic pub start_button std_msgs/Bool '{data: true}' --once</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:
-<center><videoflash>IY63zJgQYGI</videoflash></center>
+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:
-<center>[http://www.youtube.com/watch?v=IY63zJgQYGI Ver vídeo en YouTube]</center>
+<syntaxhighlight>
-==Ejecución del programa (entre la cerveza y la cola, la plataforma) escoja==
+material material_fer/NOMBRE_DEL_NUEVO_MATERIAL
+{
+	technique
+	{
+		pass
+		{
+                        lighting off
-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.
-===Creación de modelos de objetos===
+                        ambient 1 1 1 1
+			diffuse 1 1 1 1
-====Mesa====
+			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
+			}
+		}
+	}
+}
+</syntaxhighlight>
-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).
+====Latas====
-[[file:modelo_mesa.png|thumb|200px|center|Modelo mesa en [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
+=====Lata de cerveza=====
-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:
+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">
@@ Line 2,256: / Line 2,378: @@
 <?xml version="1.0"?>
-<robot name="mesa" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<robot name="lata_cerveza" xmlns:xacro="http://www.ros.org/wiki/xacro">
 <link name="world" />
-<joint name="mesa" type="floating">
+<joint name="lata" type="floating">
    <parent link="world"/>
-   <child link="mesa_link"/>
+   <child link="lata_link"/>
    <origin xyz="0 0 0" rpy="0 0 0" />
 </joint>
-<link name="mesa_link">
+<link name="lata_link">
    <visual>
      <geometry>
-       <box size="2 0.75 0.03" />
+       <cylinder radius="0.0325" length="0.1115" />
      </geometry>
-     <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
+     <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
-      <material name="wood">
+    <material name="red">
-		<color rgba="${205/255} ${133/255} ${63/255} 1" />
+      <color rgba="1 0 0 1" />
      </material>
    </visual>
    <collision>
-     <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
+     <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
      <geometry>
-       <box size="2 0.75 0.03" />
+       <cylinder radius="0.0325" length="0.1115" />
      </geometry>
    </collision>
    <inertial>
-     <origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
+     <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
-     <mass value="5" />
+     <mass value="0.01" />
-       <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
+       <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>
-<xacro:macro name="pata" params="id refx refy">
+<link name="lata_top_link">
-<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" />
+       <cylinder radius="0.0325" length="0.0015" />
      </geometry>
-     <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
+     <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
-     <material name="black">
+     <material name="grey">
-       <color rgba="0 0 0 1" />
+       <color rgba="0.8 0.8 0.8 1" />
      </material>
    </visual>
    <collision>
-     <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
+     <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
      <geometry>
-       <cylinder radius="0.025" length="0.71" />
+       <cylinder radius="0.0325" length="0.0015" />
      </geometry>
    </collision>
    <inertial>
-     <origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
+     <origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
-     <mass value="3" />
+     <mass value="0.001" />
-       <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
+       <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>
 </link>
-</xacro:macro>
-<xacro:pata id="1" refx="1" refy="1"/>
+<joint name="lata_bottom" type="fixed">
-<xacro:pata id="2" refx="1" refy="-1"/>
+  <parent link="lata_link"/>
-<xacro:pata id="3" refx="-1" refy="-1"/>
+  <child link="lata_bottom_link"/>
-<xacro:pata id="4" refx="-1" refy="1"/>
+  <origin xyz="0 0 0" rpy="0 0 0" />
+</joint>
-<gazebo reference="mesa_link">
+<link name="lata_bottom_link">
-   <material>Gazebo/LightWood</material>
+   <visual>
-   <mu1>10</mu1>
+    <geometry>
-  <mu2>10</mu2>
+      <cylinder radius="0.0325" length="0.007" />
-  <selfCollide>true</selfCollide>
+    </geometry>
-   <turnGravityOff>false</turnGravityOff>
+    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
-</gazebo>
+    <material name="grey">
+      <color rgba="0.8 0.8 0.8 1" />
-<xacro:macro name="gazebo_pata" params="id">
+    </material>
-<gazebo reference="pata_${id}_link">
+   </visual>
-   <material>PR2/Black</material>
+  <collision>
-   <mu1>0.9</mu1>
+    <origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
-   <mu2>0.9</mu2>
+    <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>
-</xacro:macro>
-<xacro:gazebo_pata id="1"/>
+<gazebo reference="lata_top_link">
-<xacro:gazebo_pata id="2"/>
+  <material>PR2/Grey0</material>
-<xacro:gazebo_pata id="3"/>
+  <mu1>100</mu1>
-<xacro:gazebo_pata id="4"/>
+  <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>
@@ Line 2,353: / Line 2,497: @@
 </syntaxhighlight>
-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:
+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>
-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.
+material material_fer/Amstel
-====Creación de nuevos materiales en gazebo====
-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:
-<syntaxhighlight>
-material Gazebo/NOMBRE_DEL_NUEVO_MATERIAL
 {
 	technique
@@ Line 2,389: / Line 2,521: @@
 			texture_unit
 			{
-				texture NUESTRO_ARCHIVO_DE_IMAGEN
+				texture amstel.png
 				tex_coord_set 0
 				colour_op modulate
@@ Line 2,401: / Line 2,533: @@
 </syntaxhighlight>
-====Latas====
+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 cerveza
+=====Lata de cola=====
-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:
+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">
@@ Line 2,411: / Line 2,547: @@
 <?xml version="1.0"?>
-<robot name="lata_cerveza" xmlns:xacro="http://www.ros.org/wiki/xacro">
+<robot name="lata_cola" xmlns:xacro="http://www.ros.org/wiki/xacro">
 <link name="world" />
@@ Line 2,503: / Line 2,639: @@
 <gazebo reference="lata_link">
-   <material>Gazebo/Amstel</material>
+   <material>material_fer/CocaCola</material>
-   <mu1>100</mu1>
+   <mu1>10</mu1>
-   <mu2>100</mu2>
+   <mu2>10</mu2>
    <selfCollide>true</selfCollide>
    <turnGravityOff>false</turnGravityOff>
@@ Line 2,512: / Line 2,648: @@
 <gazebo reference="lata_top_link">
    <material>PR2/Grey0</material>
-   <mu1>100</mu1>
+   <mu1>10</mu1>
-   <mu2>100</mu2>
+   <mu2>10</mu2>
    <selfCollide>true</selfCollide>
    <turnGravityOff>false</turnGravityOff>
@@ Line 2,520: / Line 2,656: @@
 <gazebo reference="lata_bottom_link">
    <material>PR2/Grey0</material>
-   <mu1>100</mu1>
+   <mu1>10</mu1>
-   <mu2>100</mu2>
+   <mu2>10</mu2>
    <selfCollide>true</selfCollide>
    <turnGravityOff>false</turnGravityOff>
@@ Line 2,530: / Line 2,666: @@
 </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 "Gazebo.material":
+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/Amstel
+material Gazebo/CocaCola
 {
 	technique
@@ Line 2,554: / Line 2,690: @@
 			texture_unit
 			{
-				texture amstel.png
+				texture coca_cola.png
 				tex_coord_set 0
 				colour_op modulate
@@ Line 2,568: / Line 2,704: @@
 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]]]
+[[file:coca_cola.png|thumb|200px|center|Textura lata de cola]]
-* Lata de cola
+[[file:modelo_lata_cola.png|thumb|200px|center|Lata de cola [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
-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 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 lang="xml" enclose="div">
+<syntaxhighlight>
+roscd objetos_fer_modelos
+cd urdf
+rosrun xacro xacro.py OBJETO.xacro > OBJETO.urdf
+</syntaxhighlight>
-<?xml version="1.0"?>
+===Ejecución===
-<robot name="lata_cola" xmlns:xacro="http://www.ros.org/wiki/xacro">
+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:
-<link name="world" />
+<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" />
-<joint name="lata" type="floating">
+  <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" />
-  <parent link="world"/>
-  <child link="lata_link"/>
-  <origin xyz="0 0 0" rpy="0 0 0" />
-</joint>
-<link name="lata_link">
+   <include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>
-   <visual>
-    <geometry>
+</launch>
-      <cylinder radius="0.0325" length="0.1115" />
-    </geometry>
+</syntaxhighlight>
-    <origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
-    <material name="red">
+Para iniciar el ''launcher'' simplemente debemos ejecutar el siguiente comando en un terminal:
-      <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">
+<syntaxhighlight>roslaunch myrabot_fer myrabot_latas.launch</syntaxhighlight>
-  <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">
+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:
-  <visual>
-    <geometry>
+<syntaxhighlight>roslaunch brazo_fer escoja.launch</syntaxhighlight>
-      <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">
+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:
-  <parent link="lata_link"/>
-  <child link="lata_bottom_link"/>
-  <origin xyz="0 0 0" rpy="0 0 0" />
-</joint>
-<link name="lata_bottom_link">
+<syntaxhighlight>rostopic pub selected_object std_msgs/Int16 1 --once</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">
+Si todo va bien el resultado será similar al que se muestra en el siguiente vídeo:
-  <material>Gazebo/CocaCola</material>
-  <mu1>10</mu1>
-  <mu2>10</mu2>
-  <selfCollide>true</selfCollide>
-  <turnGravityOff>false</turnGravityOff>
-</gazebo>
-<gazebo reference="lata_top_link">
+<center><videoflash>lp2FUFlsFV4</videoflash></center>
-  <material>PR2/Grey0</material>
-  <mu1>10</mu1>
-  <mu2>10</mu2>
-  <selfCollide>true</selfCollide>
-  <turnGravityOff>false</turnGravityOff>
-</gazebo>
-<gazebo reference="lata_bottom_link">
+<center>[http://www.youtube.com/watch?v=lp2FUFlsFV4 Ver vídeo en YouTube]</center>
-  <material>PR2/Grey0</material>
-  <mu1>10</mu1>
-  <mu2>10</mu2>
-  <selfCollide>true</selfCollide>
-  <turnGravityOff>false</turnGravityOff>
-</gazebo>
-</robot>
+==Ejecución del programa seguir objetos==
-</syntaxhighlight>
+===Programa para mover objetos en gazebo usando el teclado===
-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 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>
+<syntaxhighlight lang="cpp" enclose="div">
-material Gazebo/CocaCola
+  #include "ros/ros.h"
-{
+  #include "gazebo/ModelState.h"
-	technique
+  #include <signal.h>
-	{
+  #include <termios.h>
-		pass
+  #include <stdio.h>
-		{
-                        lighting off
+  #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;
-                        ambient 1 1 1 1
+double retardo = 0.1;
-			diffuse 1 1 1 1
-			specular 0 0 0 0
-			emissive 0 0 0
+gazebo::ModelState objeto;
+void quit(int sig)
+{
+  tcsetattr(0, TCSANOW, &cooked);
+  ros::shutdown();
+  exit(0);
+}
-			alpha_rejection greater 128
+void callback(const ros::TimerEvent&)
-			depth_write on
+{
+	ros::NodeHandle n;
+  	ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);
-			texture_unit
+        std::string modelo_objeto;
-			{
-				texture coca_cola.png
-				tex_coord_set 0
-				colour_op modulate
-				rotate 180
-				scale 1 1
-			}
-		}
-	}
-}
-</syntaxhighlight>
+        ros::NodeHandle nh("~");
-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]:
+        nh.param("modelo_objeto", modelo_objeto, std::string("Lata_amstel"));
+	ros::Time anterior;
-[[file:coca_cola.png|thumb|200px|center|Textura lata de cola]]
+	if (::cont == 0)
+        {
-[[file:modelo_lata_cola.png|thumb|200px|center|Lata de cola [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
+		anterior = ros::Time::now();
-===Ejecución===
+		::objeto.model_name = modelo_objeto;
-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" />
+		::objeto.reference_frame = "world";
-  <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" />
+		::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;
-  <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" />
+		object_pub_.publish(::objeto);
+	}
+  char c;
-   <include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>
+   // 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);
-</launch>
+  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");
-</syntaxhighlight>
+    while (ros::ok())
-Para iniciar el ''launcher'' simplemente debemos ejecutar el siguiente comando en un terminal:
+    {
-<syntaxhighlight>roslaunch myrabot_fer myrabot_latas.launch</syntaxhighlight>
+    // get the next event from the keyboard
-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:
+    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;
-<syntaxhighlight>roslaunch brazo_fer escoja.launch</syntaxhighlight>
+			anterior = ros::Time::now();
+		}
-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:
+      }
+      if (c == KEYCODE_Ymas)
-<syntaxhighlight>rostopic pub selected_object std_msgs/Int16 1 --once</syntaxhighlight>
+      {
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
-Si todo va bien el resultado será similar al que se muestra en el siguiente vídeo:
+		{
+			::objeto.pose.position.z = ::objeto.pose.position.z + 0.005;
-<center><videoflash>lp2FUFlsFV4</videoflash></center>
+			anterior = ros::Time::now();
-<center>[http://www.youtube.com/watch?v=lp2FUFlsFV4 Ver vídeo en YouTube]</center>
+		}
+      }
-==Ejecución del programa seguir objetos==
+      if (c == KEYCODE_Ymenos)
+      {
-===Programa para mover objetos en gazebo usando el teclado===
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
+		{
-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:
+			::objeto.pose.position.z = ::objeto.pose.position.z - 0.005;
-<syntaxhighlight lang="cpp" enclose="div">
+			anterior = ros::Time::now();
+		}
-  #include "ros/ros.h"
+      }
-  #include "gazebo/ModelState.h"
+      if (c == KEYCODE_Zmas)
-  #include <signal.h>
+      {
-  #include <termios.h>
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
-  #include <stdio.h>
+		{
+			::objeto.pose.position.y = ::objeto.pose.position.y + 0.005;
-  #define PI 3.14159265
-  #define L1 104
+			anterior = ros::Time::now();
-  #define L2 104
+		}
-  #define Lp 60
+      }
+      if (c == KEYCODE_Zmenos)
-  #define KEYCODE_Xmas 0x43
+      {
-  #define KEYCODE_Xmenos 0x44
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
-  #define KEYCODE_Ymas 0x41
+		{
-  #define KEYCODE_Ymenos 0x42
+			::objeto.pose.position.y = ::objeto.pose.position.y - 0.005;
-  #define KEYCODE_Zmas 0x77
-  #define KEYCODE_Zmenos 0x73
+			anterior = ros::Time::now();
-  #define KEYCODE_XgiroMas 0x61
+		}
-  #define KEYCODE_XgiroMenos 0x64
+      }
-  #define KEYCODE_YgiroMas 0x65
+      if (c == KEYCODE_XgiroMas)
-  #define KEYCODE_YgiroMenos 0x71
+      {
+		if (ros::Time::now() > anterior + ros::Duration(::retardo))
-struct termios cooked, raw;
+		{
+			::objeto.pose.orientation.x = ::objeto.pose.orientation.x + 0.1;
-int cont = 0;
+			anterior = ros::Time::now();
-double retardo = 0.1;
+		}
+      }
+      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;
-gazebo::ModelState objeto;
+			anterior = ros::Time::now();
+		}
-void quit(int sig)
+      }
-{
-  tcsetattr(0, TCSANOW, &cooked);
+      object_pub_.publish(::objeto);
-  ros::shutdown();
-  exit(0);
+	}
 }
-void callback(const ros::TimerEvent&)
-{
-	ros::NodeHandle n;
+  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);
-        std::string modelo_objeto;
-        ros::NodeHandle nh("~");
+	signal(SIGINT,quit);
-        nh.param("modelo_objeto", modelo_objeto, std::string("Lata_amstel"));
+	ros::Timer timer = n.createTimer(ros::Duration(1), callback);
-	ros::Time anterior;
+	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:
-	if (::cont == 0)
+<syntaxhighlight>
-        {
+roscd objetos_fer_modelos
-		anterior = ros::Time::now();
+make
+</syntaxhighlight>
-		::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);
+===Ejecución===
-	}
-  char c;
-  // get the console in raw mode
+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:
-  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("");
+<syntaxhighlight lang="xml" enclose="div">
-   puts("#####################################################");
-  puts("                   CONTROLES OBJETO");
+<launch>
-   puts("#####################################################");
-  puts("");
+   <param name="/use_sim_time" value="true" />
-  puts("Flecha arriba:_______ Subir objeto");
-  puts("Flecha abajo:________ Bajar objeto");
+   <node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>
-  puts("Flecha izquierda:____ Desplazar objeto a la izquierda");
-   puts("Flecha derecha:______ Desplazar objeto a la derecha");
+   <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" />
-   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");
+  <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" />
-    while (ros::ok())
-    {
+  <include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>
+</launch>
-    // 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();
+</syntaxhighlight>
-		}
-      }
+Para iniciar el ''launcher'' simplemente debemos ejecutar el siguiente comando en un terminal:
-      if (c == KEYCODE_Ymas)
-      {
+<syntaxhighlight>roslaunch myrabot_fer myrabot_lata.launch</syntaxhighlight>
-		if (ros::Time::now() > anterior + ros::Duration(::retardo))
-		{
+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:
-			::objeto.pose.position.z = ::objeto.pose.position.z + 0.005;
+<syntaxhighlight>roslaunch brazo_fer seguir.launch</syntaxhighlight>
-			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();
+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:
-		}
-      }
-      object_pub_.publish(::objeto);
-	}
+<syntaxhighlight>rostopic pub selected_object std_msgs/Int16 1 --once</syntaxhighlight>
-}
-  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);
+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:
-	signal(SIGINT,quit);
+<syntaxhighlight>rosrun objetos_fer_modelos mover_objetos _modelo_objeto:=lata_amstel</syntaxhighlight>
-	ros::Timer timer = n.createTimer(ros::Duration(1), callback);
-	ros::spin();
-        return 0;
-  }
-</syntaxhighlight>
+Si todo va bien el resultado será similar al que se muestra en el vídeo siguiente:
-Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro ''package'':
+<center><videoflash>kLfgfHo8OnI</videoflash></center>
-<syntaxhighlight>rosbuild_add_executable(mover_objetos src/mover_objetos.cpp)</syntaxhighlight>
+<center>[http://www.youtube.com/watch?v=kLfgfHo8OnI Ver vídeo en YouTube]</center>
-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>
-<center>[http://www.youtube.com/watch?v=kLfgfHo8OnI Ver vídeo en YouTube]</center>
 -->
 [[Mobile manipulation | < go back to main]]