Skip to content
Snippets Groups Projects
Commit 04541f9b authored by Fernando Herrero's avatar Fernando Herrero
Browse files

Add semaphores and meshes

parent c17b0509
No related branches found
No related tags found
1 merge request!3Add inverted alvar png/dae. Update sign, semaphore, localization_sign xacros...
Showing with 939 additions and 9 deletions
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="name" default="semaphore"/>
<arg name="model" default="semaphore"/>
<arg name="tag" default="alvar0"/>
<arg name="x" default="0.5"/>
<arg name="y" default="-0.5"/>
<arg name="yaw" default="3.14159"/>
<arg name="parent" default="map"/>
<param name="$(arg name)_description"
command="$(find xacro)/xacro '$(find iri_sign_description)/urdf/$(arg model).xacro'
name:=$(arg name)
tag:=$(arg tag)">
</param>
<node name="$(arg name)_state_publisher"
pkg ="robot_state_publisher"
type="robot_state_publisher">
<!--<param name="tf_prefix" value="/$(arg name)" type="str" />-->
<remap from="robot_description" to="$(arg name)_description" />
<remap from="/joint_states" to="/$(arg name)/joint_states" />
</node>
<node name="spawn_urdf_$(arg name)"
pkg ="gazebo_ros"
type="spawn_model"
args="-param /$(arg name)_description -urdf -model $(arg name) -x $(arg x) -y $(arg y) -z 0 -Y $(arg yaw)">
</node>
<node name="static_tf_$(arg name)_base_link_to_$(arg parent)" pkg="tf" type="static_transform_publisher"
args="$(arg x) $(arg y) 0 $(arg yaw) 0 0 $(arg parent) $(arg name)_base_link 100">
</node>
</launch>
This diff is collapsed.
File added
File added
File added
File added
File added
<?xml version="1.0"?>
<robot name="$(arg name)" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find iri_sign_description)/urdf/signs/semaphore_macro.xacro" />
<xacro:semaphore name="$(arg name)"
tag_dae="$(arg tag)">
</xacro:semaphore>
</robot>
<?xml version="1.0" encoding="utf-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
<asset>
<contributor>
<author></author>
<authoring_tool>FBX COLLADA exporter</authoring_tool>
<comments></comments>
</contributor>
<created>2014-10-05T08:59:28Z</created>
<keywords></keywords>
<modified>2014-10-05T08:59:28Z</modified>
<revision></revision>
<subject></subject>
<title></title>
<unit meter="1.0" name="meter"></unit>
<up_axis>Y_UP</up_axis></asset>
<library_images>
<image id="Map #1-image" name="Map #1">
<init_from>png/alvar0_negative.png</init_from>
</image>
</library_images>
<library_materials>
<material id="Material #36" name="Material #36">
<instance_effect url="#Material #36-fx"/>
</material>
</library_materials>
<library_effects>
<effect id="Material #36-fx" name="Material #36">
<profile_COMMON>
<technique sid="standard">
<phong>
<emission>
<color sid="emission">0 0 0 1</color>
</emission>
<ambient>
<color sid="ambient">1 1 1 1</color>
</ambient>
<diffuse>
<texture texture="Map #1-image" texcoord="CHANNEL0">
<extra>
<technique profile="MAYA">
<wrapU sid="wrapU0">TRUE</wrapU>
<wrapV sid="wrapV0">TRUE</wrapV>
<blend_mode>ADD</blend_mode>
</technique>
</extra>
</texture>
</diffuse>
<specular>
<color sid="specular">0 0 0 1</color>
</specular>
<shininess>
<float sid="shininess">2</float>
</shininess>
<reflective>
<color sid="reflective">0 0 0 1</color>
</reflective>
<reflectivity>
<float sid="reflectivity">1</float>
</reflectivity>
<transparent opaque="RGB_ZERO">
<color sid="transparent">1 1 1 1</color>
</transparent>
<transparency>
<float sid="transparency">0</float>
</transparency>
</phong>
</technique>
</profile_COMMON>
</effect>
</library_effects>
<library_geometries>
<geometry id="image-lib" name="image">
<mesh>
<source id="image-POSITION">
<float_array id="image-POSITION-array" count="12">
-0.5 -0.5 0
0.5 -0.5 0
-0.5 0.5 0
0.5 0.5 0
</float_array>
<technique_common>
<accessor source="#image-POSITION-array" count="4" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="image-Normal0">
<float_array id="image-Normal0-array" count="18">
0 0 1
0 0 1
0 0 1
0 0 1
0 0 1
0 0 1
</float_array>
<technique_common>
<accessor source="#image-Normal0-array" count="6" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="image-UV0">
<float_array id="image-UV0-array" count="8">
0 0
1 0
0 1
1 1
</float_array>
<!--
0.000499 0.000500
0.999500 0.000499
0.000500 0.999501
0.999501 0.999500
-->
<technique_common>
<accessor source="#image-UV0-array" count="4" stride="2">
<param name="S" type="float"/>
<param name="T" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="image-VERTEX">
<input semantic="POSITION" source="#image-POSITION"/>
</vertices>
<triangles count="2" material="Material #36">
<input semantic="VERTEX" offset="0" source="#image-VERTEX"/>
<input semantic="NORMAL" offset="1" source="#image-Normal0"/>
<input semantic="TEXCOORD" offset="2" set="0" source="#image-UV0"/>
<p> 0 0 0 1 1 1 3 2 3 3 3 3 2 4 2 0 5 0</p>
</triangles>
</mesh>
</geometry>
</library_geometries>
<library_visual_scenes>
<visual_scene id="" name="">
<node name="image" id="image" sid="image">
<matrix sid="matrix">1 0 0 0 0 0 1 0 0 -1 0 0 0 0 0 1</matrix>
<instance_geometry url="#image-lib">
<bind_material>
<technique_common>
<instance_material symbol="Material #36" target="#Material #36"/>
</technique_common>
</bind_material>
</instance_geometry>
<extra>
<technique profile="FCOLLADA">
<visibility>1</visibility>
</technique>
</extra>
</node>
<extra>
<technique profile="MAX3D">
<frame_rate>30</frame_rate>
</technique>
<technique profile="FCOLLADA">
<start_time>0</start_time>
<end_time>3.333333</end_time>
</technique>
</extra>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#"></instance_visual_scene>
</scene>
</COLLADA>
<?xml version="1.0" encoding="utf-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
<asset>
<contributor>
<author></author>
<authoring_tool>FBX COLLADA exporter</authoring_tool>
<comments></comments>
</contributor>
<created>2014-10-05T08:59:28Z</created>
<keywords></keywords>
<modified>2014-10-05T08:59:28Z</modified>
<revision></revision>
<subject></subject>
<title></title>
<unit meter="1.0" name="meter"></unit>
<up_axis>Y_UP</up_axis></asset>
<library_images>
<image id="Map #1-image" name="Map #1">
<init_from>png/alvar1_negative.png</init_from>
</image>
</library_images>
<library_materials>
<material id="Material #36" name="Material #36">
<instance_effect url="#Material #36-fx"/>
</material>
</library_materials>
<library_effects>
<effect id="Material #36-fx" name="Material #36">
<profile_COMMON>
<technique sid="standard">
<phong>
<emission>
<color sid="emission">0 0 0 1</color>
</emission>
<ambient>
<color sid="ambient">1 1 1 1</color>
</ambient>
<diffuse>
<texture texture="Map #1-image" texcoord="CHANNEL0">
<extra>
<technique profile="MAYA">
<wrapU sid="wrapU0">TRUE</wrapU>
<wrapV sid="wrapV0">TRUE</wrapV>
<blend_mode>ADD</blend_mode>
</technique>
</extra>
</texture>
</diffuse>
<specular>
<color sid="specular">0 0 0 1</color>
</specular>
<shininess>
<float sid="shininess">2</float>
</shininess>
<reflective>
<color sid="reflective">0 0 0 1</color>
</reflective>
<reflectivity>
<float sid="reflectivity">1</float>
</reflectivity>
<transparent opaque="RGB_ZERO">
<color sid="transparent">1 1 1 1</color>
</transparent>
<transparency>
<float sid="transparency">0</float>
</transparency>
</phong>
</technique>
</profile_COMMON>
</effect>
</library_effects>
<library_geometries>
<geometry id="image-lib" name="image">
<mesh>
<source id="image-POSITION">
<float_array id="image-POSITION-array" count="12">
-0.5 -0.5 0
0.5 -0.5 0
-0.5 0.5 0
0.5 0.5 0
</float_array>
<technique_common>
<accessor source="#image-POSITION-array" count="4" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="image-Normal0">
<float_array id="image-Normal0-array" count="18">
0 0 1
0 0 1
0 0 1
0 0 1
0 0 1
0 0 1
</float_array>
<technique_common>
<accessor source="#image-Normal0-array" count="6" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="image-UV0">
<float_array id="image-UV0-array" count="8">
0 0
1 0
0 1
1 1
</float_array>
<!--
0.000499 0.000500
0.999500 0.000499
0.000500 0.999501
0.999501 0.999500
-->
<technique_common>
<accessor source="#image-UV0-array" count="4" stride="2">
<param name="S" type="float"/>
<param name="T" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="image-VERTEX">
<input semantic="POSITION" source="#image-POSITION"/>
</vertices>
<triangles count="2" material="Material #36">
<input semantic="VERTEX" offset="0" source="#image-VERTEX"/>
<input semantic="NORMAL" offset="1" source="#image-Normal0"/>
<input semantic="TEXCOORD" offset="2" set="0" source="#image-UV0"/>
<p> 0 0 0 1 1 1 3 2 3 3 3 3 2 4 2 0 5 0</p>
</triangles>
</mesh>
</geometry>
</library_geometries>
<library_visual_scenes>
<visual_scene id="" name="">
<node name="image" id="image" sid="image">
<matrix sid="matrix">1 0 0 0 0 0 1 0 0 -1 0 0 0 0 0 1</matrix>
<instance_geometry url="#image-lib">
<bind_material>
<technique_common>
<instance_material symbol="Material #36" target="#Material #36"/>
</technique_common>
</bind_material>
</instance_geometry>
<extra>
<technique profile="FCOLLADA">
<visibility>1</visibility>
</technique>
</extra>
</node>
<extra>
<technique profile="MAX3D">
<frame_rate>30</frame_rate>
</technique>
<technique profile="FCOLLADA">
<start_time>0</start_time>
<end_time>3.333333</end_time>
</technique>
</extra>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#"></instance_visual_scene>
</scene>
</COLLADA>
urdf/signs/dae/png/alvar0_negative.png

293 B

urdf/signs/dae/png/alvar1_negative.png

311 B

urdf/signs/dae/png/qr_loc88b.png

886 B

<?xml version="1.0"?>
<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find iri_sign_description)/urdf/materials.xacro" />
<xacro:property name="PI" value="3.1415926535897931" />
<xacro:property name="box_width" value="0.07" />
<xacro:property name="box_depth" value="0.07" />
<xacro:property name="box_height" value="0.175" />
<xacro:property name="box_material" value="white" />
<xacro:property name="tag_size" value="0.05" />
<xacro:property name="tag_z_offset" value="0.065" />
<xacro:property name="delta" value="0.0001" />
<xacro:property name="top_panel_width" value="0.05" />
<xacro:property name="top_panel_height" value="0.05" />
<xacro:property name="top_panel_z_offset" value="0.12" />
<xacro:macro name="semaphore"
params="name:=semaphore
tag_dae:=alvar0_negative
box_gazebo_material:=Gazebo/Black
light_gazebo_material:=Gazebo/Green
light_gazebo_material2:=Gazebo/Red">
<link name="${name}_base_link">
<visual>
<geometry>
<!--<box size="${box_depth} ${box_width} ${box_height}"/>-->
<mesh filename="package://iri_sign_description/meshes/semaphore_case.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="${box_material}"/>
</visual>
<collision>
<geometry>
<mesh filename="package://iri_sign_description/meshes/semaphore_case.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<gazebo reference="${name}_base_link">
<material>${box_gazebo_material}</material>
</gazebo>
<link name="${name}_light_link">
<visual>
<geometry>
<mesh filename="package://iri_sign_description/meshes/semaphore_light.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
<visual>
<origin xyz="-0.042 0 0" rpy="0 0 3.14159" />
<geometry>
<mesh filename="package://iri_sign_description/meshes/semaphore_light.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://iri_sign_description/meshes/semaphore_light.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<gazebo reference="${name}_light_link">
<!-- <material>${light_gazebo_material}</material> -->
<visual>
<plugin name="${name}_color_plugin" filename="libcolor_plugin.so" >
<material>${light_gazebo_material}</material>
<material2>${light_gazebo_material2}</material2>
</plugin>
</visual>
</gazebo>
<joint name="joint_${name}_base_link_to_${name}_light_link" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="${name}_base_link"/>
<child link="${name}_light_link" />
</joint>
<link name="${name}_tag_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
</inertial>
</link>
<joint name="joint_${name}_base_link_to_${name}_tag_link" type="fixed">
<origin xyz="${delta} 0 ${tag_size/2.0+tag_z_offset}" rpy="0 0 0" />
<parent link="${name}_base_link"/>
<child link="${name}_tag_link" />
</joint>
<gazebo reference="${name}_tag_link">
<material>${box_gazebo_material}</material>
</gazebo>
<link name="${name}_tag_image">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://iri_sign_description/urdf/signs/dae/${tag_dae}.dae" scale="${tag_size} 1.0 ${tag_size}"/>
</geometry>
</visual>
</link>
<joint name="joint_${name}_tag_link_to_${name}_tag_image" type="fixed">
<origin xyz="0 0 0" rpy="0 ${PI} -${PI/2}" />
<parent link="${name}_tag_link"/>
<child link="${name}_tag_image" />
</joint>
<gazebo reference="${name}_tag_image">
</gazebo>
<gazebo>
<static>true</static>
</gazebo>
<!--
<gazebo>
<plugin name="${name}_ground_truth" filename="libgazebo_ros_p3d.so">
<frameName>map</frameName>
<bodyName>${name}_base_link</bodyName>
<topicName>${name}_odom_ground_truth</topicName>
<updateRate>5.0</updateRate>
</plugin>
</gazebo>
-->
</xacro:macro>
</root>
......@@ -9,32 +9,31 @@
<xacro:property name="box_depth" value="0.07" />
<xacro:property name="box_height" value="0.175" />
<xacro:property name="box_material" value="white" />
<xacro:property name="box_gazebo_material" value="Gazebo/White" />
<xacro:property name="tag_size" value="0.05" />
<xacro:property name="tag_z_offset" value="0.065" />
<xacro:property name="tag_z_offset" value="0.04" />
<xacro:property name="delta" value="0.0001" />
<xacro:property name="top_panel_width" value="0.05" />
<xacro:property name="top_panel_height" value="0.05" />
<xacro:property name="top_panel_z_offset" value="0.12" />
<xacro:property name="top_panel_z_offset" value="0.10" />
<xacro:macro name="sign"
params="name:=sign
tag_dae:=alvar0
tag_dae:=alvar0_negative
top_panel_dae:=stop
box_gazebo_material:=Gazebo/White">
box_gazebo_material:=Gazebo/Black
light_gazebo_material:=Gazebo/Black">
<link name="${name}_base_link">
<visual>
<origin xyz="-${box_depth/2.0} 0 ${box_height/2.0}" rpy="0 0 0"/>
<geometry>
<box size="${box_depth} ${box_width} ${box_height}"/>
<!--<box size="${box_depth} ${box_width} ${box_height}"/>-->
<mesh filename="package://iri_sign_description/meshes/semaphore_case.dae" scale="0.001 0.001 0.001"/>
</geometry>
<material name="${box_material}"/>
</visual>
<collision>
<origin xyz="-${box_depth/2.0} 0 ${box_height/2.0}" rpy="0 0 0"/>
<geometry>
<box size="${box_depth} ${box_width} ${box_height}"/>
<mesh filename="package://iri_sign_description/meshes/semaphore_case.dae" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
......@@ -42,6 +41,31 @@
<gazebo reference="${name}_base_link">
<material>${box_gazebo_material}</material>
</gazebo>
<link name="${name}_light_link">
<visual>
<geometry>
<!--<box size="${box_depth} ${box_width} ${box_height}"/>-->
<mesh filename="package://iri_sign_description/meshes/semaphore_light.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="${box_material}"/>
</visual>
<collision>
<geometry>
<mesh filename="package://iri_sign_description/meshes/semaphore_light.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<gazebo reference="${name}_light_link">
<material>${light_gazebo_material}</material>
</gazebo>
<joint name="joint_${name}_base_link_to_${name}_light_link" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="${name}_base_link"/>
<child link="${name}_light_link" />
</joint>
<link name="${name}_tag_link">
<inertial>
......
<?xml version="1.0"?>
<root xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find iri_sign_description)/urdf/materials.xacro" />
<xacro:property name="PI" value="3.1415926535897931" />
<xacro:property name="box_width" value="0.07" />
<xacro:property name="box_depth" value="0.07" />
<xacro:property name="box_height" value="0.175" />
<xacro:property name="box_material" value="white" />
<xacro:property name="box_gazebo_material" value="Gazebo/White" />
<xacro:property name="tag_size" value="0.05" />
<xacro:property name="tag_z_offset" value="0.065" />
<xacro:property name="delta" value="0.0001" />
<xacro:property name="top_panel_width" value="0.05" />
<xacro:property name="top_panel_height" value="0.05" />
<xacro:property name="top_panel_z_offset" value="0.12" />
<xacro:macro name="sign"
params="name:=sign
tag_dae:=alvar0
top_panel_dae:=stop
box_gazebo_material:=Gazebo/White">
<link name="${name}_base_link">
<visual>
<origin xyz="-${box_depth/2.0} 0 ${box_height/2.0}" rpy="0 0 0"/>
<geometry>
<box size="${box_depth} ${box_width} ${box_height}"/>
</geometry>
<material name="${box_material}"/>
</visual>
<collision>
<origin xyz="-${box_depth/2.0} 0 ${box_height/2.0}" rpy="0 0 0"/>
<geometry>
<box size="${box_depth} ${box_width} ${box_height}"/>
</geometry>
</collision>
</link>
<gazebo reference="${name}_base_link">
<material>${box_gazebo_material}</material>
</gazebo>
<link name="${name}_tag_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
</inertial>
</link>
<joint name="joint_${name}_base_link_to_${name}_tag_link" type="fixed">
<origin xyz="${delta} 0 ${tag_size/2.0+tag_z_offset}" rpy="0 0 0" />
<parent link="${name}_base_link"/>
<child link="${name}_tag_link" />
</joint>
<gazebo reference="${name}_tag_link">
<material>${box_gazebo_material}</material>
</gazebo>
<link name="${name}_tag_image">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://iri_sign_description/urdf/signs/dae/${tag_dae}.dae" scale="${tag_size} 1.0 ${tag_size}"/>
</geometry>
</visual>
</link>
<joint name="joint_${name}_tag_link_to_${name}_tag_image" type="fixed">
<origin xyz="0 0 0" rpy="0 ${PI} -${PI/2}" />
<parent link="${name}_tag_link"/>
<child link="${name}_tag_image" />
</joint>
<gazebo reference="${name}_tag_image">
</gazebo>
<link name="${name}_top_panel_link">
</link>
<joint name="joint_${name}_base_link_to_${name}_top_panel_link" type="fixed">
<origin xyz="${delta} 0 ${top_panel_height/2.0+top_panel_z_offset}" rpy="0 0 0"/>
<parent link="${name}_base_link"/>
<child link="${name}_top_panel_link"/>
</joint>
<gazebo reference="${name}_top_panel_link">
</gazebo>
<link name="${name}_top_panel_image">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://iri_sign_description/urdf/signs/dae/${top_panel_dae}.dae" scale="${top_panel_width} 1.0 ${top_panel_height}"/>
<!--<box size="${top_panel_width} ${delta} ${top_panel_height}"/>-->
</geometry>
<material name="${box_material}"/>
</visual>
</link>
<gazebo reference="${name}_top_panel_image">
</gazebo>
<joint name="joint_${name}_top_panel_link_to_${name}_top_panel_image" type="fixed">
<origin xyz="0 0 0" rpy="0 ${PI} -${PI/2}" />
<parent link="${name}_top_panel_link"/>
<child link="${name}_top_panel_image" />
</joint>
<gazebo>
<static>true</static>
</gazebo>
<!--
<gazebo>
<plugin name="${name}_ground_truth" filename="libgazebo_ros_p3d.so">
<frameName>map</frameName>
<bodyName>${name}_base_link</bodyName>
<topicName>${name}_odom_ground_truth</topicName>
<updateRate>5.0</updateRate>
</plugin>
</gazebo>
-->
</xacro:macro>
</root>
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment