Difference between revisions of "MYRAbot model for simulation (urdf+gazebo)"
From robotica.unileon.es
| Line 20: | Line 20: | ||
| valign="top" | | | valign="top" | | ||
| − | [[CeRVaNTeS' arm control (maxon+epos2)]]<br/> | + | [[CeRVaNTeS' arm and base control (maxon+epos2)]]<br/> |
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]]<br/> | [[CeRVaNTeS' arm model for simulation (urdf+gazebo)]]<br/> | ||
[[CeRVaNTeS model for simulation (urdf+gazebo)]]<br/> | [[CeRVaNTeS model for simulation (urdf+gazebo)]]<br/> | ||
| Line 3,217: | Line 3,217: | ||
| valign="top" | | | valign="top" | | ||
| − | [[CeRVaNTeS' arm control (maxon+epos2)]]<br/> | + | [[CeRVaNTeS' arm and base control (maxon+epos2)]]<br/> |
[[CeRVaNTeS' arm model for simulation (urdf+gazebo)]]<br/> | [[CeRVaNTeS' arm model for simulation (urdf+gazebo)]]<br/> | ||
[[CeRVaNTeS model for simulation (urdf+gazebo)]]<br/> | [[CeRVaNTeS model for simulation (urdf+gazebo)]]<br/> | ||
Latest revision as of 11:10, 29 September 2014
Contents
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 roscppURDF 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:
- 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
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 roscppWe 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(¶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_;
}
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
makeURDF
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.launchThe 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:
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.launchWe 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 roscppWe 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
makeWe will execute the next command in a terminal in order to launch gazebo with the robot model:
roslaunch myrabot_fer_modelo myrabot_gazebo.launchWe will execute the next command in a new terminal in order to launch the test program:
rosrun myrabot_fer vuelta_al_ruedoWe 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}' --onceThe 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:
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 roscppTable
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:
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:
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.urdfExecution
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.launchWe will execute in a new terminal the next command in order to start the program Choose:
roslaunch brazo_fer escoja.launchWe 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 --onceThe arm points the selected object and after a time the arm returns to the home position. We can see this in the next video:
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
makeExecution
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.launchWe will execute the next command in a new terminal in order to launch the tracking objects program:
roslaunch brazo_fer seguir.launchWe 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 --onceWe 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_amstelThe arm follows the selected object. We can see this in the next video:
