Modelo para simulación MYRAbot (urdf+gazebo)
From robotica.unileon.es
Contents
Componentes del modelo de MYRAbot
Se ha comenzado realizado un modelo de la estructura del MYRAbot a la cual se le ha añadido la base Roomba, la cámara kinect, el modelo del brazo y la cámara web Logitech Webcam Pro 9000. Hemos comenzado creando un package que va a contener el modelo de nuestro robot, que llamaremos "myrabot_fer_modelo", con las dependencias urdf gazebo sensor_msgs roscpp.
URDF estructura MYRAbot
Se ha realizado previamente un modelo 3D de la estructura con un programa CAD, del que vamos a emplear las partes con una geometria singular, ya que el diseño empleando los elementos geométricos básicos sería complicado.
Diseño CAD 3D de la estructura del MYRAbot
Para el archivo principal crearemos un archivo llamado "estructura-myrabot.xacro" en el directorio "urdf" de el package anteriormente creado, con el siguiente contenido:
<?xml version="1.0"?>
<robot>
<include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot-macros.xacro" />
<macro name="estructura_myrabot" params="parent *origin">
<joint name="world_joint" type="fixed">
<parent link="${parent}"/>
<child link="e_base_1_link"/>
<insert_block name="origin" />
</joint>
<link name="e_base_1_link">
<inertial>
<mass value="0.05"/>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
<geometry>
<cylinder radius="0.135" length="0.015" />
</geometry>
<material name="wood">
<color rgba="${205/255} ${133/255} ${63/255} 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.015/2}"/>
<geometry>
<cylinder radius="0.135" length="0.015" />
</geometry>
</collision>
</link>
<separador_base id="1" />
<e_separador_base id="1" xsim="1" ysim="1" />
<separador_base id="2" />
<e_separador_base id="2" xsim="1" ysim="-1" />
<separador_base id="3" />
<e_separador_base id="3" xsim="-1" ysim="-1" />
<separador_base id="4" />
<e_separador_base id="4" xsim="-1" ysim="1" />
<joint name="base_2" type="fixed">
<parent link="e_base_1_link"/>
<child link="e_base_2_link"/>
<origin xyz="0 0 0.135" rpy="0 0 0" />
</joint>
<link name="e_base_2_link">
<inertial>
<mass value="0.05"/>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
<geometry>
<cylinder radius="0.135" length="0.02" />
</geometry>
<material name="wood">
<color rgba="${205/255} ${133/255} ${63/255} 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 0.135 ${0.02/2}"/>
<geometry>
<cylinder radius="0.135" length="0.02" />
</geometry>
</collision>
</link>
<joint name="pilar" type="fixed">
<parent link="e_base_2_link"/>
<child link="e_pilar_link"/>
<origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>
<link name="e_pilar_link">
<inertial>
<mass value="0.08"/>
<origin rpy="0 0 0" xyz="0 0 ${0.7/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_pilar.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_pilar.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint name="ultrasonidos" type="fixed">
<parent link="e_base_2_link"/>
<child link="e_ultrasonidos_link"/>
<origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>
<link name="e_ultrasonidos_link">
<inertial>
<mass value="0.04"/>
<origin rpy="0 0 0" xyz="0.0 0.0 ${0.04/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.160 -0.135 0"/>
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/e_ultrasonidos.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.160 -0.135 0"/>
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/e_ultrasonidos.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint name="botonera" type="fixed">
<parent link="e_base_2_link"/>
<child link="e_botonera_link"/>
<origin xyz="0 0 0.02" rpy="0 0 0" />
</joint>
<link name="e_botonera_link">
<inertial>
<mass value="0.06"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
<geometry>
<box size="0.2 0.09 0.1"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.1/2}"/>
<geometry>
<box size="0.2 0.09 0.1"/>
</geometry>
</collision>
</link>
<joint name="start_button_1" type="fixed">
<parent link="e_botonera_link"/>
<child link="e_start_button_1_link"/>
<origin xyz="-0.06 0 0.1" rpy="0 0 0" />
</joint>
<link name="e_start_button_1_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
<geometry>
<cylinder radius="0.009" length="0.002"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.002/2}"/>
<geometry>
<cylinder radius="0.009" length="0.002"/>
</geometry>
</collision>
</link>
<joint name="start_button_2" type="fixed">
<parent link="e_start_button_1_link"/>
<child link="e_start_button_2_link"/>
<origin xyz="0.0 0 0.002" rpy="0 0 0" />
</joint>
<link name="e_start_button_2_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
<geometry>
<cylinder radius="0.0075" length="0.004"/>
</geometry>
<material name="green">
<color rgba="0 1 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.004/2}"/>
<geometry>
<cylinder radius="0.0075" length="0.004"/>
</geometry>
</collision>
</link>
<joint name="emergency_button_1" type="fixed">
<parent link="e_botonera_link"/>
<child link="e_emergency_button_1_link"/>
<origin xyz="0.06 0 0.1" rpy="0 0 0" />
</joint>
<link name="e_emergency_button_1_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
<geometry>
<cylinder radius="0.0125" length="0.015"/>
</geometry>
<material name="metal" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.015/2}"/>
<geometry>
<cylinder radius="0.0125" length="0.015"/>
</geometry>
</collision>
</link>
<joint name="emergency_button_2" type="fixed">
<parent link="e_emergency_button_1_link"/>
<child link="e_emergency_button_2_link"/>
<origin xyz="0.0 0 0.015" rpy="0 0 0" />
</joint>
<link name="e_emergency_button_2_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
<geometry>
<cylinder radius="0.018" length="0.003"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.135 ${0.165-(0.09/2)} ${0.003/2}"/>
<geometry>
<cylinder radius="0.018" length="0.003"/>
</geometry>
</collision>
</link>
<joint name="base_kinect" type="fixed">
<parent link="e_pilar_link"/>
<child link="e_base_kinect_link"/>
<origin xyz="0 0 ${0.395-0.155}" rpy="0 0 0" />
</joint>
<link name="e_base_kinect_link">
<inertial>
<mass value="0.02"/>
<origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_kinect.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_kinect.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint name="base_brazo_1" type="fixed">
<parent link="e_pilar_link"/>
<child link="e_base_brazo_1_link"/>
<origin xyz="0 0 ${0.708-(0.395-0.155)}" rpy="0 0 0" />
</joint>
<link name="e_base_brazo_1_link">
<inertial>
<mass value="0.03"/>
<origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<separador_base_brazo id="1" />
<e_separador_base_brazo id="1" xsim="1" ysim="1" />
<separador_base_brazo id="2" />
<e_separador_base_brazo id="2" xsim="1" ysim="-1" />
<separador_base_brazo id="3" />
<e_separador_base_brazo id="3" xsim="-1" ysim="-1" />
<separador_base_brazo id="4" />
<e_separador_base_brazo id="4" xsim="-1" ysim="1" />
<joint name="base_brazo_2" type="fixed">
<parent link="e_base_brazo_1_link"/>
<child link="e_base_brazo_2_link"/>
<origin xyz="0 0 0.074" rpy="0 0 0" />
</joint>
<link name="e_base_brazo_2_link">
<inertial>
<mass value="0.005"/>
<origin rpy="0 0 0" xyz="0 0 ${0.005/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_base_brazo_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint name="soporte_1" type="fixed">
<parent link="e_pilar_link"/>
<child link="e_soporte_1_link"/>
<origin xyz="0 0 ${0.995-(0.708-0.155)-(0.395-0.155)}" rpy="0 0 0" />
</joint>
<link name="e_soporte_1_link">
<inertial>
<mass value="0.05"/>
<origin rpy="0 0 0" xyz="0 0 ${0.64/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint name="soporte_2" type="fixed">
<parent link="e_soporte_1_link"/>
<child link="e_soporte_2_link"/>
<origin xyz="0 0.2 0.61" rpy="-0.12 0 0" />
</joint>
<link name="e_soporte_2_link">
<inertial>
<mass value="0.01"/>
<origin rpy="0 0 0" xyz="0 0 ${0.06/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 -0.2 -0.03"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.2 -0.03"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_soporte_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint name="monitor" type="fixed">
<parent link="e_soporte_2_link"/>
<child link="e_monitor_link"/>
<origin xyz="0 -0.2 -0.09" rpy="0 0 0" />
</joint>
<link name="e_monitor_link">
<inertial>
<mass value="0.1"/>
<origin rpy="0 0 0" xyz="0 0 ${0.180/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_monitor.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="wood" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/e_monitor.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint name="pantalla" type="fixed">
<parent link="e_monitor_link"/>
<child link="e_pantalla_link"/>
<origin xyz="0.135 ${0.135-0.005-0.03-0.0001} 0.086" rpy="0 0 0" />
</joint>
<link name="e_pantalla_link">
<inertial>
<mass value="0.0001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.22 0.0001 0.130" />
</geometry>
<material name="black" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.22 0.0001 0.127" />
</geometry>
</collision>
</link>
<gazebo_propiedades nombre="e_base_1_link" material="LightWood" />
<gazebo_propiedades nombre="e_base_2_link" material="LightWood" />
<gazebo_propiedades nombre="e_separador_base_1_link" material="Grey" />
<gazebo_propiedades nombre="e_separador_base_2_link" material="Grey" />
<gazebo_propiedades nombre="e_separador_base_3_link" material="Grey" />
<gazebo_propiedades nombre="e_separador_base_4_link" material="Grey" />
<gazebo_propiedades nombre="e_separador_base_brazo_1_link" material="Grey" />
<gazebo_propiedades nombre="e_separador_base_brazo_2_link" material="Grey" />
<gazebo_propiedades nombre="e_separador_base_brazo_3_link" material="Grey" />
<gazebo_propiedades nombre="e_separador_base_brazo_4_link" material="Grey" />
<gazebo_propiedades nombre="e_pilar_link" material="LightWood" />
<gazebo_propiedades nombre="e_base_kinect_link" material="LightWood" />
<gazebo_propiedades nombre="e_base_brazo_1_link" material="LightWood" />
<gazebo_propiedades nombre="e_base_brazo_2_link" material="LightWood" />
<gazebo_propiedades nombre="e_botonera_link" material="LightWood" />
<gazebo_propiedades nombre="e_start_button_1_link" material="Black" />
<gazebo_propiedades nombre="e_start_button_2_link" material="Green" />
<gazebo_propiedades nombre="e_emergency_button_1_link" material="Grey" />
<gazebo_propiedades nombre="e_emergency_button_2_link" material="Red" />
<gazebo_propiedades nombre="e_soporte_1_link" material="LightWood" />
<gazebo_propiedades nombre="e_soporte_2_link" material="LightWood" />
<gazebo_propiedades nombre="e_monitor_link" material="LightWood" />
<gazebo_propiedades nombre="e_pantalla_link" material="Black" />
</macro>
</robot>
Los archivos empleados, con los modelos de malla 3D ".stl", en la descrición de la estructura del robot se pueden descargar a continuación, deben guardarse en el directorio "meshes" de nuestro package:
- e_pilar.stl
- e_base_kinect.stl
- e_base_brazo_1.stl
- e_base_brazo_2.stl
- e_soporte_1.stl
- e_soporte_2.stl
- e_monitor.stl
- e_ultrasonidos.stl
Los macros que estamos usando en el archivo principal del URDF son los que se muestran continuación, que guardaremos en un archivo llamado "estructura-myrabot-macros.xacro" en el directorio "urdf" de nuestro package:
<?xml version="1.0"?>
<robot>
<macro name="default_inertia_e">
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
</macro>
<macro name="separador_base" params="id">
<joint name="separador_base_${id}" type="fixed">
<parent link="e_base_1_link"/>
<child link="e_separador_base_${id}_link"/>
<origin xyz="0.135 0.135 0.015" rpy="0 0 0" />
</joint>
</macro>
<macro name="e_separador_base" params="id xsim ysim">
<link name="e_separador_base_${id}_link">
<inertial>
<mass value="0.02"/>
<origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
<geometry>
<cylinder radius="0.0075" length="0.120" />
</geometry>
<material name="metal">
<color rgba="${211/255} ${211/255} ${211/255} 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="${xsim*0.040} ${ysim*0.105} ${0.120/2}"/>
<geometry>
<cylinder radius="0.0075" length="0.120" />
</geometry>
</collision>
</link>
</macro>
<macro name="separador_base_brazo" params="id">
<joint name="separador_base_brazo_${id}" type="fixed">
<parent link="e_base_brazo_1_link"/>
<child link="e_separador_base_brazo_${id}_link"/>
<origin xyz="0.135 ${0.135-0.082} 0.075" rpy="0 0 0" />
</joint>
</macro>
<macro name="e_separador_base_brazo" params="id xsim ysim">
<link name="e_separador_base_brazo_${id}_link">
<inertial>
<mass value="0.002"/>
<origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
<default_inertia_e />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
<geometry>
<cylinder radius="0.002" length="0.034" />
</geometry>
<material name="metal" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="${xsim*0.029} ${ysim*0.019} ${0.034/2}"/>
<geometry>
<cylinder radius="0.002" length="0.034" />
</geometry>
</collision>
</link>
</macro>
<macro name="gazebo_propiedades" params="nombre material">
<gazebo reference="${nombre}">
<material>Gazebo/${material}</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
</macro>
</robot>
URDF Roomba
Para la base móvil Roomba se ha empleado el modelo del turtlebot, aunque equipado con una base móvil create, esta tiene unas físicas y sensores similares a la nuestra. Para esto necesitamos instalar los stacks turtlebot y turtlebot_simulator, que contienen el modelo y los plugins para gazebo, respectivamente. Eliminando lo que no necesitamos del modelo del turtlebot y modificacndo el elemento de la base, el contenido del archivo que guardaremos con el nombre "roomba.xacro" en el directorio "urdf" de nuestro package, es el siguiente:
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<include filename="$(find myrabot_fer_modelo)/urdf/roomba-gazebo.xacro"/>
<xacro:macro name="roomba">
<material name="Green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<link name="base_footprint">
<inertial>
<mass value="0.0001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
<material name="Green" />
</visual>
<collision>
<origin xyz="0 0 0.017" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</collision>
</link>
<link name="base_link">
<inertial>
<mass value="3.6" />
<origin xyz="0 0 ${0.063/2}" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.5" />
</inertial>
<visual>
<origin xyz=" 0 0 ${0.063/2}" rpy="0 0 0" />
<geometry>
<cylinder length="0.063" radius="0.17"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1" />
</material>
</visual>
<collision>
<origin xyz="0.0 0.0 ${0.063/2}" rpy="0 0 0" />
<geometry>
<cylinder length="0.063" radius="0.17"/>
</geometry>
</collision>
</link>
<link name="wall_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>
<link name="left_cliff_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>
<link name="right_cliff_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>
<link name="leftfront_cliff_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0"
iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>
<link name="rightfront_cliff_sensor_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
</link>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 0.017" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
</joint>
<joint name="base_wall_sensor_joint" type="fixed">
<origin xyz="0.09 -0.120 0.042" rpy="0 0 -1.0" />
<parent link="base_link"/>
<child link="wall_sensor_link" />
</joint>
<joint name="base_left_cliff_sensor_joint" type="fixed">
<origin xyz="0.07 0.14 0.01" rpy="0 1.57079 0" />
<parent link="base_link"/>
<child link="left_cliff_sensor_link" />
</joint>
<joint name="base_right_cliff_sensor_joint" type="fixed">
<origin xyz="0.07 -0.14 0.01" rpy="0 1.57079 0" />
<parent link="base_link"/>
<child link="right_cliff_sensor_link" />
</joint>
<joint name="base_leftfront_cliff_sensor_joint" type="fixed">
<origin xyz="0.15 0.04 0.01" rpy="0 1.57079 0" />
<parent link="base_link"/>
<child link="leftfront_cliff_sensor_link" />
</joint>
<joint name="base_rightfront_cliff_sensor_joint" type="fixed">
<origin xyz="0.15 -0.04 0.01" rpy="0 1.57079 0" />
<parent link="base_link"/>
<child link="rightfront_cliff_sensor_link" />
</joint>
<link name="left_wheel_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<cylinder radius="0.033" length = "0.023"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<cylinder radius="0.033" length = "0.023"/>
</geometry>
</collision>
</link>
<joint name="left_wheel_joint" type="continuous">
<origin xyz="0 0.13 0.015" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="left_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="right_wheel_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<cylinder radius="0.033" length = "0.023"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<cylinder radius="0.033" length = "0.023"/>
</geometry>
</collision>
</link>
<joint name="right_wheel_joint" type="continuous">
<origin xyz="0 -0.13 0.015" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="right_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="rear_wheel_link">
<inertial>
<origin xyz="0 0 0"/>
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0" izz="0.0001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
<geometry>
<sphere radius="0.015" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<sphere radius="0.015" />
</geometry>
</collision>
</link>
<joint name="rear_castor_joint" type="fixed">
<origin xyz="-0.13 0 0.0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="rear_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<link name="front_wheel_link">
<inertial>
<origin xyz="0 0 0" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707"/>
<geometry>
<sphere radius="0.018" />
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 1.5707 1.5707" />
<geometry>
<sphere radius="0.018" />
</geometry>
</collision>
</link>
<joint name="front_castor_joint" type="fixed">
<origin xyz="0.13 0 0.0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="front_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<joint name="gyro_joint" type="fixed">
<axis xyz="0 1 0" />
<origin xyz="0 0 0.04" rpy="0 0 0" />
<parent link="base_link"/>
<child link="gyro_link"/>
</joint>
<link name="gyro_link">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001" />
</inertial>
</link>
<joint name="laser_joint" type="fixed">
<origin xyz="-0.065 0 0.57" rpy="0 0 0" />
<parent link="base_link" />
<child link="laser" />
</joint>
<link name="laser" />
<gaezebo>
<material>Gazebo/White</material>
</gaezebo>
<myrabot_sim_imu/>
<myrabot_sim_laser/>
<myrabot_sim_roomba/>
<myrabot_sim_wall_sensors/>
</xacro:macro>
</robot>
Para su uso en el simulador gazebo debemos añadir los plugins de los diferentes sensores de la base. Crearemos un archivo llamado "roomba-gazebo.xacro" en el directorio "urdf" de nuestro package con el siguiente contenido:
<?xml version="1.0"?>
<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="myrabot_sim_imu">
<gazebo>
<controller:gazebo_ros_imu name="imu_controller" plugin="libgazebo_ros_imu.so">
<robotNamespace>/</robotNamespace>
<alwaysOn>true</alwaysOn>
<updateRate>30</updateRate>
<bodyName>gyro_link</bodyName>
<topicName>imu/data</topicName>
<gaussianNoise>${0.0017*0.0017}</gaussianNoise>
<xyzOffsets>0 0 0</xyzOffsets>
<rpyOffsets>0 0 0</rpyOffsets>
<interface:position name="imu_position"/>
</controller:gazebo_ros_imu>
</gazebo>
</xacro:macro>
<xacro:macro name="myrabot_sim_laser">
<gazebo reference="laser">
<sensor:ray name="laser">
<rayCount>180</rayCount>
<rangeCount>180</rangeCount>
<laserCount>1</laserCount>
<origin>0.0 0.0 0.0</origin>
<displayRays>false</displayRays>
<minAngle>-${60/2}</minAngle>
<maxAngle>${60/2}</maxAngle>
<minRange>0.08</minRange>
<maxRange>5.0</maxRange>
<resRange>0.01</resRange>
<updateRate>20</updateRate>
<controller:gazebo_ros_laser name="gazebo_ros_laser_controller" plugin="libgazebo_ros_laser.so">
<robotNamespace>/</robotNamespace>
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<topicName>scan</topicName>
<frameName>laser</frameName>
<interface:laser name="gazebo_ros_laser_iface" />
</controller:gazebo_ros_laser>
</sensor:ray>
</gazebo>
</xacro:macro>
<xacro:macro name="myrabot_sim_roomba">
<gazebo>
<controller:gazebo_ros_roomba name="roomba_controller" plugin="libgazebo_ros_create.so">
<alwaysOn>true</alwaysOn>
<node_namespace>myrabot_node</node_namespace>
<left_wheel_joint>left_wheel_joint</left_wheel_joint>
<right_wheel_joint>right_wheel_joint</right_wheel_joint>
<front_castor_joint>front_castor_joint</front_castor_joint>
<rear_castor_joint>rear_castor_joint</rear_castor_joint>
<wheel_separation>.230</wheel_separation>
<wheel_diameter>0.070</wheel_diameter>
<base_geom>base_roomba_link</base_geom>
<updateRate>40</updateRate>
<torque>1.0</torque>
</controller:gazebo_ros_roomba>
</gazebo>
</xacro:macro>
<xacro:macro name="myrabot_sim_wall_sensors">
<gazebo reference="wall_sensor_link">
<sensor:ray name="wall_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>
<minAngle>0</minAngle>
<maxAngle>0</maxAngle>
<minRange>0.0160</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>
<gazebo reference="left_cliff_sensor_link">
<sensor:ray name="left_cliff_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>
<minAngle>0</minAngle>
<maxAngle>0</maxAngle>
<minRange>0.01</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>
<gazebo reference="right_cliff_sensor_link">
<sensor:ray name="right_cliff_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>
<minAngle>0</minAngle>
<maxAngle>0</maxAngle>
<minRange>0.01</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>
<gazebo reference="leftfront_cliff_sensor_link">
<sensor:ray name="leftfront_cliff_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>
<minAngle>0</minAngle>
<maxAngle>0</maxAngle>
<minRange>0.01</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>
<gazebo reference="rightfront_cliff_sensor_link">
<sensor:ray name="rightfront_cliff_sensor">
<alwaysActive>true</alwaysActive>
<rayCount>1</rayCount>
<rangeCount>1</rangeCount>
<resRange>0.1</resRange>
<minAngle>0</minAngle>
<maxAngle>0</maxAngle>
<minRange>0.01</minRange>
<maxRange>0.04</maxRange>
<displayRays>false</displayRays>
</sensor:ray>
</gazebo>
<gazebo reference="left_wheel_link">
<mu1 value="10"/>
<mu2 value="10"/>
<kp value="100000000.0"/>
<kd value="10000.0"/>
<fdir value="1 0 0"/>
</gazebo>
<gazebo reference="right_wheel_link">
<mu1 value="10"/>
<mu2 value="10"/>
<kp value="100000000.0"/>
<kd value="10000.0"/>
<fdir value="1 0 0"/>
</gazebo>
<gazebo reference="rear_wheel_link">
<mu1 value="0"/>
<mu2 value="0"/>
<kp value="100000000.0"/>
<kd value="10000.0"/>
</gazebo>
<gazebo reference="front_wheel_link">
<mu1 value="0"/>
<mu2 value="0"/>
<kp value="100000000.0"/>
<kd value="10000.0"/>
</gazebo>
</xacro:macro>
</robot>
URDF kinect
Para la cámara kinect incluiremos el modelo y el plugin para el simulador en un solo archivo. Crearemos un archivo llamado "kinect.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:
<?xml version="1.0"?>
<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
<property name="M_PI" value="3.14159"/>
<xacro:macro name="kinect" params="parent *origin">
<joint name="base_camera_joint" type="fixed">
<insert_block name="origin" />
<parent link="${parent}" />
<child link="camera_link" />
</joint>
<link name="camera_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/kinect.dae"/>
</geometry>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_fer_modelo/meshes/kinect.dae"/>
</geometry>
</collision>
</link>
<joint name="camera_depth_joint" type="fixed">
<origin xyz="0 0.018 0" rpy="0 0 0" />
<parent link="camera_link" />
<child link="camera_depth_frame" />
</joint>
<link name="camera_depth_frame">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>
<joint name="camera_depth_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_depth_frame" />
<child link="camera_depth_optical_frame" />
</joint>
<link name="camera_depth_optical_frame">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
</link>
<joint name="camera_rgb_joint" type="fixed">
<origin xyz="0 -0.005 0" rpy="0 0 0" />
<parent link="camera_link" />
<child link="camera_rgb_frame" />
</joint>
<link name="camera_rgb_frame">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
</link>
<joint name="camera_rgb_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="camera_rgb_frame" />
<child link="camera_rgb_optical_frame" />
</joint>
<link name="camera_rgb_optical_frame">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
iyy="0.0001" iyz="0.0"
izz="0.0001" />
</inertial>
</link>
<gazebo reference="camera_link">
<sensor:camera name="camera">
<imageFormat>R8G8B8</imageFormat>
<imageSize>640 480</imageSize>
<hfov>60</hfov>
<nearClip>0.05</nearClip>
<farClip>5</farClip>
<updateRate>20</updateRate>
<baseline>0.1</baseline>
<controller:gazebo_ros_openni_kinect name="kinect_camera_controller" plugin="libgazebo_ros_openni_kinect.so">
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<robotNamespace>/</robotNamespace>
<imageTopicName>/camera/image_raw</imageTopicName>
<pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
<cameraInfoTopicName>/camera/camera_info</cameraInfoTopicName>
<frameName>camera_depth_optical_frame</frameName>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
</controller:gazebo_ros_openni_kinect>
</sensor:camera>
</gazebo>
</xacro:macro>
</robot>
Los archivos empleados, con el modelo de malla 3D ".stl", en la descrición de la cámara se pueden descargar a continuación, deben guardarse en el directorio "meshes" de nuestro package:
URDF cámara web
Para la cámara web vamos a emplear el plugin de la cámara kinect, pero sin usar la nube de puntos (point cloud). La cámara web empleada es una Logitech Webcam Pro 9000. Crearemos un archivo llamado "webcam.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:
<?xml version="1.0"?>
<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="webcam" params="parent *origin">
<joint name="webcam_1" type="fixed">
<insert_block name="origin" />
<parent link="${parent}" />
<child link="e_webcam_1_link" />
</joint>
<link name="e_webcam_1_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
<visual>
<origin xyz=" 0 0 0 " rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/webcam_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint name="webcam_2" type="fixed">
<origin xyz="0 ${0.135-0.04-(0.02-0.009)} ${(0.185/2)+0.0045}" rpy="0.82 0 0" />
<parent link="e_webcam_1_link" />
<child link="e_webcam_2_link" />
</joint>
<link name="e_webcam_2_link">
<inertial>
<mass value="0.01" />
<origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
</inertial>
<visual>
<origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="black" />
</visual>
<collision>
<origin xyz="0 -${0.135-0.04-(0.02-0.009)} -${(0.185/2)+0.0045}" rpy="0 0 0" />
<geometry>
<mesh filename="package://myrabot_robot_model/meshes/webcam_2.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint name="webcam_optica" type="fixed">
<origin xyz="0.135 -0.0285 0" rpy="0 0 -1.57" />
<parent link="e_webcam_2_link" />
<child link="e_webcam_optica_link" />
</joint>
<link name="e_webcam_optica_link">
<inertial>
<mass value="0.001" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 1.57 0" />
<geometry>
<cylinder radius="0.017" length="0.004" />
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 1.57 0" />
<geometry>
<cylinder radius="0.017" length="0.004" />
</geometry>
</collision>
</link>
<gazebo reference="e_webcam_1_link">
<material>myrabot_fer/Black</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="e_webcam_2_link">
<material>myrabot_fer/Black</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="e_webcam_optica_link">
<material>myrabot_fer/White</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="e_webcam_optica_link">
<sensor:camera name="webcam">
<imageFormat>R8G8B8</imageFormat>
<imageSize>640 480</imageSize>
<hfov>63.09</hfov>
<nearClip>0.02</nearClip>
<farClip>3</farClip>
<updateRate>20</updateRate>
<baseline>0.1</baseline>
<controller:gazebo_ros_openni_kinect name="webcam_camera_controller" filename="libgazebo_ros_openni_kinect.so">
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<robotNamespace>/</robotNamespace>
<imageTopicName>/webcam/image_raw</imageTopicName>
<pointCloudTopicName>/webcam/depth/points</pointCloudTopicName>
<cameraInfoTopicName>/webcam/camera_info</cameraInfoTopicName>
<frameName>e_webcam_optica_link</frameName>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
</controller:gazebo_ros_openni_kinect>
</sensor:camera>
</gazebo>
</xacro:macro>
</robot>
Los archivos empleados, con los modelos de malla 3D ".stl", en la descrición de la cámara web empleada se pueden descargar a continuación, deben guardarse en el directorio "meshes" de nuestro package:
URDF sensor ultrasonidos
Creación de un plugin para gazebo
Primero crearemos un package que va a contener los plugins de nuestro robot, que llamaremos "myrabot_gazebo_plugins", con las dependencias gazebo sensor_msgs roscpp.. Dentro de la carpeta "src" del package creado para los plugins crearemos un archivo llamado "gazebo_ros_sonar.cpp" con el siguiente contenido:
/*
* Software License Agreement (Modified BSD License)
*
* Copyright (c) 2012, PAL Robotics, S.L.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of PAL Robotics, S.L. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <gazebo/Sensor.hh>
#include <gazebo/Global.hh>
#include <gazebo/XMLConfig.hh>
#include <gazebo/Simulator.hh>
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
#include <gazebo/ControllerFactory.hh>
#include <limits>
#include <myrabot_gazebo_plugin/gazebo_ros_sonar.h>
using namespace gazebo;
GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_sonar", GazeboRosSonar)
GazeboRosSonar::GazeboRosSonar(Entity *parent) : Controller(parent)
{
sensor_ = dynamic_cast<RaySensor*>(parent);
if (!sensor_) gzthrow("GazeboRosSonar controller requires a RaySensor as its parent");
Param::Begin(¶meters);
topic_param_ = new ParamT<std::string>("topicName", "sonar", false);
frame_id_param_ = new ParamT<std::string>("frameId", "", false);
radiation_param_ = new ParamT<std::string>("radiation","ultrasound",false);
fov_param_ = new ParamT<double>("fov", 0.05, false);
gaussian_noise_ = new ParamT<double>("gaussianNoise", 0.0, 0);
Param::End();
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
GazeboRosSonar::~GazeboRosSonar()
{
delete topic_param_;
delete frame_id_param_;
delete radiation_param_;
delete fov_param_;
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosSonar::LoadChild(XMLConfigNode *node)
{
ROS_INFO("INFO: gazebo_ros_ir plugin loading" );
topic_param_->Load(node);
frame_id_param_->Load(node);
radiation_param_->Load(node);
fov_param_->Load(node);
gaussian_noise_->Load(node);
}
///////////////////////////////////////////////////////
// Initialize the controller
void GazeboRosSonar::InitChild()
{
Range.header.frame_id = **frame_id_param_;
if (**radiation_param_==std::string("ultrasound"))
Range.radiation_type = sensor_msgs::Range::ULTRASOUND;
else
Range.radiation_type = sensor_msgs::Range::INFRARED;
Range.field_of_view = **fov_param_;
Range.max_range = sensor_->GetMaxRange();
Range.min_range = sensor_->GetMinRange();
sensor_->SetActive(false);
node_handle_ = new ros::NodeHandle("");
publisher_ = node_handle_->advertise<sensor_msgs::Range>(**topic_param_, 10);
}
////////////////////////////////////////////////////////////////////////////////
// Utility for adding noise
double GazeboRosSonar::GaussianKernel(double mu,double sigma)
{
// using Box-Muller transform to generate two independent standard normally disbributed normal variables
// see wikipedia
double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
//double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
// we'll just use X
// scale to our mu and sigma
X = sigma * X + mu;
return X;
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosSonar::UpdateChild()
{
if (!sensor_->IsActive()) sensor_->SetActive(true);
Range.header.stamp.sec = (Simulator::Instance()->GetSimTime()).sec;
Range.header.stamp.nsec = (Simulator::Instance()->GetSimTime()).nsec;
Range.range = std::numeric_limits<sensor_msgs::Range::_range_type>::max();
for(int i = 0; i < sensor_->GetRangeCount(); ++i) {
double ray = sensor_->GetRange(i);
if (ray < Range.range) Range.range = ray;
}
Range.range = std::min(Range.range + this->GaussianKernel(0,**gaussian_noise_), sensor_->GetMaxRange());
publisher_.publish(Range);
}
////////////////////////////////////////////////////////////////////////////////
// Finalize the controller
void GazeboRosSonar::FiniChild()
{
sensor_->SetActive(false);
node_handle_->shutdown();
delete node_handle_;
}
Crearemos un archivo llamado "gazebo_ros_sonar.h" situdo en la ruta "include/myrabot_gazebo_plugins/" dentro del package que hemos creado, con el siguiente contenido:
/*
* Software License Agreement (Modified BSD License)
*
* Copyright (c) 2012, PAL Robotics, S.L.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of PAL Robotics, S.L. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <gazebo.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/sensors/RaySensor.hh>
#include <ros/callback_queue.h>
#include <sensor_msgs/Range.h>
namespace gazebo
{
class GazeboRosSonar : public gazebo::SensorPlugin
{
public:
GazeboRosSonar();
virtual ~GazeboRosSonar();
protected:
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
virtual void Update();
/// \brief Gaussian noise generator
private:
double GaussianKernel(double mu,double sigma);
private:
gazebo::sensors::RaySensorPtr sensor_;
ros::NodeHandle* node_handle_;
ros::Publisher publisher_;
sensor_msgs::Range range_;
std::string namespace_;
std::string topic_name_;
std::string frame_id_;
std::string radiation_;
double fov_;
double gaussian_noise_;
gazebo::physics::WorldPtr parent_;
common::Time last_time;
gazebo::event::ConnectionPtr updateConnection;
};
} // namespace gazebo
Añadiremos al archivo "CMakeLists.txt" del package la siguiente línea al final:
rosbuild_add_library(gazebo_ros_sonar src/gazebo_ros_sonar.cpp)También añadiremos las siguientes líneas al archivo "manifest.xml" del package, antes de las etiqueta </package>:
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib" />
<gazebo plugin_path="${prefix}/lib" />
</export>
Para compilar el nuevo plugin simplemente deberemos ejecutar en un terminal la siguiente secuencia de comandos:
roscd myrabot_gazebo_plugins
makeURDF
En el directorio "urdf" del package del robot vamos a crear un archivo llamado "ultrasonidos.xacro" con el siguiente contenido:
<?xml version="1.0"?>
<robot xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="ultrasonidos" params="id parent d_centro *origin">
<joint name="ultrasonidos_${id}_joint_fisico" type="fixed">
<parent link="${parent}"/>
<child link="ultrasonidos_${id}_link_fisico"/>
<insert_block name="origin" />
</joint>
<link name="ultrasonidos_${id}_link_fisico">
<inertial>
<mass value="0.001"/>
<origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
<geometry>
<cylinder radius="${0.015/2}" length="0.015" />
</geometry>
<material name="black">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="1.57 0 0" xyz="0 -${d_centro} ${0.015/2}"/>
<geometry>
<cylinder radius="${0.015/2}" length="0.015" />
</geometry>
</collision>
</link>
<joint name="ultrasonidos_${id}_joint" type="fixed">
<parent link="ultrasonidos_${id}_link_fisico"/>
<child link="ultrasonidos_${id}_link"/>
<origin rpy="0 0 -1.57" xyz="0 -${d_centro+(0.015/2)} ${0.015/2}"/>
</joint>
<link name="ultrasonidos_${id}_link">
</link>
<gazebo reference="ultrasonidos_${id}_link_fisico">
<material>myrabot_fer/Black</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="ultrasonidos_${id}_link">
<sensor:ray name="ultrasonidos_${id}">
<displayRays>false</displayRays>
<rayCount>5</rayCount>
<rangeCount>5</rangeCount>
<verticalRayCount>1</verticalRayCount>
<verticalRangeCount>1</verticalRangeCount>
<minAngle>-16</minAngle>
<verticalMinAngle>-16</verticalMinAngle>
<maxAngle>16</maxAngle>
<verticalMaxAngle>16</verticalMaxAngle>
<minRange>0</minRange>
<maxRange>7</maxRange>
<resRange>0.01</resRange>
<updateRate>20</updateRate>
<controller:gazebo_ros_sonar name="gazebo_ros_ultrasonidos_${id}_controller" plugin="libgazebo_ros_sonar.so">
<robotNamespace>/</robotNamespace>
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
<updateRate>20</updateRate>
<topicName>ultrasonidos_${id}</topicName>
<frameId>ultrasonidos_${id}_link</frameId>
<radiation>ultrasound</radiation>
<fov>32</fov>
</controller:gazebo_ros_sonar>
</sensor:ray>
</gazebo>
</xacro:macro>
</robot>
Ensamblado del modelo del MYRAbot
Para poder formar el modelo de nuestro robot tenemos que ensamblar las diferentes partes que lo componen, las que hemos descrito en los puntos anteriores y el brazo. Los componentes individuales se han definido como macros con los parámetros de link parent y origen, con lo que deberemos indicar el link y el origen del componente al que se une. Crearemos un archivo llamado "myrabot.urdf.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:
<?xml version="1.0"?>
<robot name="MYRAbot"
xmlns:xi="http://www.w3.org/2001/XInclude"
xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
<include filename="$(find myrabot_fer_modelo)/urdf/roomba.xacro" />
<include filename="$(find myrabot_fer_modelo)/urdf/estructura-myrabot.xacro" />
<include filename="$(find myrabot_fer_modelo)/urdf/kinect.xacro" />
<include filename="$(find brazo_fer_modelo)/urdf/brazo.xacro" />
<include filename="$(find myrabot_fer_modelo)/urdf/webcam.xacro" />
<roomba />
<estructura_myrabot parent="base_link">
<origin rpy="0 0 1.57" xyz="0.135 -0.135 0.063"/>
</estructura_myrabot>
<kinect parent="e_base_kinect_link">
<origin rpy="0 0 -1.57" xyz="0.135 0.090 0.112"/>
</kinect>
<brazo parent="e_base_brazo_1_link">
<origin rpy="0 0 3.1416" xyz="0.135 0.0335 0.075"/>
</brazo>
<webcam parent="e_monitor_link">
<origin rpy="0 0 0" xyz="0 0 ${0.185/2}"/>
</webcam>
<ultrasonidos id="1" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 1.57" xyz="0.135 0.135 0.02"/>
</ultrasonidos>
<ultrasonidos id="2" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 2.36" xyz="0.135 0.135 0.02"/>
</ultrasonidos>
<ultrasonidos id="3" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 3.14" xyz="0.135 0.135 0.02"/>
</ultrasonidos>
<ultrasonidos id="4" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 3.93" xyz="0.135 0.135 0.02"/>
</ultrasonidos>
<ultrasonidos id="5" parent="e_base_2_link" d_centro="0.150">
<origin rpy="0 0 4.71" xyz="0.135 0.135 0.02"/>
</ultrasonidos>
</robot>
Visualización del modelo en rviz
Para la comprobación y visualización del correcto ensamblado del modelo, así como la prueba de las uniones móviles, se ha creado un launcher para cargar el modelo en rviz. Crearemos un archivo llamado "myrabot_rviz.launch" dentro del directorio "launch" de nuestro package con el siguiente contenido:
<launch>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_fer_modelo)/urdf/myrabot.urdf.xacro'" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<param name="use_gui" value="true"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" />
</launch>
Para iniciar el launcher debemos ejecutar en un terminal el siguiente comando:
roslaunch myrabot_fer_modelo myrabot_rviz.launchSi todo va bien, se ejecutará rviz y el interfaz del joint_state_publisher, sobre el cual al modificar los controles deslizables observaremos como se modifica la posición de las uniones móviles en el modelo 3D cargado en rviz. Para que se muestre correctamente nuestro modelo en rviz debemos seleccionar el elemento link que se va ha tomar como elemento fijo "fixed frame", en nuestro caso en "Global Options" el "Fixed Frame" será el link "/base_footprint". El resultado es el que se muestra a continuación:
Carga del modelo en gazebo
Uso de un único topic para joint states
Para poder tener en el topic "joint_states_myrabot" el estado de las uniones del brazo y de la base, hemos creado un pequeño programa que toma los valores de los topics "brazo/joint_states" y "joint_states", y los publica en este único topic. Crearemos un archivo llamado "remap_joint_states.cpp" dentro del directorio "src" de nuestro package:
#include "ros/ros.h"
#include "ros/time.h"
#include "sensor_msgs/JointState.h"
sensor_msgs::JointState joint_states_todo;
int cont_brazo = 0;
int cont_roomba = 0;
std::string name[9];
float position[9];
float velocity[9];
float effort[9];
void brazo(const sensor_msgs::JointState& brazo_states)
{
int tam = brazo_states.name.size();
if (::cont_brazo == 0)
{
for (int i = 0; i < tam; i++)
{
::name[i] = brazo_states.name[i];
::position[i] = brazo_states.position[i];
::velocity[i] = brazo_states.velocity[i];
::effort[i] = brazo_states.effort[i];
}
::cont_brazo = 1;
}
}
void roomba(const sensor_msgs::JointState& roomba_states)
{
int tam = roomba_states.name.size();
if (::cont_roomba == 0)
{
for (int i = 0; i < tam; i++)
{
::name[i+5] = roomba_states.name[i];
::position[i+5] = roomba_states.position[i];
::velocity[i+5] = roomba_states.velocity[i];
::effort[i+5] = roomba_states.effort[i];
}
::cont_roomba = 1;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "remap_joint_states");
ros::NodeHandle n;
ros::Subscriber joint_states_brazo_sub_= n.subscribe("brazo/joint_states", 1, brazo);
ros::Subscriber joint_states_roomba_sub_= n.subscribe("joint_states", 1, roomba);
ros::Publisher joint_states_pub_=n.advertise<sensor_msgs::JointState>("joint_states_myrabot", 1);
ros::Rate loop_rate(100);
ros::Time ahora;
::joint_states_todo.name = std::vector<std::string>(9);
::joint_states_todo.position = std::vector<double>(9);
::joint_states_todo.velocity = std::vector<double>(9);
::joint_states_todo.effort = std::vector<double>(9);
while (ros::ok())
{
if (::cont_brazo == 1 && ::cont_roomba == 1)
{
for (int i = 0; i < 9; i++)
{
::joint_states_todo.name[i] = ::name[i];
::joint_states_todo.position[i] = ::position[i];
::joint_states_todo.velocity[i] = ::velocity[i];
::joint_states_todo.effort[i] = ::effort[i];
}
ahora = ros::Time::now();
::joint_states_todo.header.stamp = ahora;
joint_states_pub_.publish(::joint_states_todo);
::cont_brazo = 0;
::cont_roomba = 0;
}
ros::spinOnce();
loop_rate.sleep();
}
ros::spin();
return 0;
}
Carga en gazebo
Para poder cargar el modelo en gazebo e iniciar los controladores de todos sus componentes, se ha creado un launcher. Crearemos un archivo llamado "myrabot_gazebo.launch" dentro del directorio "launch" de nuestro package con el siguiente contenido:
<launch>
<param name="/use_sim_time" value="true" />
<node name="gazebo" pkg="gazebo" type="gazebo" args="-u $(find turtlebot_gazebo)/worlds/empty.world" respawn="false" output="screen"/>
<node name="gazebo_gui" pkg="gazebo" type="gui" />
<group ns="brazo">
<param name="robot_description" command="$(find xacro)/xacro.py '$(find myrabot_robot_model)/urdf/myrabot.urdf.xacro'" />
<node name="spawn_myrabot" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model MYRAbot" respawn="false" output="screen" />
<rosparam file="$(find myrabot_arm_model)/controller.yaml" command="load"/>
<node name="spawn_controller" pkg="pr2_controller_manager" type="spawner" args="base_pos_controller arti1_pos_controller arti2_pos_controller arti3_pos_controller pinza_pos_controller base_vel_controller arti1_vel_controller arti2_vel_controller arti3_vel_controller pinza_vel_controller" respawn="false" output="screen" />
</group>
<node name="modelo_brazo" pkg="myrabot_arm_model" type="modelo_brazo" />
<node name="remap_joint_states" pkg="myrabot_robot_model" type="remap_joint_states" />
<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
<param name="freq" value="30.0"/>
<param name="sensor_timeout" value="1.0"/>
<param name="publish_tf" value="true"/>
<param name="odom_used" value="true"/>
<param name="imu_used" value="false"/>
<param name="vo_used" value="false"/>
<param name="output_frame" value="odom"/>
</node>
<node pkg="nodelet" type="nodelet" name="pointcloud_throttle" args="load pointcloud_to_laserscan/CloudThrottle openni_manager" respawn="true">
<param name="max_rate" value="20.0"/>
<remap from="cloud_in" to="/camera/depth/points"/>
<remap from="cloud_out" to="cloud_throttled"/>
</node>
<node pkg="nodelet" type="nodelet" name="kinect_laser" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
<param name="output_frame_id" value="/camera_depth_frame"/>
<!-- heights are in the (optical?) frame of the kinect -->
<param name="min_height" value="-0.15"/>
<param name="max_height" value="0.15"/>
<remap from="cloud" to="/cloud_throttled"/>
</node>
<node pkg="nodelet" type="nodelet" name="kinect_laser_narrow" args="load pointcloud_to_laserscan/CloudToScan openni_manager" respawn="true">
<param name="output_frame_id" value="/camera_depth_frame"/>
<!-- heights are in the (optical?) frame of the kinect -->
<param name="min_height" value="-0.025"/>
<param name="max_height" value="0.025"/>
<remap from="cloud" to="/cloud_throttled"/>
<remap from="scan" to="/narrow_scan"/>
</node>
</launch>
Para lanzar el launcher ejecutaremos el siguiente comando en un terminal:
roslaunch myrabot_fer_modelo myrabot_gazebo.launchSi todo va bien podremos ver en la ventana gráfica de gazebo el modelo del MYRAbot situado en un mundo vacío, tal como se muestra en la siguiente imagen:
Modelo MYRAbot en gazebo
Programa de prueba (Vuela al ruedo)
Se ha realizado un pequeño programa para probar el funcionamiento del modelo del MYRAbot, que simplemente hace girar en circulo al robot y gira el brazo de un lado a otro en posición vertical, cuando se acciona el pulsado de inicio del robot. Primero vamos a crear un package que va a contener nuestro programa y que llamaremos "myrabot_fer", con las siguientes dependencias: brazo_fer geometry_msgs std_msgs nav_msgs sensor_msgs image_transport roscpp.
Dentro de este package vamos a crear un archivo llamado "vuelta_al_ruedo.cpp" en el directorio "src" con el siguiente contenido:
#include "ros/ros.h"
#include "brazo_fer/Servos.h"
#include "brazo_fer/WriteServos.h"
#include "brazo_fer/ReadServos.h"
#include "geometry_msgs/Twist.h"
#include "std_msgs/Bool.h"
int cont = 0;
int cont1= 0;
int encendido = 0;
brazo_fer::Servos move, on, vel;
void home()
{
if (cont1 == 0)
{
ros::NodeHandle n;
ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
brazo_fer::WriteServos home;
::move.base = 511;
::move.arti1 = 511;
::move.arti2 = 511;
::move.arti3 = 511;
::move.pinza = 511;
::on.base = 1;
::on.arti1 = 1;
::on.arti2 = 1;
::on.arti3 = 1;
::on.pinza = 1;
::vel.base = 400;
::vel.arti1 = 400;
::vel.arti2 = 400;
::vel.arti3 = 400;
::vel.pinza = 400;
home.posicion = ::move;
home.par = ::on;
home.velocidad = ::vel;
move_arm_pub_.publish(home);
hand_pub_.publish(home);
::cont1 = 1;
}
}
void inicio(const std_msgs::Bool& inicio)
{
if (inicio.data == true && ::cont == 0)
{
::encendido = 1;
::cont = 1;
}
else if (inicio.data == true && ::cont == 1)
{
::encendido = 0;
::cont = 0;
}
}
void posicion(const brazo_fer::ReadServos& posicion)
{
ros::NodeHandle n;
ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
brazo_fer::Servos p = posicion.posicion;
brazo_fer::WriteServos mover_brazo;
home();
if (::encendido == 1)
{
geometry_msgs::Twist mover_base;
mover_base.linear.x = 0.1;
mover_base.angular.z = 0.3;
move_base_pub_.publish(mover_base);
if (::cont1 == 1)
{
::move.base = 718;
::cont1 = 2;
mover_brazo.posicion = ::move;
mover_brazo.par = ::on;
mover_brazo.velocidad = ::vel;
move_arm_pub_.publish(mover_brazo);
}
else if ((p.base-110) < ::move.base && ::move.base < (p.base+110))
{
if (::cont1 == 2)
{
::move.base = 304;
::cont1 = 3;
mover_brazo.posicion = ::move;
mover_brazo.par = ::on;
mover_brazo.velocidad = ::vel;
move_arm_pub_.publish(mover_brazo);
}
else
{
::cont1 = 1;
}
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "vuelta_al_ruedo");
ros::NodeHandle n;
ros::Subscriber pose_sub_= n.subscribe("pose_arm", 1, posicion);
ros::Subscriber button_sub_= n.subscribe("start_button", 1, inicio);
ros::Publisher move_arm_pub_=n.advertise<brazo_fer::WriteServos>("move_arm", 1);
ros::Publisher hand_pub_=n.advertise<brazo_fer::WriteServos>("hand_arm", 1);
ros::Publisher move_base_pub_=n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
ros::spin();
return 0;
}
Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro package:
rosbuild_add_executable(vuelta_al_ruedo src/vuelta_al_ruedo.cpp)Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal:
roscd myrabot_fer
makePara lanzar gazebo con el modelo del robot en un mundo vacío, ejecutaremos el siguiente comando en un terminal:
roslaunch myrabot_fer_modelo myrabot_gazebo.launchPara ejecutar el programa que hemos creado, en otro terminal ejecutaremos el siguiente comando:
rosrun myrabot_fer vuelta_al_ruedoEl modelo del robot permanecerá detenido hasta que publiquemos el valor "true" en el topic "start_button", que corresponde al botón de inicio del robot. Para simular la pulsación del botón de inicio vamos a ejecutar el siguiente comando en un nuevo terminal:
rostopic pub start_button std_msgs/Bool '{data: true}' --onceSi todo a funcionado bien el robot comenzara a desplazarse en círculo y girar el brazo de un lado a otro en posición vertical, como se muestra en el vídeo:
Ejecución del programa (entre la cerveza y la cola, la plataforma) escoja
Para la ejecución del programa escoja en el simulador gazebo usando el modelo del MYRAbot, necesitamos crear previamente los modelos de los objetos a reconocer y una mesa, sobre la cual estarán situados.
Creación de modelos de objetos
Mesa
Primero vamos crear el modelo de la mesa donde vamos a situar los objetos. Se trata de una mesa con tablero de madera y patas metálicas negras de dimensiones 200x75x74 cm (largo x ancho x alto).
Modelo mesa en gazebo
Para contener los objetos vamos a crear un package que llamaremos "objetos_fer_modelos" con las siguientes dependencias: geometry_msgs gazebo urdf roscpp. Para el modelo de la mesa crearemos un archivo llamado "mesa.xacro" dentro del directorio "urdf" del package que acabamos de crear, con el siguiente contenido:
<?xml version="1.0"?>
<robot name="mesa" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="world" />
<joint name="mesa" type="floating">
<parent link="world"/>
<child link="mesa_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<link name="mesa_link">
<visual>
<geometry>
<box size="2 0.75 0.03" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
<material name="wood">
<color rgba="${205/255} ${133/255} ${63/255} 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
<geometry>
<box size="2 0.75 0.03" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${(0.03/2)+0.71}" />
<mass value="5" />
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
</inertial>
</link>
<xacro:macro name="pata" params="id refx refy">
<joint name="pata_${id}" type="fixed">
<parent link="mesa_link"/>
<child link="pata_${id}_link"/>
<origin xyz="${refx*((2/2)-0.045)} ${refy*((0.75/2)-0.045)} 0" rpy="0 0 0" />
</joint>
<link name="pata_${id}_link">
<visual>
<geometry>
<cylinder radius="0.025" length="0.71" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
<material name="black">
<color rgba="0 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
<geometry>
<cylinder radius="0.025" length="0.71" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.71/2}" />
<mass value="3" />
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
</inertial>
</link>
</xacro:macro>
<xacro:pata id="1" refx="1" refy="1"/>
<xacro:pata id="2" refx="1" refy="-1"/>
<xacro:pata id="3" refx="-1" refy="-1"/>
<xacro:pata id="4" refx="-1" refy="1"/>
<gazebo reference="mesa_link">
<material>Gazebo/LightWood</material>
<mu1>10</mu1>
<mu2>10</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<xacro:macro name="gazebo_pata" params="id">
<gazebo reference="pata_${id}_link">
<material>PR2/Black</material>
<mu1>0.9</mu1>
<mu2>0.9</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
</xacro:macro>
<xacro:gazebo_pata id="1"/>
<xacro:gazebo_pata id="2"/>
<xacro:gazebo_pata id="3"/>
<xacro:gazebo_pata id="4"/>
</robot>
Para el modelo de la mesa hemos empleado los materiales que vienen por defecto en gazebo, pero para las latas vamos a necesitar crear materiales nuevos con las texturas de las etiquetas.
Creación de nuevos materiales para gazebo
Comenzaremos creando dentro de nuestro package un directorio llamado "Media", dentro del cual crearemos otro directorio llamado "materials", y dentro de este crearemos otros dos directorios llamados "scripts" y "textures". También debemos añadir las siguientes líneas al archivo "manifest.xml" de nuestro package, para colocarlo en la ruta de gazebo:
<export>
<gazebo gazebo_media_path="${prefix}" />
</export>
Los materiales se definen para el motor gráfico OGRE. Para las latas vamos a necesitar asociar una imagen al material para que aplique una textura a la geometría. La imagen que usaremos para la textura debe guardarse en la carpeta "textures" creada dentro de nuestro package. Crearemos un archivo llamado "myrabot_fer.material" dentro del directorio "scripts" de nuestro package con el contenido que se muestra a continuación, para la definición de un nuevo material con textura:
material material_fer/NOMBRE_DEL_NUEVO_MATERIAL
{
technique
{
pass
{
lighting off
ambient 1 1 1 1
diffuse 1 1 1 1
specular 0 0 0 0
emissive 0 0 0
alpha_rejection greater 128
depth_write on
texture_unit
{
texture NUESTRO_ARCHIVO_DE_IMAGEN
tex_coord_set 0
colour_op modulate
rotate 180
scale 1 1
}
}
}
}Latas
Lata de cerveza
Para el modelo simplificado de la lata crearemos un archivo llamado "lata_cerveza.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:
<?xml version="1.0"?>
<robot name="lata_cerveza" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="world" />
<joint name="lata" type="floating">
<parent link="world"/>
<child link="lata_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<link name="lata_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.1115" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<material name="red">
<color rgba="1 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<geometry>
<cylinder radius="0.0325" length="0.1115" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<joint name="lata_top" type="fixed">
<parent link="lata_link"/>
<child link="lata_top_link"/>
<origin xyz="0 0 0.1185" rpy="0 0 0" />
</joint>
<link name="lata_top_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.0015" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<material name="grey">
<color rgba="0.8 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<geometry>
<cylinder radius="0.0325" length="0.0015" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
</link>
<joint name="lata_bottom" type="fixed">
<parent link="lata_link"/>
<child link="lata_bottom_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<link name="lata_bottom_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.007" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<material name="grey">
<color rgba="0.8 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<geometry>
<cylinder radius="0.0325" length="0.007" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
</link>
<gazebo reference="lata_link">
<material>material_fer/Amstel</material>
<mu1>100</mu1>
<mu2>100</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="lata_top_link">
<material>PR2/Grey0</material>
<mu1>100</mu1>
<mu2>100</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="lata_bottom_link">
<material>PR2/Grey0</material>
<mu1>100</mu1>
<mu2>100</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
</robot>
Ahora debemos añadir el material "Amstel" a gazebo. Copiaremos las siguientes líneas al final del archivo "myrabot_fer.material":
material material_fer/Amstel
{
technique
{
pass
{
lighting off
ambient 1 1 1 1
diffuse 1 1 1 1
specular 0 0 0 0
emissive 0 0 0
alpha_rejection greater 128
depth_write on
texture_unit
{
texture amstel.png
tex_coord_set 0
colour_op modulate
rotate 180
scale 1 1
}
}
}
}A continuación se muestra la imagen que se ha empleado para la textura y una captura del modelo en gazebo:
Lata de cerveza en gazebo
Lata de cola
Para el modelo simplificado de la lata crearemos un archivo llamado "lata_cola.xacro" dentro del directorio "urdf" de nuestro package con el siguiente contenido:
<?xml version="1.0"?>
<robot name="lata_cola" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="world" />
<joint name="lata" type="floating">
<parent link="world"/>
<child link="lata_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<link name="lata_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.1115" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<material name="red">
<color rgba="1 0 0 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<geometry>
<cylinder radius="0.0325" length="0.1115" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${(0.1115/2)+0.007}" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
</link>
<joint name="lata_top" type="fixed">
<parent link="lata_link"/>
<child link="lata_top_link"/>
<origin xyz="0 0 0.1185" rpy="0 0 0" />
</joint>
<link name="lata_top_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.0015" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<material name="grey">
<color rgba="0.8 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<geometry>
<cylinder radius="0.0325" length="0.0015" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.0015/2}" />
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
</link>
<joint name="lata_bottom" type="fixed">
<parent link="lata_link"/>
<child link="lata_bottom_link"/>
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<link name="lata_bottom_link">
<visual>
<geometry>
<cylinder radius="0.0325" length="0.007" />
</geometry>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<material name="grey">
<color rgba="0.8 0.8 0.8 1" />
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<geometry>
<cylinder radius="0.0325" length="0.007" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 ${0.007/2}" />
<mass value="0.001" />
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>
</link>
<gazebo reference="lata_link">
<material>material_fer/CocaCola</material>
<mu1>10</mu1>
<mu2>10</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="lata_top_link">
<material>PR2/Grey0</material>
<mu1>10</mu1>
<mu2>10</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="lata_bottom_link">
<material>PR2/Grey0</material>
<mu1>10</mu1>
<mu2>10</mu2>
<selfCollide>true</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
</robot>
Ahora debemos añadir el material "CocaCola" a gazebo. Copiaremos las siguientes líneas al final del archivo "myrabot_fer.material":
material Gazebo/CocaCola
{
technique
{
pass
{
lighting off
ambient 1 1 1 1
diffuse 1 1 1 1
specular 0 0 0 0
emissive 0 0 0
alpha_rejection greater 128
depth_write on
texture_unit
{
texture coca_cola.png
tex_coord_set 0
colour_op modulate
rotate 180
scale 1 1
}
}
}
}A continuación se muestra la imagen que se ha empleado para la textura y una captura del modelo en gazebo:
Lata de cola gazebo
Para poder cargar los modelo en gazebo debemos convertirlos a ".urdf", para extender las macros y calcular campos, de nuestros archivos ".xacro". Para esto simplemente deberemos ejecutar la siguiente secuencia de comandos en un terminal:
roscd objetos_fer_modelos
cd urdf
rosrun xacro xacro.py OBJETO.xacro > OBJETO.urdfEjecución
Para la carga de todos los modelos se ha creado un launcher, donde le indicamos a cada modelo sus coordenadas de inserción. Crearemos un archivo llamado "myrabot_latas.launch" dentro del directorio "launch" del package "myrabot_fer", con el sigiente contenido:
<launch>
<param name="/use_sim_time" value="true" />
<node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>
<node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0.55 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />
<node name="spawn_lata_coca_cola" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cola.urdf -urdf -x 0.38 -y -0.18 -z 0.74 -Y -1.57 -model lata_cocacola" respawn="false" output="screen" />
<node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.38 -y 0.17 -z 0.74 -Y -1.57 -model lata_amstel" respawn="false" output="screen" />
<include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>
</launch>
Para iniciar el launcher simplemente debemos ejecutar el siguiente comando en un terminal:
roslaunch myrabot_fer myrabot_latas.launchEn el siguiente vídeo se muestra al modelo del MYRAbot ejecutando el programa escoja, en el que el brazo señala el objeto que le indiquemos y pasado un breve período de tiempo regresa a su posición inicial. Para iniciar el programa ejecutaremos el siguiente comando en otro terminal:
roslaunch brazo_fer escoja.launchPara seleccionar el objeto a señalar publicaremos en el topic "selected_object" su número de identificación, ejecutando en un terminal el siguiente comando:
rostopic pub selected_object std_msgs/Int16 1 --onceSi todo va bien el resultado será similar al que se muestra en el siguiente vídeo:
Ejecución del programa seguir objetos
Programa para mover objetos en gazebo usando el teclado
Para poder desplazar los modelos de los objetos dentro de gazebo hemos escrito un pequeño programa que publica en el topic "gazebo/set_model_state" la posición del modelo del objeto indicado, en este caso "Lata_amstel", usando el teclado. Crearemos un archivo llamado "mover_objetos.cpp" que guardaremos en el directorio "src" del package que hemos creado anteriormente para contener los modelos de los objetos, con el siguiente contenido:
#include "ros/ros.h"
#include "gazebo/ModelState.h"
#include <signal.h>
#include <termios.h>
#include <stdio.h>
#define PI 3.14159265
#define L1 104
#define L2 104
#define Lp 60
#define KEYCODE_Xmas 0x43
#define KEYCODE_Xmenos 0x44
#define KEYCODE_Ymas 0x41
#define KEYCODE_Ymenos 0x42
#define KEYCODE_Zmas 0x77
#define KEYCODE_Zmenos 0x73
#define KEYCODE_XgiroMas 0x61
#define KEYCODE_XgiroMenos 0x64
#define KEYCODE_YgiroMas 0x65
#define KEYCODE_YgiroMenos 0x71
struct termios cooked, raw;
int cont = 0;
double retardo = 0.1;
gazebo::ModelState objeto;
void quit(int sig)
{
tcsetattr(0, TCSANOW, &cooked);
ros::shutdown();
exit(0);
}
void callback(const ros::TimerEvent&)
{
ros::NodeHandle n;
ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);
std::string modelo_objeto;
ros::NodeHandle nh("~");
nh.param("modelo_objeto", modelo_objeto, std::string("Lata_amstel"));
ros::Time anterior;
if (::cont == 0)
{
anterior = ros::Time::now();
::objeto.model_name = modelo_objeto;
::objeto.reference_frame = "world";
::objeto.pose.position.x = 0;
::objeto.pose.position.y = 0.5;
::objeto.pose.position.z = 0.74;
::objeto.pose.orientation.x = 0;
::objeto.pose.orientation.y = 0;
::objeto.pose.orientation.z = 0;
::objeto.pose.orientation.w = 1;
::cont = 1;
object_pub_.publish(::objeto);
}
char c;
// get the console in raw mode
tcgetattr(0, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
// Setting a new line, then end of file
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(0, TCSANOW, &raw);
puts("");
puts("#####################################################");
puts(" CONTROLES OBJETO");
puts("#####################################################");
puts("");
puts("Flecha arriba:_______ Subir objeto");
puts("Flecha abajo:________ Bajar objeto");
puts("Flecha izquierda:____ Desplazar objeto a la izquierda");
puts("Flecha derecha:______ Desplazar objeto a la derecha");
puts("Tecla W:_____________ Desplazar objeto hacia delante");
puts("Tecla S:_____________ Desplazar objeto hacia atrás");
puts("Tecla A:_____________ Inclinar objeto en eje x mas");
puts("Tecla D:_____________ Inclinar objeto en eje x menos");
puts("Tecla Q:_____________ Inclinar objeto en eje y mas");
puts("Tecla E:_____________ Inclinar objeto en eje y menos");
while (ros::ok())
{
// get the next event from the keyboard
if(read(0, &c, 1) < 0)
{
perror("read():");
exit(-1);
}
if (c == KEYCODE_Xmas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.x = ::objeto.pose.position.x + 0.005;
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Xmenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.x = ::objeto.pose.position.x - 0.005;
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Ymas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.z = ::objeto.pose.position.z + 0.005;
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Ymenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.z = ::objeto.pose.position.z - 0.005;
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Zmas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.y = ::objeto.pose.position.y + 0.005;
anterior = ros::Time::now();
}
}
if (c == KEYCODE_Zmenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.position.y = ::objeto.pose.position.y - 0.005;
anterior = ros::Time::now();
}
}
if (c == KEYCODE_XgiroMas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.orientation.x = ::objeto.pose.orientation.x + 0.1;
anterior = ros::Time::now();
}
}
if (c == KEYCODE_XgiroMenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.orientation.x = ::objeto.pose.orientation.x - 0.1;
anterior = ros::Time::now();
}
}
if (c == KEYCODE_YgiroMas)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.orientation.y = ::objeto.pose.orientation.y + 0.1;
anterior = ros::Time::now();
}
}
if (c == KEYCODE_YgiroMenos)
{
if (ros::Time::now() > anterior + ros::Duration(::retardo))
{
::objeto.pose.orientation.y = ::objeto.pose.orientation.y - 0.1;
anterior = ros::Time::now();
}
}
object_pub_.publish(::objeto);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "mover_objetos");
ros::NodeHandle n;
ros::Publisher object_pub_=n.advertise<gazebo::ModelState>("gazebo/set_model_state", 1);
signal(SIGINT,quit);
ros::Timer timer = n.createTimer(ros::Duration(1), callback);
ros::spin();
return 0;
}
Para compilar el programa añadiremos la siguiente línea al final del archivo "CMakeLists.txt" de nuestro package:
rosbuild_add_executable(mover_objetos src/mover_objetos.cpp)Para generar el ejecutable debemos ejecutar la siguiente secuencia de comandos en un terminal:
roscd objetos_fer_modelos
makeEjecución
Para la carga de todos los modelos se ha creado un launcher, donde le indicamos a cada modelo sus coordenadas de inserción. Crearemos un archivo llamado "myrabot_lata.launch" dentro del directorio "launch" del package "myrabot_fer", con el sigiente contenido:
<launch>
<param name="/use_sim_time" value="true" />
<node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen"/>
<node name="spawn_mesa" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/mesa.urdf -urdf -x 0.55 -y 0 -z 0 -Y -1.57 -model mesa" respawn="false" output="screen" />
<node name="spawn_lata_amstel" pkg="gazebo" type="spawn_model" args="-file $(find objetos_fer_modelos)/urdf/lata_cerveza.urdf -urdf -x 0.5 -y 0 -z 0.74 -Y -1.57 -model lata_amstel" respawn="false" output="screen" />
<include file="$(find myrabot_fer_modelo)/launch/myrabot_gazebo.launch"/>
</launch>
Para iniciar el launcher simplemente debemos ejecutar el siguiente comando en un terminal:
roslaunch myrabot_fer myrabot_lata.launchEn el siguiente vídeo se muestra al modelo del MYRAbot ejecutando el programa seguir objetos, en el que el brazo sigue el objeto que le indiquemos. Para iniciar el programa ejecutaremos el siguiente comando en otro terminal:
roslaunch brazo_fer seguir.launchPara seleccionar el objeto a señalar publicaremos en el topic "selected_object" su número de identificación, ejecutando en un terminal el siguiente comando:
rostopic pub selected_object std_msgs/Int16 1 --oncePara poder mover la lata, ejecutaremos en un nuevo terminal el siguiente comando, donde indicamos en el parámetro "modelo_objeto" el nombre del modelo en gazebo que queremos mover:
rosrun objetos_fer_modelos mover_objetos _modelo_objeto:=lata_amstelSi todo va bien el resultado será similar al que se muestra en el vídeo siguiente:
