Latest revision as of 11:10, 29 September 2014

< go back to main

Related articles	Other articles
MYRAbot's arm control (bioloid+arduino) Objects recognition and position calculation (webcam) MYRAbot's arm model for simulation (urdf+gazebo) Integration of MYRAbot in moveIt! (gazebo+moveIt!) Voice control (sphinx+festival) MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)	CeRVaNTeS' arm and base control (maxon+epos2) CeRVaNTeS' arm model for simulation (urdf+gazebo) CeRVaNTeS model for simulation (urdf+gazebo) Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!) CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)

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

We will create a file named "kinect.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">
	
  <property name="M_PI" value="3.14159"/>

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

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

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

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

  </link>

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

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

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

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

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

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

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

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

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

</robot>

URDF webcam

We will create a file named "webcam.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="webcam" params="parent *origin">
  
  <joint name="webcam_1" type="fixed">
	<insert_block name="origin" />
    <parent link="${parent}" />
    <child link="e_webcam_1_link" />
  </joint>

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

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

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

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

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

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

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

</robot>

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

URDF ultrasound sensor

Creation of a gazebo plugin

we will create a new package with the necessary dependences for our plugins (gazebo sensor_msgs roscpp). We will execute the next commands in a terminal:

cd ~/ros_workspace
roscreate-pkg myrabot_gazebo_plugins gazebo sensor_msgs roscpp

We will create a file named "gazebo_ros_sonar.cpp" within the folder "src" of the created package with the content that is shown below:

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

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

using namespace gazebo;

GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_sonar", GazeboRosSonar)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

We will create a file named "gazebo_ros_sonar.h" within the path "include/myrabot_gazebo_plugins/" of the created package with the content that is shown below:

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


#include <ros/ros.h>

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

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


namespace gazebo
{

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

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

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

private:
  gazebo::sensors::RaySensorPtr sensor_;

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

  sensor_msgs::Range range_;

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

  gazebo::physics::WorldPtr parent_;

  common::Time last_time;

  gazebo::event::ConnectionPtr updateConnection;


};

} // namespace gazebo

We have to add the next lines at the end within the file CMakeLists.txt of the plugins package:

rosbuild_add_library(gazebo_ros_sonar src/gazebo_ros_sonar.cpp)

Also, we have to add the next lines before the tag </package> within the file "manifest.xml" of the plugins package:

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

We will execute the next commands in a terminal in order to compile the plugin:

roscd myrabot_gazebo_plugins
make

URDF

We will create a file named "ultrasonidos.xacro" within the folder "urdf" of our robot model 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="ultrasonidos" params="id parent d_centro *origin">

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

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

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

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

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

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

        <updateRate>20</updateRate>

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

Assembled of MYRAbot model

We have to join all the parts that form the MYRAbot. We have to set the parent link and origin of each part. We will create a file named "myrabot.urdf.xacro" within the folder "urdf" of our package with the content that is shown below:

<?xml version="1.0"?>

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

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

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

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

</robot>

Display of the model in rviz

We will create a file named "myrabot_rviz.launch" within the folder "launch" of our package with the content that is shown below, in order to test in rviz the correct assembly of the model and the correct performance of the joints.

<launch>

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

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

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

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

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

</launch>

We will execute the next command in a terminal in order to start the launcher:

roslaunch myrabot_fer_modelo myrabot_rviz.launch

The launcher starts rviz and the GUI of joint_state_publisher. We have to set in "Global Options" the field "Fixed Frame" to "/base_footprint" link. We can change the position of the joints using the sliders of the GUI of joint_state_publisher. We must obtain something similar to the following:

MYRAbot model in rviz and joint_state_publisher interface

Load of the model in gazebo

Using an onlytopic for joint states

We will create a file named "remap_joint_states.cpp" within the folder "src" of our package with the content that is shown below, in order to publish the joint states of the arm (brazo/joint_states) and the mobile base (joint_states) in a only topic (joint_states_myrabot):

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

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

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

Load in gazebo

We will create a file named "myrabot_gazebo.launch" within the folder "launch" of our package with the content that is shown below in order to start gazebo and to load the model and the controllers:

<launch>

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

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

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

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

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

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


  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="publish_tf" value="true"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="false"/>
    <param name="vo_used" value="false"/>
    <param name="output_frame" value="odom"/>
  </node>

  <node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager" respawn="true">
    <param name="max_rate" value="20.0"/>
    <remap from="cloud_in" to="/camera/depth/points"/>
    <remap from="cloud_out" to="cloud_throttled"/>
  </node>

  <node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
    <param name="output_frame_id" value="/camera_depth_frame"/>
    <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>

We will execute the next command in a terminal in order to start the launcher:

roslaunch myrabot_fer_modelo myrabot_gazebo.launch

We can see the MYRAbot model in vertical position placed in the point xyz=(0 0 0) of a empty world in gazebo.

MYRAbot model in gazebo

Test program (around the bullring)

We have made a little program to test the performance of the MYRAbot model. When we push the start button the model revolves in circle and the arm rotates from side to side in vertical position. First, we will create a new package with the necessary dependences for our programs (brazo_fer geometry_msgs std_msgs nav_msgs sensor_msgs image_transport roscpp). We will execute the next commands in a terminal:

cd ~/ros_workspace
roscreate-pkg myrabot_fer brazo_fer geometry_msgs std_msgs nav_msgs sensor_msgs image_transport roscpp

We will create a file named "vuelta_al_ruedo.cpp" within the folder "src" of the created package with the content that is shown below:

  #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;
  }

We have to add the next line to the file "CMakeLists.txt" of our package in order to compile and create the executable file:

rosbuild_add_executable(vuelta_al_ruedo src/vuelta_al_ruedo.cpp)

We will execute the next commands in a terminal in order to compile and create the executable file:

roscd myrabot_fer
make

We will execute the next command in a terminal in order to launch gazebo with the robot model:

roslaunch myrabot_fer_modelo myrabot_gazebo.launch

We will execute the next command in a new terminal in order to launch the test program:

rosrun myrabot_fer vuelta_al_ruedo

We start the program publishing "true" value in the topic "start_button". We will execute the next command in a new terminal in order to simulate the start button of the real robot:

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

The robot model will start to revolve in circle and the arm to rotate from side to side in vertical position. We can see this in the next video:

<videoflash>IY63zJgQYGI</videoflash> Watch in YouTube

Execution of the program (between the beer and the cola, the platform) choose

We need to create the objects' models (cans and table) in order to execute the program Choose in the gazebo simulator using the MYRAbot model.

Creation of the objects' models

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

cd ~/ros_workspace
roscreate-pkg objetos_fer_modelos geometry_msgs gazebo urdf roscpp

Table

We have crated a table with the dimensions 200x75x74 cm (length x width x height). We will create a file named "mesa.xacro" within the folder "urdf" of the created package with the content that is shown below:

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

Table model in gazebo

We have used the default materials included in gazebo for the table, but we need create new materials with texture for the labels of the cans.

Creation of new materials for gazebo

First, we have to add the next lines within the file "manifest.xml" of the created package in order to add the package path to the gazebo path:

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

gazebo uses the graphics engine OGRE. We will create a file named "myrabot_fer.material" within the objects package in the relative path "Media/materials/scripts/" with the content that is shown below in order to define a new material with texture:

material material_fer/NEW_MATERIAL_NAME
{
	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 OUR_IMAGE_FILE
				tex_coord_set 0
				colour_op modulate
				rotate 180
				scale 1 1
			}
		}
	}
}

The image file must be in the relative path "Media/materials/textures/" within the objects package.

Cans

Can of beer

We will create a file named "lata_cerveza.xacro" within the folder "urdf" of the objects package with the content that is shown below:

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

We have to add the next lines within the file "myrabot_fer.material" in order to add the new material named "Amstel":

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

As follows, we can see the image file used for the texture and a screenshot of the can model in gazebo:

Texture of the Can of beer

Can of beer in gazebo

Can of cola

We will create a file named "lata_cola.xacro" within the folder "urdf" of the objects package with the content that is shown below:

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

We have to add the next lines within the file "myrabot_fer.material" in order to add the new material named "CocaCola":

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

As follows, we can see the image file used for the texture and a screenshot of the can model in gazebo:

Texture of the can of cola

Can of cola in gazebo

Note: We have to convert the .xacro files into .urdf in order to load the models in gazebo. We will execute the next commands in a terminal:

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

Execution

We will create a file named "myrabot_latas.launch" within the "myrabot_fer" package with the content that is shown below in order to load the models of the objects and the robot in gazebo:

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

We will execute in a terminal the next commad in order to launch gazebo with the robot model and the objects:

roslaunch myrabot_fer myrabot_latas.launch

We will execute in a new terminal the next command in order to start the program Choose:

roslaunch brazo_fer escoja.launch

We will execute the next command in a terminal in order to publish the identifier of the selected object in the topic "selected_object":

rostopic pub selected_object std_msgs/Int16 1 --once

The arm points the selected object and after a time the arm returns to the home position. We can see this in the next video:

<videoflash>lp2FUFlsFV4</videoflash> Watch in YouTube

Execution of the program tracking objects

Program to move objects in gazebo using the keyboard

We will create a new file named "mover_objetos.cpp" within the folder "src" of the objects package with yhe content that is shown below, in order to move an object model (the default object model is Lata_amstel) in gazebo publishing its position in the topic "gazebo/set_model_state":

  #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("Up arrow:_______________________ Move up object");
  puts("Down arrow:_____________________ Move down object");
  puts("Left arrow:_____________________ Move left object"); 
  puts("Right arrow:____________________ Move right object");
  puts("W key:__________________________ Move forward object");
  puts("S key:__________________________ Move backward object");
  puts("A key:__________________________ Tilt + x axis object");
  puts("D key:__________________________ Tilt - x axis object");
  puts("Q key:__________________________ Tilt + y axis object");
  puts("E key:__________________________ Tilt - y axis object");                               

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

We have to add the next line at the end of the file "CMakeLists.txt" of the objects package in order to compile the program and make the executable:

rosbuild_add_executable(mover_objetos src/mover_objetos.cpp)

We will execute the nest commands in a terminal in order to compile the program and make the executable:

roscd objetos_fer_modelos
make

Execution

We will create a file named "myrabot_lata.launch" within the folder "launch" of the "myrabot_fer" package with the content that is shown below, in order to load the models in the corrects positions:

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

We will execute the next command in a terminal in order to start the launcher:

roslaunch myrabot_fer myrabot_lata.launch

We will execute the next command in a new terminal in order to launch the tracking objects program:

roslaunch brazo_fer seguir.launch

We will execute the next command in a new terminal in order to publish the object to tracking in the topic "selected_object":

rostopic pub selected_object std_msgs/Int16 1 --once

We will execute the next command in a new terminal in order to launch the "mover_objetos" program with the parameter "modelo_objeto" equal to "lata_amstel":

rosrun objetos_fer_modelos mover_objetos _modelo_objeto:=lata_amstel

The arm follows the selected object. We can see this in the next video:

<videoflash>kLfgfHo8OnI</videoflash> Watch in YouTube

Related articles	Other articles
MYRAbot's arm control (bioloid+arduino) Objects recognition and position calculation (webcam) MYRAbot's arm model for simulation (urdf+gazebo) Integration of MYRAbot in moveIt! (gazebo+moveIt!) Voice control (sphinx+festival) MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)	CeRVaNTeS' arm and base control (maxon+epos2) CeRVaNTeS' arm model for simulation (urdf+gazebo) CeRVaNTeS model for simulation (urdf+gazebo) Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!) CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)

< go back to main

@@ Line 16: / Line 16: @@
 [[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]]<br/>
 [[Voice control (sphinx+festival)]]<br/>
-[[Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
+[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
 | valign="top" |
-[[CeRVaNTeS's arm model for simulation (urdf+gazebo)]]<br/>
+[[CeRVaNTeS' arm and base control (maxon+epos2)]]<br/>
-[[CeRVaNTeS model for simulation (urdf+gazebo)]]
+[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]]<br/>
+[[CeRVaNTeS model for simulation (urdf+gazebo)]]<br/>
+[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]<br/>
+[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
 |}
@@ Line 27: / Line 30: @@
-==Components of MYRAbot model==
+=Components of MYRAbot model=
 We have started creating a model of the MYRAbot's structure and we have added the mobile base [http://store.irobot.com/family/index.jsp?categoryId=3358508&s=A-ProductAge Roomba], the [http://www.xbox.com/es-ES/Kinect kinect] camera, the [[MYRAbot's arm model for simulation (urdf+gazebo)|arm model]] and the webcam [http://www.logitech.com/es-es/support/5867?crid=405 Logitech Webcam Pro 9000].
@@ Line 37: / Line 40: @@
 roscreate-pkg myrabot_fer_modelo urdf std_msgs sensor_msgs tf roscpp</syntaxhighlight>
-===URDF MYRAbot's structure===
+==URDF MYRAbot's structure==
 We have made the 3D model of the structure with a [http://en.wikipedia.org/wiki/Computer-aided_design CAD] software from which we use the parts with a complex geometry.
@@ Line 675: / Line 678: @@
 </syntaxhighlight>
-===URDF Roomba===
+==URDF Roomba==
 We have used the model of the mobile base [http://store.irobot.com/family/index.jsp?categoryId=2591511&s=A-ProductAge create] of the robot [http://www.turtlebot.com/ turtlebot], because the vacuum [http://store.irobot.com/family/index.jsp?categoryId=3358508&s=A-ProductAge Roomba] has similar sensors and physics. We have to install the ''stacks'' [http://wiki.ros.org/turtlebot turtlebot] and [http://wiki.ros.org/turtlebot_simulator turtlebot_simulator] that contain the robot model and the ''plugins'' for [http://wiki.ros.org/simulator_gazebo?distro=electric 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:
@@ Line 1,178: / Line 1,181: @@
 </syntaxhighlight>
-===URDF kinect===
+==URDF kinect==
 We will create a file named "kinect.xacro" within the folder "urdf" of our ''package'' with the content that is shown below:
@@ Line 1,319: / Line 1,322: @@
 </syntaxhighlight>
-===URDF webcam===
+==URDF webcam==
 We will create a file named "webcam.xacro" within the folder "urdf" of our ''package'' with the content that is shown below:
@@ Line 1,478: / Line 1,481: @@
 * [http://www.fernando.casadogarcia.es/files/webcam_2.stl webcam_2.stl]
-===URDF ultrasound sensor===
+==URDF ultrasound sensor==
-====Creation of a gazebo plugin====
+===Creation of a gazebo plugin===
 we will create a new ''package'' with the necessary dependences for our plugins (gazebo sensor_msgs roscpp). We will execute the next commands in a terminal:
@@ Line 1,760: / Line 1,763: @@
 </syntaxhighlight>
-====URDF====
+===URDF===
 We will create a file named "ultrasonidos.xacro" within the folder "urdf" of our robot model ''package'' with the content that is shown below:
@@ Line 1,859: / Line 1,862: @@
 </syntaxhighlight>
-==Assembled of MYRAbot model==
+=Assembled of MYRAbot model=
 We have to join all the parts that form the MYRAbot. We have to set the parent link and origin of each part. We will create a file named "myrabot.urdf.xacro" within the folder "urdf" of our ''package'' with the content that is shown below:
@@ Line 1,933: / Line 1,936: @@
 </syntaxhighlight>
-===Display of the model in rviz===
+==Display of the model in rviz==
 We will create a file named "myrabot_rviz.launch" within the folder "launch" of our ''package'' with the content that is shown below, in order to test in [http://wiki.ros.org/rviz rviz] the correct assembly of the model and the correct performance of the joints.
@@ Line 1,963: / Line 1,966: @@
 [[file:modelo_MYRAbot_rviz.png|thumb|500px|center|MYRAbot model in [http://wiki.ros.org/rviz rviz] and [http://wiki.ros.org/joint_state_publisher joint_state_publisher] interface]]
-==Load of the model in gazebo==
+=Load of the model in gazebo=
-===Using an only''topic'' for joint states===
+==Using an only''topic'' for joint states==
 We will create a file named "remap_joint_states.cpp" within the folder "src" of our ''package'' with the content that is shown below, in order to publish the joint states of the arm (brazo/joint_states) and the mobile base (joint_states) in a only ''topic'' (joint_states_myrabot):
@@ Line 2,076: / Line 2,079: @@
 </syntaxhighlight>
-===Load in gazebo===
+==Load in gazebo==
 We will create a file named "myrabot_gazebo.launch" within the folder "launch" of our ''package'' with the content that is shown below in order to start [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] and to load the model and the controllers:
@@ Line 2,149: / Line 2,152: @@
 [[file:modelo_MYRAbot_gazebo.png|thumb|500px|center|MYRAbot model in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]]]
-===Test program (around the bullring)===
+==Test program (around the bullring)==
 We have made a little program to test the performance of the MYRAbot model. When we push the start button the model revolves in circle and the arm rotates from side to side in vertical position. First, we will create a new ''package'' with the necessary dependences for our programs ([[Control brazo MYRAbot (bioloid+arduino)|brazo_fer]] geometry_msgs std_msgs nav_msgs sensor_msgs image_transport roscpp). We will execute the next commands in a terminal:
@@ Line 2,330: / Line 2,333: @@
 <center>[http://www.youtube.com/watch?v=IY63zJgQYGI Watch in YouTube]</center>
-==Execution of the program (between the beer and the cola, the platform) choose==
+=Execution of the program (between the beer and the cola, the platform) choose=
 We need to create the objects' models (cans and table) in order to execute the program [[Objects recognition and position calculation (webcam)#To point objects (Choose)|Choose]] in the [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] simulator using the MYRAbot model.
-===Creation of the objects' models===
+==Creation of the objects' models==
 First, we will create a new ''package'' with the necessary dependences for our programs (geometry_msgs gazebo urdf roscpp). We will execute the next commands in a terminal:
@@ Line 2,342: / Line 2,345: @@
 roscreate-pkg objetos_fer_modelos geometry_msgs gazebo urdf roscpp</syntaxhighlight>
-====Table====
+===Table===
 We have crated a table with the dimensions 200x75x74 cm (length x width x height). We will create a file named "mesa.xacro" within the folder "urdf" of the created ''package'' with the content that is shown below:
@@ Line 2,451: / Line 2,454: @@
 We have used the default materials included in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] for the table, but we need create new materials with texture for the labels of the cans.
-====Creation of new materials for gazebo====
+===Creation of new materials for gazebo===
 First, we have to add the next lines within the file "manifest.xml" of the created ''package'' in order to add the ''package'' path to the [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] path:
@@ Line 2,501: / Line 2,504: @@
 The image file must be in the relative path "Media/materials/textures/" within the objects ''package''.
-====Cans====
+===Cans===
-=====Can of beer=====
+====Can of beer====
 We will create a file named "lata_cerveza.xacro" within the folder "urdf" of the objects ''package'' with the content that is shown below:
@@ Line 2,673: / Line 2,676: @@
-=====Can of cola=====
+====Can of cola====
 We will create a file named "lata_cola.xacro" within the folder "urdf" of the objects ''package'' with the content that is shown below:
@@ Line 2,852: / Line 2,855: @@
 </syntaxhighlight>
-===Execution===
+==Execution==
 We will create a file named "myrabot_latas.launch" within the "myrabot_fer" ''package'' with the content that is shown below in order to load the models of the objects and the robot in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo]:
@@ Line 2,894: / Line 2,897: @@
 <center>[http://www.youtube.com/watch?v=lp2FUFlsFV4 Watch in YouTube]</center>
-==Execution of the program tracking objects==
+=Execution of the program tracking objects=
-===Program to move objects in gazebo using the keyboard===
+==Program to move objects in gazebo using the keyboard==
 We will create a new file named "mover_objetos.cpp" within the folder "src" of the objects ''package'' with yhe content that is shown below, in order to move an object model (the default object model is Lata_amstel) in [http://wiki.ros.org/simulator_gazebo?distro=electric gazebo] publishing its position in the ''topic'' "gazebo/set_model_state":
@@ Line 3,151: / Line 3,154: @@
 </syntaxhighlight>
-===Execution===
+==Execution==
 We will create a file named "myrabot_lata.launch" within the folder "launch" of the "myrabot_fer" ''package'' with the content that is shown below, in order to load the models in the corrects positions:
@@ Line 3,193: / Line 3,196: @@
 <center><videoflash>kLfgfHo8OnI</videoflash></center>
-<center>[http://www.youtube.com/watch?v=kLfgfHo8OnI Ver vídeo en YouTube]</center>
+<center>[http://www.youtube.com/watch?v=kLfgfHo8OnI Watch in YouTube]</center>
@@ Line 3,210: / Line 3,213: @@
 [[Integration of MYRAbot in moveIt! (gazebo+moveIt!)]]<br/>
 [[Voice control (sphinx+festival)]]<br/>
-[[Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
+[[MYRAbot's Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
 | valign="top" |
-[[CeRVaNTeS's arm model for simulation (urdf+gazebo)]]<br/>
+[[CeRVaNTeS' arm and base control (maxon+epos2)]]<br/>
-[[CeRVaNTeS model for simulation (urdf+gazebo)]]
+[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]]<br/>
+[[CeRVaNTeS model for simulation (urdf+gazebo)]]<br/>
+[[Integration of CeRVaNTeS in moveIt! (gazebo+moveIt!)]]<br/>
+[[CeRVaNTeS' Teleoperation with xbox360 wireless controller (xbox360 controller+joy)]]
 |}

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

Latest revision as of 11:10, 29 September 2014

Contents

Components of MYRAbot model

URDF MYRAbot's structure

URDF Roomba

URDF kinect

URDF webcam

URDF ultrasound sensor

Creation of a gazebo plugin

URDF

Assembled of MYRAbot model

Display of the model in rviz

Load of the model in gazebo

Using an onlytopic for joint states

Load in gazebo

Test program (around the bullring)

Execution of the program (between the beer and the cola, the platform) choose

Creation of the objects' models

Table

Creation of new materials for gazebo

Cans

Can of beer

Can of cola

Execution

Execution of the program tracking objects

Program to move objects in gazebo using the keyboard

Execution

Navigation menu

Personal tools

Namespaces

Variants

Views

More

Search

Resources

Activities

Tools