Skip to content
Snippets Groups Projects
Commit 0daf6c3b authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added all the STL models and xacro and launch files for the challanges of the...

Added all the STL models and xacro and launch files for the challanges of the CEABOT robot competition, except for sumo.
parent 6a497459
No related branches found
No related tags found
No related merge requests found
Pipeline #
Showing
with 461 additions and 6 deletions
<launch>
<arg name="robot" default="bioloid_ceabot" />
<arg name="environment" default="obstacles_env" />
<include file="$(find bioloid_apps)/launch/ceabot/ceabot_base.launch">
<arg name="robot" value="$(arg robot)" />
</include>
<include file="$(find bioloid_description)/launch/obstacles_env.launch">
<arg name="environment" value="$(arg environment)" />
</include>
</launch>
<launch>
<arg name="robot" default="bioloid_ceabot" />
<arg name="environment" default="stairs_env" />
<include file="$(find bioloid_apps)/launch/ceabot/ceabot_base.launch">
<arg name="robot" value="$(arg robot)" />
</include>
<include file="$(find bioloid_description)/launch/stairs_env.launch">
<arg name="environment" value="$(arg environment)" />
</include>
</launch>
<launch>
<arg name="robot" default="bioloid_ceabot" />
<arg name="environment" default="vision_env" />
<include file="$(find bioloid_apps)/launch/ceabot/ceabot_base.launch">
<arg name="robot" value="$(arg robot)" />
</include>
<include file="$(find bioloid_description)/launch/vision_env.launch">
<arg name="environment" value="$(arg environment)" />
</include>
</launch>
......@@ -323,10 +323,7 @@ namespace bioloid_controller_cm510
user_init();
manager_init(num_servos);
if(exp_board_id!=-1)
{
std::cout << "init exp board" << std::endl;
exp_board_init(exp_board_id);
}
first=false;
}
......
<launch>
<arg name="environment" default="obstacles_env" />
<!-- Convert an xacro and put on parameter server -->
<param name="obstacles_environment"
command="$(find xacro)/xacro.py '$(find bioloid_description)/urdf/ceabot/$(arg environment).xacro'" />
<node name="spawn_obstacles" pkg="gazebo_ros" type="spawn_model" args="-param obstacles_environment -urdf -model obstacles -x 1.0 -y 0.25 -z 0.03 -R 0 -P 0 -Y 3.14159"/>
</launch>
<launch>
<arg name="environment" default="stairs_env" />
<!-- Convert an xacro and put on parameter server -->
<param name="stairs_environment"
command="$(find xacro)/xacro.py '$(find bioloid_description)/urdf/ceabot/$(arg environment).xacro'" />
<node name="spawn_stairs" pkg="gazebo_ros" type="spawn_model" args="-param stairs_environment -urdf -model obstacles -x 1.0 -y 0.25 -z 0.03 -R 0 -P 0 -Y 3.14159"/>
</launch>
<launch>
<arg name="environment" default="vision_env" />
<!-- Convert an xacro and put on parameter server -->
<param name="vision_environment"
command="$(find xacro)/xacro.py '$(find bioloid_description)/urdf/ceabot/$(arg environment).xacro'" />
<node name="spawn_vision" pkg="gazebo_ros" type="spawn_model" args="-param vision_environment -urdf -model obstacles -x 1.0 -y 1.25 -z 0.03 -R 0 -P 0 -Y 3.14159"/>
</launch>
File added
File added
File added
......@@ -50,4 +50,4 @@
<!-- Other tools can request additional information be placed here -->
</export>
</package>
\ No newline at end of file
</package>
<robot name="bioloid" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find bioloid_description)/urdf/bioloid.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/sharp_ir.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/feet_ir.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/sensors/sharp_ir.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/sensors/feet_ir.xacro" />
<xacro:sharp_ir name="IR1" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="0 0.0 0.035" rpy="-1.5707 -1.5707 0" />
......
<?xml version="1.0"?>
<root xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find bioloid_description)/urdf/qrcodes/qrcode.xacro" />
<xacro:macro name="obstacle" params="name parent grid_x grid_y north_code south_code east_code west_code">
<!-- obstacle -->
<link name="${name}_link">
<inertial>
<mass value="2"/>
<origin xyz="0.0 0.25 0.0" rpy="0 0 0"/>
<inertia ixx="0.05208333" ixy="0.0" ixz="0.0" iyy="0.02083333" iyz="0.0" izz="0.17708333" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://bioloid_description/meshes/ceabot/obstacle.stl"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://bioloid_description/meshes/ceabot/obstacle.stl"/>
</geometry>
</collision>
</link>
<joint name="${name}_joint" type="fixed">
<origin xyz="${grid_x*0.25+0.13} ${grid_y*0.25+0.63} 0" rpy="1.5707 0 0"/>
<parent link="${parent}_link"/>
<child link="${name}_link"/>
</joint>
<xacro:qrcode name="${name}_north" parent="${name}" code="${north_code}">
<origin xyz="0.0 0.375 0.126" rpy="1.5707 0 0" />
</xacro:qrcode>
<xacro:qrcode name="${name}_south" parent="${name}" code="${south_code}">
<origin xyz="0.0 0.375 -0.126" rpy="-1.5707 0 0" />
</xacro:qrcode>
<xacro:qrcode name="${name}_west" parent="${name}" code="${west_code}">
<origin xyz="0.126 0.375 0" rpy="3.14159 0 1.5707" />
</xacro:qrcode>
<xacro:qrcode name="${name}_east" parent="${name}" code="${east_code}">
<origin xyz="-0.126 0.375 0.0" rpy="3.14159 0 -1.5707" />
</xacro:qrcode>-->
<gazebo reference="${name}_link">
</gazebo>
</xacro:macro>
</root>
<?xml version="1.0"?>
<root xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="obstacle_base" params="name">
<!-- IR distance sensors -->
<link name="${name}_link">
<inertial>
<mass value="50"/>
<origin xyz="1.01500000 1.26500000 0.04510729" rpy="0 0 0"/>
<inertia ixx="32.72577613" ixy="0.0" ixz="0.0" iyy="22.18978101" iyz="0.0" izz="53.20566219" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://bioloid_description/meshes/ceabot/obstacle_base.stl"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://bioloid_description/meshes/ceabot/obstacle_base.stl"/>
</geometry>
</collision>
</link>
<gazebo>
<static>1</static>
</gazebo>
</xacro:macro>
</root>
<?xml version="1.0"?>
<root xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find bioloid_description)/urdf/qrcodes/qrcode.xacro" />
<xacro:macro name="obstacle_vis" params="name parent center_x center_y distance angle cosine sine code">
<!-- obstacle -->
<link name="${name}_link">
<inertial>
<mass value="2"/>
<origin xyz="0.0 0.25 0.0" rpy="0 0 0"/>
<inertia ixx="0.05208333" ixy="0.0" ixz="0.0" iyy="0.02083333" iyz="0.0" izz="0.17708333" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://bioloid_description/meshes/ceabot/obstacle.stl"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://bioloid_description/meshes/ceabot/obstacle.stl"/>
</geometry>
</collision>
</link>
<joint name="${name}_joint" type="fixed">
<origin xyz="${center_x+distance*sine} ${center_y+distance*cosine} 0" rpy="1.5707 0 ${-angle}"/>
<parent link="${parent}_link"/>
<child link="${name}_link"/>
</joint>
<xacro:qrcode name="${name}_code" parent="${name}" code="${code}">
<origin xyz="0.0 0.375 0.126" rpy="1.5707 0 0" />
</xacro:qrcode>
<gazebo reference="${name}_link">
</gazebo>
</xacro:macro>
</root>
<robot name="bioloid" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find bioloid_description)/urdf/ceabot/obstacle_base.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/ceabot/obstacle.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/qrcodes/qrcode.xacro" />
<xacro:obstacle_base name="obstacle_base"/>
<xacro:obstacle name="obstacle1" parent="obstacle_base" grid_x="0" grid_y="0" north_code="N1" south_code="S1" east_code="E1" west_code="W1"/>
<xacro:obstacle name="obstacle2" parent="obstacle_base" grid_x="1" grid_y="1" north_code="N2" south_code="S2" east_code="E2" west_code="W2"/>
<xacro:obstacle name="obstacle3" parent="obstacle_base" grid_x="2" grid_y="2" north_code="N3" south_code="S3" east_code="E3" west_code="W3"/>
<xacro:obstacle name="obstacle4" parent="obstacle_base" grid_x="3" grid_y="3" north_code="N4" south_code="S4" east_code="E4" west_code="W4"/>
<xacro:obstacle name="obstacle5" parent="obstacle_base" grid_x="4" grid_y="4" north_code="N5" south_code="S5" east_code="E5" west_code="W5"/>
<xacro:obstacle name="obstacle6" parent="obstacle_base" grid_x="5" grid_y="5" north_code="N6" south_code="S6" east_code="E6" west_code="W6"/>
<xacro:qrcode name="contour_west_25" parent="obstacle_base" code="W25">
<origin xyz="0.016 0.265 0.375" rpy="0 0 -1.5707" />
</xacro:qrcode>
<xacro:qrcode name="contour_west_75" parent="obstacle_base" code="W75">
<origin xyz="0.016 0.765 0.375" rpy="0 0 -1.5707" />
</xacro:qrcode>
<xacro:qrcode name="contour_west_125" parent="obstacle_base" code="W125">
<origin xyz="0.016 1.265 0.375" rpy="0 0 -1.5707" />
</xacro:qrcode>
<xacro:qrcode name="contour_west_175" parent="obstacle_base" code="W175">
<origin xyz="0.016 1.765 0.375" rpy="0 0 -1.5707" />
</xacro:qrcode>
<xacro:qrcode name="contour_west_225" parent="obstacle_base" code="W225">
<origin xyz="0.016 2.265 0.375" rpy="0 0 -1.5707" />
</xacro:qrcode>
<xacro:qrcode name="contour_east_25" parent="obstacle_base" code="W25">
<origin xyz="1.999 0.265 0.375" rpy="0 0 1.5707" />
</xacro:qrcode>
<xacro:qrcode name="contour_east_75" parent="obstacle_base" code="W75">
<origin xyz="1.999 0.765 0.375" rpy="0 0 1.5707" />
</xacro:qrcode>
<xacro:qrcode name="contour_east_125" parent="obstacle_base" code="W125">
<origin xyz="1.999 1.265 0.375" rpy="0 0 1.5707" />
</xacro:qrcode>
<xacro:qrcode name="contour_east_175" parent="obstacle_base" code="W175">
<origin xyz="1.999 1.765 0.375" rpy="0 0 1.5707" />
</xacro:qrcode>
<xacro:qrcode name="contour_east_225" parent="obstacle_base" code="W225">
<origin xyz="1.999 2.265 0.375" rpy="0 0 1.5707" />
</xacro:qrcode>
<xacro:qrcode name="contour_north_25" parent="obstacle_base" code="N25">
<origin xyz="0.265 2.514 0.375" rpy="0 0 3.14159" />
</xacro:qrcode>
<xacro:qrcode name="contour_north_75" parent="obstacle_base" code="N75">
<origin xyz="0.765 2.514 0.375" rpy="0 0 3.14159" />
</xacro:qrcode>
<xacro:qrcode name="contour_north_125" parent="obstacle_base" code="N125">
<origin xyz="1.265 2.514 0.375" rpy="0 0 3.14159" />
</xacro:qrcode>
<xacro:qrcode name="contour_north_175" parent="obstacle_base" code="N175">
<origin xyz="1.765 2.514 0.375" rpy="0 0 3.14159" />
</xacro:qrcode>
<xacro:qrcode name="contour_south_25" parent="obstacle_base" code="S25">
<origin xyz="0.265 0.016 0.375" rpy="0 0 0" />
</xacro:qrcode>
<xacro:qrcode name="contour_south_75" parent="obstacle_base" code="S75">
<origin xyz="0.765 0.016 0.375" rpy="0 0 0" />
</xacro:qrcode>
<xacro:qrcode name="contour_south_125" parent="obstacle_base" code="S125">
<origin xyz="1.265 0.016 0.375" rpy="0 0 0" />
</xacro:qrcode>
<xacro:qrcode name="contour_south_175" parent="obstacle_base" code="S175">
<origin xyz="1.765 0.016 0.375" rpy="0 0 0" />
</xacro:qrcode>
</robot>
<?xml version="1.0"?>
<root xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="stairs" params="name parent orientation">
<!-- obstacle -->
<link name="${name}_link">
<inertial>
<mass value="2"/>
<origin xyz="0.0 0.25 0.0" rpy="0 0 0"/>
<inertia ixx="0.05208333" ixy="0.0" ixz="0.0" iyy="0.02083333" iyz="0.0" izz="0.17708333" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://bioloid_description/meshes/ceabot/stairs.stl"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://bioloid_description/meshes/ceabot/stairs.stl"/>
</geometry>
</collision>
</link>
<joint name="${name}_joint" type="fixed">
<origin xyz="1.0 1.25 0" rpy="0 0 ${orientation}"/>
<parent link="${parent}_link"/>
<child link="${name}_link"/>
</joint>
<gazebo reference="${name}_link">
</gazebo>
</xacro:macro>
</root>
<robot name="bioloid" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find bioloid_description)/urdf/ceabot/obstacle_base.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/ceabot/stairs.xacro" />
<xacro:obstacle_base name="obstacle_base"/>
<xacro:stairs name="srairs" parent="obstacle_base" orientation="0"/>
</robot>
<robot name="bioloid" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find bioloid_description)/urdf/ceabot/obstacle_base.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/ceabot/obstacle_vis.xacro" />
<xacro:obstacle_base name="obstacle_base"/>
<!-- 0 45 90 135 180 225 270 315 -->
<!-- cosine: 1 0.707 0 -0.707 -1 -0.707 0 0.707 -->
<!-- sine: 0 0.707 1 0.707 0 -0.707 -1 -0.707 -->
<xacro:obstacle_vis name="obstacle1" parent="obstacle_base" center_x="1.015" center_y="1.265" distance="0.75" angle="0" cosine="1" sine="0" code="N1"/>
<xacro:obstacle_vis name="obstacle2" parent="obstacle_base" center_x="1.015" center_y="1.265" distance="0.75" angle="0.79" cosine="0.707" sine="0.707" code="N1"/>
<xacro:obstacle_vis name="obstacle3" parent="obstacle_base" center_x="1.015" center_y="1.265" distance="0.75" angle="3.14159" cosine="-1" sine="0" code="N1"/>
<xacro:obstacle_vis name="obstacle4" parent="obstacle_base" center_x="1.015" center_y="1.265" distance="0.75" angle="3.93" cosine="-0.707" sine="0.707" code="N1"/>
<xacro:obstacle_vis name="obstacle5" parent="obstacle_base" center_x="1.015" center_y="1.265" distance="0.75" angle="4.71" cosine="0" sine="-1" code="N1"/>
</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="0.025400" name="centimeter"></unit><up_axis>Y_UP</up_axis></asset>
<library_images>
<image id="Map #1-image" name="Map #1"><init_from>E1.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.000000 0.000000 0.000000 1.000000</color>
</emission>
<ambient>
<color sid="ambient">0.588000 0.588000 0.588000 1.000000</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.000000 0.000000 0.000000 1.000000</color>
</specular>
<shininess>
<float sid="shininess">2.000000</float>
</shininess>
<reflective>
<color sid="reflective">0.000000 0.000000 0.000000 1.000000</color>
</reflective>
<reflectivity>
<float sid="reflectivity">1.000000</float>
</reflectivity>
<transparent opaque="RGB_ZERO">
<color sid="transparent">1.000000 1.000000 1.000000 1.000000</color>
</transparent>
<transparency>
<float sid="transparency">0.000000</float>
</transparency>
</phong>
</technique>
</profile_COMMON>
</effect>
</library_effects>
<library_geometries>
<geometry id="QR_Code-lib" name="QR_CodeMesh">
<mesh>
<source id="QR_Code-POSITION">
<float_array id="QR_Code-POSITION-array" count="12">
-3.149606 -3.149606 0.000000
3.149606 -3.149606 0.000000
-3.149606 3.149606 0.000000
3.149606 3.149606 0.000000
</float_array>
<technique_common>
<accessor source="#QR_Code-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="QR_Code-Normal0">
<float_array id="QR_Code-Normal0-array" count="18">
0.000000 0.000000 1.000000
0.000000 0.000000 1.000000
0.000000 0.000000 1.000000
0.000000 0.000000 1.000000
0.000000 0.000000 1.000000
0.000000 0.000000 1.000000
</float_array>
<technique_common>
<accessor source="#QR_Code-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="QR_Code-UV0">
<float_array id="QR_Code-UV0-array" count="8">
0.000499 0.000500
0.999500 0.000499
0.000500 0.999501
0.999501 0.999500
</float_array>
<technique_common>
<accessor source="#QR_Code-UV0-array" count="4" stride="2">
<param name="S" type="float"/>
<param name="T" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="QR_Code-VERTEX">
<input semantic="POSITION" source="#QR_Code-POSITION"/>
</vertices>
<triangles count="2" material="Material #36"><input semantic="VERTEX" offset="0" source="#QR_Code-VERTEX"/><input semantic="NORMAL" offset="1" source="#QR_Code-Normal0"/><input semantic="TEXCOORD" offset="2" set="0" source="#QR_Code-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="QR_Code" id="QR_Code" sid="QR_Code"><matrix sid="matrix">1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000</matrix><instance_geometry url="#QR_Code-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.000000</visibility></technique></extra></node>
<extra><technique profile="MAX3D"><frame_rate>30.000000</frame_rate></technique><technique profile="FCOLLADA"><start_time>0.000000</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>
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