forked from xtdrone/XTDrone
124 lines
4.7 KiB
XML
124 lines
4.7 KiB
XML
<?xml version="1.0" ?>
|
|
<!-- World containing sandisland model and some course challenges -->
|
|
<world xmlns:xacro="http://ros.org/wiki/xacro">
|
|
<xacro:macro name="dock" params="model_name:=robotx_dock_2018
|
|
uri:=dock_2018 x:=60 y:=-2.75 z:=0
|
|
R:=0 P:=0 Y:=0 bay1_allowed:=false bay1_symbol:=red_circle
|
|
bay2_allowed:=true bay2_symbol:=blue_cross
|
|
light_buoy_present:=false
|
|
color_1:=red color_2:=green color_3:=blue
|
|
lb_x:=90 lb_y:=70 lb_z:=0 lb_R:=0 lb_P:=0 lb_Y:=0
|
|
enable_color_checker:=false
|
|
task_name:=dock">
|
|
<!-- The 2018 dock with the two placards -->
|
|
<include>
|
|
<uri>model://${uri}</uri>
|
|
<pose>${x} ${y} ${z} ${R} ${P} ${Y}</pose>
|
|
</include>
|
|
|
|
<!-- Add a ligth buoy if doing Scan and Dock -->
|
|
<xacro:if value="${light_buoy_present}">
|
|
<include>
|
|
<uri>model://robotx_light_buoy</uri>
|
|
<pose>${lb_x} ${lb_y} ${lb_z} ${lb_R} ${lb_P} ${lb_Y}</pose>
|
|
</include>
|
|
</xacro:if>
|
|
|
|
<!-- The scoring plugin -->
|
|
<plugin name="scan_dock_scoring_plugin"
|
|
filename="libscan_dock_scoring_plugin.so">
|
|
<!-- Parameters for scoring_plugin -->
|
|
<vehicle>wamv</vehicle>
|
|
<task_name>${task_name}</task_name>
|
|
<initial_state_duration>10</initial_state_duration>
|
|
<ready_state_duration>10</ready_state_duration>
|
|
<running_state_duration>300</running_state_duration>
|
|
<release_joints>
|
|
<joint>
|
|
<name>wamv_external_pivot_joint</name>
|
|
</joint>
|
|
<joint>
|
|
<name>wamv_external_riser</name>
|
|
</joint>
|
|
</release_joints>
|
|
|
|
<!-- Color sequence checker -->
|
|
<enable_color_checker>${enable_color_checker}</enable_color_checker>
|
|
<robot_namespace>vrx</robot_namespace>
|
|
<color_sequence_service>scan_dock/color_sequence</color_sequence_service>
|
|
<color_1>${color_1}</color_1>
|
|
<color_2>${color_2}</color_2>
|
|
<color_3>${color_3}</color_3>
|
|
|
|
<!-- Dock checkers -->
|
|
<bays>
|
|
<bay>
|
|
<name>bay1</name>
|
|
<internal_activation_topic>/vrx/dock_2018/bay_1_internal/contain</internal_activation_topic>
|
|
<external_activation_topic>/vrx/dock_2018/bay_1_external/contain</external_activation_topic>
|
|
<symbol_topic>/vrx/dock_2018_placard1/symbol</symbol_topic>
|
|
<min_dock_time>10.0</min_dock_time>
|
|
<dock_allowed>${bay1_allowed}</dock_allowed>
|
|
<symbol>${bay1_symbol}</symbol>
|
|
</bay>
|
|
|
|
<bay>
|
|
<name>bay2</name>
|
|
<internal_activation_topic>/vrx/dock_2018/bay_2_internal/contain</internal_activation_topic>
|
|
<external_activation_topic>/vrx/dock_2018/bay_2_external/contain</external_activation_topic>
|
|
<symbol_topic>/vrx/dock_2018_placard2/symbol</symbol_topic>
|
|
<min_dock_time>10.0</min_dock_time>
|
|
<dock_allowed>${bay2_allowed}</dock_allowed>
|
|
<symbol>${bay2_symbol}</symbol>
|
|
</bay>
|
|
</bays>
|
|
</plugin>
|
|
|
|
<!-- Triggers a message when the vehicle enters and exits the bay #1 -->
|
|
<plugin name="vehicle_docked_bay1" filename="libContainPlugin.so">
|
|
<entity>wamv::base_link</entity>
|
|
<namespace>vrx/dock_2018/bay_1_internal</namespace>
|
|
<pose frame="${model_name}::dock_2018_placard1::placard::link">0 -5 -1.5 0 0 0</pose>
|
|
<geometry>
|
|
<box>
|
|
<size>4 1.5 2</size>
|
|
</box>
|
|
</geometry>
|
|
</plugin>
|
|
<!-- Triggers a message when the vehicle enters and exits the bay #1 -->
|
|
<plugin name="vehicle_docked_bay1_exterior" filename="libContainPlugin.so">
|
|
<entity>wamv::base_link</entity>
|
|
<namespace>vrx/dock_2018/bay_1_external</namespace>
|
|
<pose frame="${model_name}::dock_2018_placard1::placard::link">0 -10 -1.5 0 0 0</pose>
|
|
<geometry>
|
|
<box>
|
|
<size>8 1.5 2</size>
|
|
</box>
|
|
</geometry>
|
|
</plugin>
|
|
|
|
<!-- Triggers a message when the vehicle enters and exits the bay #2 -->
|
|
<plugin name="vehicle_docked_bay2" filename="libContainPlugin.so">
|
|
<entity>wamv::base_link</entity>
|
|
<namespace>vrx/dock_2018/bay_2_internal</namespace>
|
|
<pose frame="${model_name}::dock_2018_placard2::placard::link">0 5 -1.5 0 0 0</pose>
|
|
<geometry>
|
|
<box>
|
|
<size>4 1.5 2</size>
|
|
</box>
|
|
</geometry>
|
|
</plugin>
|
|
<!-- Triggers a message when the vehicle enters and exits the bay #1 -->
|
|
<plugin name="vehicle_docked_bay2_exterior" filename="libContainPlugin.so">
|
|
<entity>wamv::base_link</entity>
|
|
<namespace>vrx/dock_2018/bay_2_external</namespace>
|
|
<pose frame="${model_name}::dock_2018_placard1::placard::link">0 10 -1.5 0 0 0</pose>
|
|
<geometry>
|
|
<box>
|
|
<size>8 1.5 2</size>
|
|
</box>
|
|
</geometry>
|
|
</plugin>
|
|
</xacro:macro>
|
|
</world>
|