diff --git a/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.config b/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.config new file mode 100644 index 000000000..2bf440cd0 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.config @@ -0,0 +1,20 @@ + + + robotx_dock_2024 + 1.0 + model.sdf + + + Max Vu + m.vu@ufl.edu + + + + Carlos Agüero + caguero@openrobotics.org + + + + The dock used in RobotX 2024. + + diff --git a/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf b/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf new file mode 100644 index 000000000..0c354b8ed --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf @@ -0,0 +1,349 @@ + + + + + true + + + + model://dock_2024_base + + + + + + 5.75 3 1.5 0 0 1.5707963267948966 + + true + 0 -0.2 0.25 0 0 3.14159 + model://placard_2024 + + + true + 0 0.07 0 0 0 0 + + + + + 0 0 0 1.571 0 0 + + + 1 1 0.001 + + + 1 + + + robotx_dock_2024::dock_2024_placard1::link_symbols::visual_square + + true + vrx/dock_2024_placard1 + shuffle + + + + + + + + + + 0 0 0 0 0 0 + placard::link + link_symbols + + + + + + + + + + + + 5.75 9 1.5 0 0 1.5707963267948966 + + true + 0 -0.2 0.25 0 0 3.14159 + model://placard_2024 + + + true + 0 0.07 0 0 0 0 + + + 0 0 0 1.571 0 0 + + + 1 1 0.001 + + + 1 + + + robotx_dock_2024::dock_2024_placard2::link_symbols::visual_square + + true + vrx/dock_2024_placard2 + shuffle + + + + + + + + + + 0 0 0 0 0 0 + placard::link + link_symbols + + + + + + + + + + + + 5.75 15 1.5 0 0 1.5707963267948966 + + true + 0 -0.2 0.25 0 0 3.14159 + model://placard_2024 + + + true + 0 0.07 0 0 0 0 + + 0 0 0 1.571 0 0 + + + 1 1 0.001 + + + 1 + + + robotx_dock_2024::dock_2024_placard3::link_symbols::visual_square + + + true + vrx/dock_2024_placard3 + shuffle + + + + + + + + + + 0 0 0 0 0 0 + placard::link + link_symbols + + + + + + + + + + + diff --git a/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf.erb b/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf.erb new file mode 100644 index 000000000..49b850bfe --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf.erb @@ -0,0 +1,215 @@ + + + + + true + + + + model://dock_2024_base + + <% + # Constants + dock_x = 5.75 + dock_y = 3 + dock_z = 1.5 + dock_roll = 0 + dock_pitch = 0 + dock_yaw = Math::PI / 2.0 + circle_offset_x = -0.06 + circle_offset_y = 0 + circle_offset_z = 0.6 + circle_offset_roll = 0 + circle_offset_pitch = Math::PI / 2.0 + circle_offset_yaw = -Math::PI / 2.0 + cross_offset_x = -0.06 + cross_offset_y = 0 + cross_offset_z = 0.6 + cross_offset_roll = 0 + cross_offset_pitch = Math::PI / 2.0 + cross_offset_yaw = -Math::PI / 2.0 + triangle_offset_x = -0.06 + triangle_offset_y = -0.5 + triangle_offset_z = 0.2 + triangle_offset_roll = Math::PI / 2.0 + triangle_offset_pitch = 0 + triangle_offset_yaw = 0 + placard_offset_x = 0 + placard_offset_y = 6 + placard_offset_z = 0 + placard_offset_roll = 0 + placard_offset_pitch = 0 + placard_offset_yaw = 0 + + # Variables + placard_x = dock_x + placard_y = dock_y + placard_z = dock_z + placard_roll = dock_roll + placard_pitch = dock_pitch + placard_yaw = dock_yaw + + circle_x = dock_x + circle_offset_x + circle_y = dock_y + circle_offset_y + circle_z = dock_z + circle_offset_z + circle_roll = dock_roll + circle_offset_roll + circle_pitch = dock_pitch + circle_offset_pitch + circle_yaw = dock_yaw + circle_offset_yaw + + cross_x = dock_x + cross_offset_x + cross_y = dock_y + cross_offset_y + cross_z = dock_z + cross_offset_z + cross_roll = dock_roll + cross_offset_roll + cross_pitch = dock_pitch + cross_offset_pitch + cross_yaw = dock_yaw + cross_offset_yaw + + triangle_x = dock_x + triangle_offset_x + triangle_y = dock_y + triangle_offset_y + triangle_z = dock_z + triangle_offset_z + triangle_roll = dock_roll + triangle_offset_roll + triangle_pitch = dock_pitch + triangle_offset_pitch + triangle_yaw = dock_yaw + triangle_offset_yaw + %> + <% for i in 1..3 %> + + + <%=placard_x%> <%=placard_y%> <%=placard_z%> <%=placard_roll%> <%=placard_pitch%> <%=placard_yaw%> + + true + 0 -0.2 0.25 0 0 3.14159 + model://placard_2024 + + + true + 0 0.07 0 0 0 0 + + + + robotx_dock_2024::dock_2024_placard<%= i %>::link_symbols::visual_square + + true + vrx/dock_2024_placard<%= i %> + shuffle + + + + 0 0 0 1.571 0 0 + + + 1.1176 1.1176 0.001 + + + 1 + + + + + + + + + 0 0 0 0 0 0 + placard::link + link_symbols + + + + + + + + <% + placard_x += placard_offset_x + placard_y += placard_offset_y + placard_z += placard_offset_z + placard_roll += placard_offset_roll + placard_pitch += placard_offset_pitch + placard_yaw += placard_offset_yaw + + circle_x += placard_offset_x + circle_y += placard_offset_y + circle_z += placard_offset_z + circle_roll += placard_offset_roll + circle_pitch += placard_offset_pitch + circle_yaw += placard_offset_yaw + + cross_x += placard_offset_x + cross_y += placard_offset_y + cross_z += placard_offset_z + cross_roll += placard_offset_roll + cross_pitch += placard_offset_pitch + cross_yaw += placard_offset_yaw + + triangle_x += placard_offset_x + triangle_y += placard_offset_y + triangle_z += placard_offset_z + triangle_roll += placard_offset_roll + triangle_pitch += placard_offset_pitch + triangle_yaw += placard_offset_yaw + %> + <% end %> + + diff --git a/NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.config b/NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.config new file mode 100644 index 000000000..d8d58d747 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.config @@ -0,0 +1,20 @@ + + + robotx_dock_2024_base + 1.0 + model.sdf + + + Max Vu + m.vu@ufl.edu + + + + Carlos Agüero + caguero@openrobotics.org + + + + The dock base used in RobotX 2024. + + diff --git a/NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.sdf b/NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.sdf new file mode 100644 index 000000000..bd7aa2e41 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.sdf @@ -0,0 +1,142 @@ + + + + + + + false + + + + + true + + + + + dock_block_0_0 + 0 0 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_0_3 + 0 6 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_0_6 + 0 12 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_0_9 + 0 18 0.25 0 0 0 + model://dock_block_4x4 + + + + + dock_block_1_0 + 2 0 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_1_3 + 2 6 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_1_6 + 2 12 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_1_9 + 2 18 0.25 0 0 0 + model://dock_block_4x4 + + + + + dock_block_2_0 + 4 0 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_2_3 + 4 6 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_2_6 + 4 12 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_2_9 + 4 18 0.25 0 0 0 + model://dock_block_4x4 + + + + + dock_block_3_0 + 6 0 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_1 + 6 2 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_2 + 6 4 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_3 + 6 6 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_4 + 6 8 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_5 + 6 10 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_6 + 6 12 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_7 + 6 14 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_8 + 6 16 0.25 0 0 0 + model://dock_block_4x4 + + + dock_block_3_9 + 6 18 0.25 0 0 0 + model://dock_block_4x4 + + + + + + + + diff --git a/NaviGator/simulation/navigator_gazebo/models/placard_2024/materials/textures/Placard.png b/NaviGator/simulation/navigator_gazebo/models/placard_2024/materials/textures/Placard.png new file mode 100755 index 000000000..8664df705 Binary files /dev/null and b/NaviGator/simulation/navigator_gazebo/models/placard_2024/materials/textures/Placard.png differ diff --git a/NaviGator/simulation/navigator_gazebo/models/placard_2024/meshes/placard_2024.dae b/NaviGator/simulation/navigator_gazebo/models/placard_2024/meshes/placard_2024.dae new file mode 100644 index 000000000..77861b7dc --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/placard_2024/meshes/placard_2024.dae @@ -0,0 +1,607 @@ + + + + + + FBX COLLADA exporter + + + 2021-08-31T22:23:17Z + + 2021-08-31T22:23:17Z + + + + <unit meter="0.010000" name="centimeter"/> + <up_axis>Z_UP</up_axis> + </asset> + <library_images> + <image id="MapFBXASC032FBXASC0351-image" name="MapFBXASC032FBXASC0351"> + <init_from>Placard.png</init_from> + </image> + </library_images> + <library_materials> + <material id="Placard" name="Placard"> + <instance_effect url="#Placard-fx"/> + </material> + </library_materials> + <library_effects> + <effect id="Placard-fx" name="Placard"> + <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="MapFBXASC032FBXASC0351-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="Box001-lib" name="Box001Mesh"> + <mesh> + <source id="Box001-POSITION"> + <float_array id="Box001-POSITION-array" count="192"> +-100.000000 -25.000000 -150.000000 +-100.000000 25.000000 -150.000000 +100.000000 25.000000 -150.000000 +100.000000 -25.000000 -150.000000 +-100.000000 -25.000000 150.000000 +100.000000 -25.000000 150.000000 +100.000000 25.000000 150.000000 +-100.000000 25.000000 150.000000 +0.000000 0.000000 -150.000000 +0.000000 0.000000 -150.000000 +0.000000 0.000000 -150.000000 +0.000000 0.000000 -150.000000 +-84.680214 14.826656 132.722687 +-84.680214 14.826664 82.722687 +-34.680214 14.826664 82.722687 +-34.680214 14.826656 132.722687 +0.000000 0.000000 -150.000000 +0.000000 0.000000 -150.000000 +0.000000 0.000000 -150.000000 +0.000000 0.000000 -150.000000 +48.787140 14.826658 120.222687 +48.787140 14.826662 95.222687 +73.787140 14.826662 95.222687 +73.787140 14.826658 120.222687 +100.000000 -25.000000 66.811462 +100.000000 25.000000 66.811462 +-100.000000 25.000000 66.811462 +-100.000000 -25.000000 66.811462 +-19.386715 -25.000000 66.811462 +25.193655 -25.000000 66.811462 +18.392944 -25.000000 150.000000 +-20.149960 -25.000000 150.000000 +-20.406273 25.000000 150.000000 +20.272606 25.000000 150.000000 +18.256310 25.000000 66.811462 +13.001312 25.000000 -150.000000 +-25.653687 25.000000 66.811462 +-39.329834 25.000000 -150.000000 +27.587723 -25.000000 -150.000000 +-12.759123 -25.000000 -150.000000 +0.000000 0.000000 -150.000000 +-84.680206 -24.999998 132.722687 +-87.685837 -25.000000 135.728302 +0.000000 0.000000 -150.000000 +-31.674593 -25.000000 135.728302 +-34.680214 -24.999998 132.722687 +0.000000 0.000000 -150.000000 +73.787140 -24.999998 120.222687 +76.792763 -25.000000 123.228302 +0.000000 0.000000 -150.000000 +76.792763 -25.000000 92.217072 +73.787140 -24.999998 95.222687 +0.000000 0.000000 -150.000000 +-84.680206 -24.999998 82.722687 +-87.685837 -25.000000 79.717072 +0.000000 0.000000 -150.000000 +48.787140 -24.999998 120.222687 +45.781517 -25.000000 123.228302 +0.000000 0.000000 -150.000000 +45.781517 -25.000000 92.217072 +48.787140 -24.999998 95.222687 +0.000000 0.000000 -150.000000 +-34.680214 -24.999998 82.722687 +-31.674593 -25.000000 79.717072 +</float_array> + <technique_common> + <accessor source="#Box001-POSITION-array" count="64" stride="3"> + <param name="X" type="float"/> + <param name="Y" type="float"/> + <param name="Z" type="float"/> + </accessor> + </technique_common> + </source> + <source id="Box001-Normal0"> + <float_array id="Box001-Normal0-array" count="828"> +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 +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 +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 0.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 0.000000 +0.000000 1.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 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 0.000000 0.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 +0.000000 0.000000 1.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 0.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 -0.000000 +0.000000 -1.000000 -0.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 +0.000000 0.000000 -1.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 0.000000 0.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 +0.000000 0.000000 1.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 0.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 -0.000000 +0.000000 -1.000000 -0.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 +0.000000 0.000000 -1.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 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 0.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 0.000000 +0.000000 1.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 0.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 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 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 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 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 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 0.000000 +0.000000 -1.000000 0.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 +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 +0.000000 0.000000 1.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 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 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 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 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 0.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 +0.000000 0.000000 -1.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 0.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 +0.000000 0.000000 -1.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 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 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 0.000000 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +-0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 -0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000000 -1.000000 0.000001 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +0.000001 -1.000000 0.000000 +</float_array> + <technique_common> + <accessor source="#Box001-Normal0-array" count="276" stride="3"> + <param name="X" type="float"/> + <param name="Y" type="float"/> + <param name="Z" type="float"/> + </accessor> + </technique_common> + </source> + <source id="Box001-UV0"> + <float_array id="Box001-UV0-array" count="232"> +0.546320 0.682655 +0.546320 0.723931 +0.522597 0.723931 +0.570855 0.629042 +0.570855 0.666927 +0.547133 0.666805 +0.406890 0.833552 +0.406890 0.818839 +0.418944 0.809071 +0.352896 0.630064 +0.376618 0.630064 +0.376618 0.732930 +0.521810 0.719682 +0.521810 0.748467 +0.418944 0.748467 +0.519765 0.629042 +0.519765 0.652764 +0.416899 0.652764 +0.352896 0.732930 +0.409559 0.180505 +0.409559 0.156782 +0.621670 0.104854 +0.376618 0.772398 +0.602774 0.104854 +0.409559 0.130203 +0.352896 0.772398 +0.428455 0.130203 +0.601961 0.081132 +0.601961 0.104854 +0.578239 0.104854 +0.416899 0.629042 +0.578239 0.131434 +0.578239 0.107712 +0.377431 0.652764 +0.609930 0.107712 +0.621791 0.107712 +0.597663 0.126608 +0.377431 0.629042 +0.597663 0.107712 +0.409559 0.183362 +0.418944 0.713193 +0.428455 0.183362 +0.578239 0.155760 +0.590100 0.155760 +0.590100 0.167621 +0.590100 0.134292 +0.379475 0.748467 +0.590100 0.153187 +0.522597 0.689576 +0.547133 0.629042 +0.418944 0.844563 +0.521810 0.810207 +0.521810 0.844563 +0.392177 0.833552 +0.379475 0.844563 +0.392177 0.818839 +0.379475 0.805845 +0.386247 0.782090 +0.412821 0.782090 +0.379475 0.710704 +0.379475 0.691404 +0.379475 0.653577 +0.418944 0.653577 +0.418944 0.692360 +0.521810 0.653577 +0.578239 0.081132 +0.521810 0.694853 +0.570855 0.685213 +0.547133 0.686105 +0.570855 0.723931 +0.578239 0.167621 +0.547133 0.723931 +0.522597 0.670433 +0.418944 0.787920 +0.546320 0.657827 +0.522597 0.629042 +0.546320 0.629042 +0.428455 0.156782 +0.379475 0.787558 +0.428455 0.180505 +0.379475 0.749673 +0.418944 0.749673 +0.428455 0.153925 +0.412821 0.755516 +0.409559 0.153925 +0.386247 0.755516 +0.602774 0.081132 +0.621670 0.081132 +0.597135 0.107712 +0.597135 0.131434 +0.621791 0.126608 +0.609930 0.126608 +0.428455 0.195223 +0.409559 0.195223 +0.521810 0.791065 +0.521810 0.749673 +0.461106 0.187098 +0.461106 0.175237 +0.449245 0.187098 +0.449245 0.175237 +0.443314 0.135636 +0.467036 0.135636 +0.467036 0.111913 +0.443314 0.111913 +0.609524 0.107712 +0.609524 0.126608 +0.578239 0.153187 +0.578239 0.134292 +0.462532 0.188524 +0.462532 0.173811 +0.447819 0.188524 +0.447819 0.173811 +0.441888 0.137062 +0.468462 0.137062 +0.468462 0.110487 +0.441888 0.110487 +</float_array> + <technique_common> + <accessor source="#Box001-UV0-array" count="116" stride="2"> + <param name="S" type="float"/> + <param name="T" type="float"/> + </accessor> + </technique_common> + </source> + <vertices id="Box001-VERTEX"> + <input semantic="POSITION" source="#Box001-POSITION"/> + </vertices> + <triangles count="92" material="Placard"> + <input semantic="VERTEX" offset="0" source="#Box001-VERTEX"/> + <input semantic="NORMAL" offset="1" source="#Box001-Normal0"/> + <input semantic="TEXCOORD" offset="2" set="0" source="#Box001-UV0"/> + <p> 38 0 48 2 1 1 3 2 2 2 3 1 38 4 48 35 5 0 32 6 5 4 7 3 31 8 4 4 9 3 32 10 5 7 11 49 24 12 18 2 13 10 25 14 11 2 15 10 24 16 18 3 17 9 36 18 40 37 19 12 26 20 14 26 21 14 37 22 12 1 23 13 26 24 30 0 25 16 27 26 17 0 27 16 26 28 30 1 29 15 12 30 79 41 31 19 53 32 20 12 33 79 53 34 20 13 35 77 14 36 87 13 37 21 53 38 23 14 39 87 53 40 23 62 41 86 15 42 84 62 43 26 45 44 82 62 45 26 15 46 84 14 47 24 12 48 29 14 49 27 15 50 28 14 51 27 12 52 29 13 53 65 45 54 31 41 55 32 15 56 89 41 57 32 12 58 88 15 59 89 20 60 91 56 61 34 60 62 35 20 63 91 60 64 35 21 65 90 22 66 105 21 67 36 60 68 38 22 69 105 60 70 38 51 71 104 23 72 93 51 73 41 47 74 92 51 75 41 23 76 93 22 77 39 23 78 44 20 79 70 21 80 42 23 81 44 21 82 42 22 83 43 20 84 107 47 85 47 56 86 106 47 87 47 20 88 107 23 89 45 50 90 6 59 91 7 29 92 8 50 93 6 29 94 8 24 95 50 24 96 18 6 97 22 5 98 25 6 99 22 24 100 18 25 101 11 32 102 59 36 103 40 7 104 46 7 105 46 36 106 40 26 107 14 26 108 30 4 109 33 7 110 37 4 111 33 26 112 30 27 113 17 24 114 50 38 115 51 3 116 52 38 117 51 24 118 50 29 119 8 24 120 50 5 121 54 48 122 53 24 123 50 48 124 53 50 125 6 5 126 54 30 127 56 57 128 55 5 129 54 57 130 55 48 131 53 28 132 73 31 133 78 44 134 57 28 135 73 44 136 57 63 137 58 4 138 80 27 139 81 42 140 85 42 141 85 27 142 81 54 143 83 31 144 78 4 145 80 42 146 85 31 147 78 42 148 85 44 149 57 32 150 5 30 151 67 33 152 68 30 153 67 32 154 5 31 155 4 33 156 68 5 157 69 6 158 71 5 159 69 33 160 68 30 161 67 6 162 61 25 163 62 33 164 60 33 165 60 25 166 62 34 167 63 25 168 62 2 169 64 34 170 63 34 171 63 2 172 64 35 173 66 33 174 60 34 175 63 32 176 59 32 177 59 34 178 63 36 179 40 34 180 63 35 181 66 36 182 40 36 183 40 35 184 66 37 185 12 28 186 73 39 187 94 29 188 8 29 189 8 39 190 94 38 191 51 35 192 0 38 193 48 39 194 72 39 195 72 37 196 74 35 197 0 27 198 81 0 199 95 28 200 73 28 201 73 0 202 95 39 203 94 39 204 72 0 205 75 37 206 74 0 207 75 1 208 76 37 209 74 54 210 83 28 211 73 63 212 58 28 213 73 54 214 83 27 215 81 31 216 78 28 217 73 30 218 56 30 219 56 28 220 73 29 221 8 30 222 56 59 223 7 57 224 55 59 225 7 30 226 56 29 227 8 59 228 109 50 229 108 51 230 96 59 231 109 51 232 96 60 233 97 50 234 108 48 235 110 47 236 98 50 237 108 47 238 98 51 239 96 48 240 110 57 241 111 56 242 99 48 243 110 56 244 99 47 245 98 63 246 113 44 247 112 45 248 100 63 249 113 45 250 100 62 251 101 42 252 115 54 253 114 53 254 102 42 255 115 53 256 102 41 257 103 44 258 112 42 259 115 41 260 103 44 261 112 41 262 103 45 263 100 63 264 113 53 265 102 54 266 114 53 267 102 63 268 113 62 269 101 57 270 111 59 271 109 60 272 97 57 273 111 60 274 97 56 275 99</p> + </triangles> + </mesh> + </geometry> + </library_geometries> + <library_visual_scenes> + <visual_scene id="placard_2022" name="placard_2022"> + <node name="Box001" id="Box001" sid="Box001"> + <matrix sid="matrix">1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000</matrix> + <instance_geometry url="#Box001-lib"> + <bind_material> + <technique_common> + <instance_material symbol="Placard" target="#Placard"/> + </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="#placard_2022"/> + </scene> +</COLLADA> diff --git a/NaviGator/simulation/navigator_gazebo/models/placard_2024/model.config b/NaviGator/simulation/navigator_gazebo/models/placard_2024/model.config new file mode 100644 index 000000000..5ca0548f5 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/placard_2024/model.config @@ -0,0 +1,20 @@ +<?xml version="1.0"?> +<model> + <name>placard_2022</name> + <version>1.0</version> + <sdf version="1.6">model.sdf</sdf> + + <author> + <name>Carlos Agüero</name> + <email>caguero@openrobotics.org</email> + </author> + + <author> + <name>Cole Biesemeyer</name> + <email>cole@openrobotics.org</email> + </author> + + <description> + A white placard with a symbol used in the RobotX competition. + </description> +</model> diff --git a/NaviGator/simulation/navigator_gazebo/models/placard_2024/model.sdf b/NaviGator/simulation/navigator_gazebo/models/placard_2024/model.sdf new file mode 100644 index 000000000..95411d3c7 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/placard_2024/model.sdf @@ -0,0 +1,72 @@ +<?xml version="1.0" ?> +<sdf version="1.6"> + <model name="placard"> + <pose>0 0 1 0 0 0</pose> + <!-- The white placard --> + <link name="link"> + <collision name="collision_1"> + <pose>-0.9234 0.0 1.164 0.0 0.0 0.0</pose> + <geometry> + <box> + <size>0.153199 0.500 0.672</size> + </box> + </geometry> + </collision> + <collision name="collision_2"> + <pose>-0.596803 0.0 1.41357 -3.14159 -1.5708 0.0</pose> + <geometry> + <box> + <size>0.17285 0.5 0.5</size> + </box> + </geometry> + </collision> + <collision name="collision_3"> + <pose>0.326598 0.0 1.35111 0.0 0.0 0.0</pose> + <geometry> + <box> + <size>1.3468 0.500 0.29777</size> + </box> + </geometry> + </collision> + <collision name="collision_4"> + <pose>0.070527 0.0 1.07723 0.0 0.0 0.0</pose> + <geometry> + <box> + <size>0.834664 0.500 0.25</size> + </box> + </geometry> + </collision> + <collision name="collision_6"> + <pose>0.326598 0.0 0.890117 0.0 0.0 0.0</pose> + <geometry> + <box> + <size>1.3468 0.500 0.124233</size> + </box> + </geometry> + </collision> + <collision name="collision_5"> + <pose>0.868936 0.0 1.07723 1.5708 0.0 1.5708</pose> + <geometry> + <box> + <size>0.500 0.25 0.262128</size> + </box> + </geometry> + </collision> + <collision name="collision_7"> + <pose>0.0 0.0 -0.336 0.0 0.0 0.0</pose> + <geometry> + <box> + <size>2.000 0.500 2.328</size> + </box> + </geometry> + </collision> + <visual name="visual"> + <geometry> + <mesh> + <uri>file://placard_2024/meshes/placard_2024.dae</uri> + </mesh> + </geometry> + </visual> + </link> + </model> +</sdf> diff --git a/NaviGator/simulation/navigator_gazebo/models/symbol_square/model.config b/NaviGator/simulation/navigator_gazebo/models/symbol_square/model.config new file mode 100644 index 000000000..741ef5967 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/symbol_square/model.config @@ -0,0 +1,20 @@ +<?xml version="1.0"?> +<model> + <name>symbol_rectangle</name> + <version>1.0</version> + <sdf version="1.6">model.sdf</sdf> + + <author> + <name>Max Vu</name> + <email>m.vu@ufl.edu</email> + </author> + <author> + <name>Carlos Agüero</name> + <email>caguero@openrobotics.org</email> + </author> + + <description> + A square symbol used in some elements of the RobotX competition. + edited for robotx 2024 + </description> +</model> diff --git a/NaviGator/simulation/navigator_gazebo/models/symbol_square/model.sdf b/NaviGator/simulation/navigator_gazebo/models/symbol_square/model.sdf new file mode 100644 index 000000000..560189836 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/models/symbol_square/model.sdf @@ -0,0 +1,17 @@ +<?xml version="1.0" ?> +<sdf version="1.6"> + <model name="symbol_square"> + <static>true</static> + <pose>0 0 0 0 1.5708 0</pose> + <link name="link"> + <visual name="visual"> + <geometry> + <box> + <size>0.6096 0.6096 0.001</size> + </box> + </geometry> + <transparency>1</transparency> + </visual> + </link> + </model> +</sdf> diff --git a/NaviGator/simulation/navigator_gazebo/src/placard_plugin_2024.cc b/NaviGator/simulation/navigator_gazebo/src/placard_plugin_2024.cc new file mode 100644 index 000000000..1b4ab604f --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/src/placard_plugin_2024.cc @@ -0,0 +1,260 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include <gazebo/rendering/Scene.hh> + +#include "vrx_gazebo/placard_plugin.hh" + +// Static initialization. +std::map<std::string, std_msgs::ColorRGBA> PlacardPlugin::kColors = { + { "red", CreateColor(1.0, 0.0, 0.0, 1.0) }, + { "green", CreateColor(0.0, 1.0, 0.0, 1.0) }, + { "blue", CreateColor(0.0, 0.0, 1.0, 1.0) }, + // {"yellow", CreateColor(1.0, 1.0, 0.0, 1.0)}, +}; + +std::vector<std::string> PlacardPlugin::kShapes = { + // "circle", "cross", "triangle", "rectangle", + "square" +}; + +///////////////////////////////////////////////// +PlacardPlugin::PlacardPlugin() : gzNode(new gazebo::transport::Node()) +{ +} + +////////////////////////////////////////////////// +std_msgs::ColorRGBA PlacardPlugin::CreateColor(const double _r, const double _g, const double _b, const double _a) +{ + static std_msgs::ColorRGBA color; + color.r = _r; + color.g = _g; + color.b = _b; + color.a = _a; + return color; +} + +////////////////////////////////////////////////// +void PlacardPlugin::InitializeAllPatterns() +{ + for (auto const &colorPair : this->kColors) + for (auto const &shape : this->kShapes) + this->allPatterns.push_back({ colorPair.first, shape }); +} + +////////////////////////////////////////////////// +void PlacardPlugin::Load(gazebo::rendering::VisualPtr _parent, sdf::ElementPtr _sdf) +{ + GZ_ASSERT(_parent != nullptr, "Received NULL model pointer"); + + this->scene = _parent->GetScene(); + GZ_ASSERT(this->scene != nullptr, "NULL scene"); + + this->InitializeAllPatterns(); + + if (!this->ParseSDF(_sdf)) + return; + + // Quit if ros plugin was not loaded + if (!ros::isInitialized()) + { + ROS_ERROR("ROS was not initialized."); + return; + } + + if (this->shuffleEnabled) + { + this->nh = ros::NodeHandle(this->ns); + this->changeSymbolSub = this->nh.subscribe(this->rosShuffleTopic, 1, &PlacardPlugin::ChangeSymbol, this); + } + + this->nextUpdateTime = this->scene->SimTime(); + + this->updateConnection = gazebo::event::Events::ConnectPreRender(std::bind(&PlacardPlugin::Update, this)); + + this->gzNode->Init(); + this->symbolSub = gzNode->Subscribe(this->symbolSubTopic, &PlacardPlugin::ChangeSymbolTo, this, true); +} + +////////////////////////////////////////////////// +void PlacardPlugin::ChangeSymbolTo(gazebo::ConstDockPlacardPtr &_msg) +{ + std::lock_guard<std::mutex> lock(this->mutex); + this->shape = _msg->shape(); + this->color = _msg->color(); +} + +////////////////////////////////////////////////// +bool PlacardPlugin::ParseSDF(sdf::ElementPtr _sdf) +{ + // We initialize it with a random shape and color. + this->ChangeSymbol(std_msgs::Empty::ConstPtr()); + + // Parse the shape. + if (_sdf->HasElement("shape")) + { + std::string aShape = _sdf->GetElement("shape")->Get<std::string>(); + std::transform(aShape.begin(), aShape.end(), aShape.begin(), ::tolower); + // Sanity check: Make sure the shape is allowed. + if (std::find(this->kShapes.begin(), this->kShapes.end(), aShape) != this->kShapes.end()) + { + this->shape = aShape; + } + else + { + ROS_INFO_NAMED("PlacardPlugin", "incorrect [%s] <shape>, using random shape", aShape.c_str()); + } + } + + // Parse the color. We initialize it with a random color. + if (_sdf->HasElement("colors")) + { + } + else if (_sdf->HasElement("color")) + { + std::string aColor = _sdf->GetElement("color")->Get<std::string>(); + std::transform(aColor.begin(), aColor.end(), aColor.begin(), ::tolower); + // Sanity check: Make sure the color is allowed. + if (this->kColors.find(aColor) != this->kColors.end()) + { + this->color = aColor; + } + else + { + ROS_INFO_NAMED("PlacardPlugin", "incorrect [%s] <color>, using random color", aColor.c_str()); + } + } + + // Required: visuals. + if (!_sdf->HasElement("visuals")) + { + ROS_ERROR("<visuals> missing"); + return false; + } + + auto visualsElem = _sdf->GetElement("visuals"); + if (!visualsElem->HasElement("visual")) + { + ROS_ERROR("<visual> missing"); + return false; + } + + auto visualElem = visualsElem->GetElement("visual"); + while (visualElem) + { + std::string visualName = visualElem->Get<std::string>(); + this->visualNames.push_back(visualName); + visualElem = visualElem->GetNextElement(); + } + + // Optional: Is shuffle enabled? + if (_sdf->HasElement("shuffle")) + { + this->shuffleEnabled = _sdf->GetElement("shuffle")->Get<bool>(); + + // Required if shuffle enabled: ROS topic. + if (!_sdf->HasElement("ros_shuffle_topic")) + { + ROS_ERROR("<ros_shuffle_topic> missing"); + } + this->rosShuffleTopic = _sdf->GetElement("ros_shuffle_topic")->Get<std::string>(); + } + + // Required: namespace. + if (!_sdf->HasElement("robot_namespace")) + { + ROS_ERROR("<robot_namespace> missing"); + } + this->ns = _sdf->GetElement("robot_namespace")->Get<std::string>(); + if (!_sdf->HasElement("gz_symbol_topic")) + { + this->symbolSubTopic = "/" + this->ns + "/symbol"; + } + else + { + this->symbolSubTopic = _sdf->GetElement("gz_symbol_topic")->Get<std::string>(); + } + return true; +} + +////////////////////////////////////////////////// +void PlacardPlugin::Update() +{ + // Get the visuals if needed. + if (this->visuals.empty()) + { + for (auto visualName : this->visualNames) + { + auto visualPtr = this->scene->GetVisual(visualName); + if (visualPtr) + this->visuals.push_back(visualPtr); + else + ROS_ERROR("Unable to find [%s] visual", visualName.c_str()); + } + } + + // Only update the plugin at 1Hz. + if (this->scene->SimTime() < this->nextUpdateTime) + return; + + this->nextUpdateTime = this->nextUpdateTime + gazebo::common::Time(1.0); + + std::lock_guard<std::mutex> lock(this->mutex); + + // Update the visuals. + for (auto visual : this->visuals) + { + std_msgs::ColorRGBA color; + color.a = 0.0; + +#if GAZEBO_MAJOR_VERSION >= 8 + auto name = visual->Name(); +#else + auto name = visual->GetName(); +#endif + auto delim = name.rfind("/"); + auto shortName = name.substr(delim + 1); + if (shortName.find(this->shape) != std::string::npos) + { + color = this->kColors[this->color]; + } +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Color gazeboColor(color.r, color.g, color.b, color.a); +#else + gazebo::common::Color gazeboColor(color.r, color.g, color.b, color.a); +#endif + + visual->SetAmbient(gazeboColor); + visual->SetDiffuse(gazeboColor); + } +} + +////////////////////////////////////////////////// +void PlacardPlugin::ChangeSymbol(const std_msgs::Empty::ConstPtr &_msg) +{ + { + std::lock_guard<std::mutex> lock(this->mutex); + this->color = this->allPatterns[this->allPatternsIdx].at(0); + this->shape = this->allPatterns[this->allPatternsIdx].at(1); + this->allPatternsIdx = (this->allPatternsIdx + 1) % this->allPatterns.size(); + } + + ROS_INFO_NAMED("PlacardPlugin", "New symbol is %s %s", this->color.c_str(), this->shape.c_str()); +} + +// Register plugin with gazebo +GZ_REGISTER_VISUAL_PLUGIN(PlacardPlugin) diff --git a/NaviGator/simulation/navigator_gazebo/worlds/scan_dock_fling_2024/default.world b/NaviGator/simulation/navigator_gazebo/worlds/scan_dock_fling_2024/default.world new file mode 100644 index 000000000..d4fbde58e --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/worlds/scan_dock_fling_2024/default.world @@ -0,0 +1,413 @@ +<?xml version="1.0" ?> +<sdf version="1.6"> + <!-- COORDINATE: {'constant': 0, 'environment': 0} --> + <world name="default_scan_dock_fling_2024"> + <!-- Estimated latitude/longitude of sydneyregatta + from satellite imagery --> + <spherical_coordinates> + <surface_model>EARTH_WGS84</surface_model> + <world_frame_orientation>ENU</world_frame_orientation> + <latitude_deg>-33.724223</latitude_deg> + <longitude_deg>150.679736</longitude_deg> + <elevation>0.0</elevation> + <heading_deg>0.0</heading_deg> + </spherical_coordinates> + <!-- A global light source --> + <include> + <uri>model://sun</uri> + </include> + <gui fullscreen="0"> + <camera name="user_camera"> + <pose>-478.101746 148.200836 13.203143 0.0 0.248344 2.936862</pose> + <view_controller>orbit</view_controller> + </camera> + <!--<plugin name="GUITaskWidget" filename="libgui_task_widget.so"/>--> + </gui> + <include> + <!-- Note - the pose tag doesn't seem to work for heightmaps, so you need + to go into the model file to change the altitutde/height!--> + <pose> 0 0 0.2 0 0 0 </pose> + <uri>model://sydney_regatta</uri> + </include> + <!-- The posts for securing the WAM-V --> + <include> + <name>post_0</name> + <pose>-535.916809 154.362869 0.675884 -0.17182 0.030464 -0.005286</pose> + <uri>model://post</uri> + </include> + <include> + <name>post_1</name> + <pose>-527.48999 153.854782 0.425844 -0.1365 0 0</pose> + <uri>model://post</uri> + </include> + <include> + <name>post_2</name> + <pose>-544.832825 156.671951 0.499025 -0.162625 0 0 </pose> + <uri>model://post</uri> + </include> + <!-- Antenna for communication with the WAM-V --> + <include> + <pose>-531.063721 147.668579 1.59471 -0.068142 0 -0.1</pose> + <uri>model://antenna</uri> + </include> + <!-- ground station tents --> + <include> + <name>ground_station_0</name> + <pose>-540.796448 146.493744 1.671421 -0.00834 0.01824 1.301726</pose> + <uri>model://ground_station</uri> + </include> + <include> + <name>ground_station_1</name> + <pose>-537.622681 145.827896 1.681931 -0.00603 0.018667 1.301571</pose> + <uri>model://ground_station</uri> + </include> + <include> + <name>ground_station_2</name> + <pose>-534.550537 144.910400 1.720474 -0.004994 0.020798 1.301492</pose> + <uri>model://ground_station</uri> + </include> + <!-- The projectile for the ball shooter --> + <include> + <name>blue_projectile</name> + <pose>-545 60 0.03 0 0 0</pose> + <uri>model://blue_projectile</uri> + </include> + <model name="ocean_waves"> + <static>true</static> + <plugin filename="libWavefieldModelPlugin.so" name="wavefield_plugin"> + <static>false</static> + <update_rate>30</update_rate> + <size>1000 1000</size> + <cell_count>50 50</cell_count> + <wave> + <model>PMS</model> + <period>5.0</period> + <number>3</number> + <scale>2.5</scale> + <gain>0.0</gain> + <direction>1.0 0.0</direction> + <angle>0.4</angle> + <tau>2.0</tau> + <amplitude>0.0</amplitude> + <!-- No effect for the PMS model --> + <steepness>0.0</steepness> + </wave> + </plugin> + <link name="ocean_waves_link"> + <visual name="ocean_waves_visual"> + <plugin filename="libWavefieldVisualPlugin.so" name="ocean_waves_visual_plugin"> + <enableRtt>true</enableRtt> + <rttNoise>0.1</rttNoise> + <refractOpacity>0.2</refractOpacity> + <reflectOpacity>0.2</reflectOpacity> + <wave> + <model>PMS</model> + <period>5.0</period> + <number>3</number> + <scale>2.5</scale> + <gain>0.0</gain> + <direction>1.0 0.0</direction> + <angle>0.4</angle> + <tau>2.0</tau> + <amplitude>0.0</amplitude> + <!-- No effect for the PMS model --> + <steepness>0.0</steepness> + </wave> + </plugin> + <geometry> + <mesh> + <scale>2.5 2.5 1</scale> + <uri>model://ocean_waves/meshes/mesh.dae</uri> + </mesh> + </geometry> + <material> + <script> + <uri>model://ocean_waves/materials/scripts/waves.material</uri> + <name>WaveSim/GerstnerWaves</name> + </script> + </material> + <laser_retro>-1</laser_retro> + </visual> + <visual name="ocean_waves_below_visual"> + <pose>0 0 -0.05 0 0 0</pose> + <!-- Offset to prevent rendering conflict --> + <geometry> + <mesh> + <scale>2.5 2.5 1</scale> + <uri>model://ocean_waves/meshes/mesh_below.dae</uri> + </mesh> + </geometry> + <material> + <script> + <uri>model://ocean_waves/materials/scripts/waves.material</uri> + <name>WaveSim/GerstnerWaves</name> + </script> + </material> + <laser_retro>-1</laser_retro> + </visual> + </link> + </model> + <!--Gazebo Plugin for simulating WAM-V dynamics--> + <plugin filename="libusv_gazebo_wind_plugin.so" name="wind"> + <!-- models to be effected by the wind --> + <wind_obj> + <name>wamv</name> + <link_name>wamv/base_link</link_name> + <coeff_vector>.5 .5 .33</coeff_vector> + </wind_obj> + <!-- Wind --> + <wind_direction>240</wind_direction> + <!-- in degrees --> + <wind_mean_velocity>0.0</wind_mean_velocity> + <var_wind_gain_constants>0</var_wind_gain_constants> + <var_wind_time_constants>2</var_wind_time_constants> + <random_seed>10</random_seed> + <!-- set to zero/empty to randomize --> + <update_rate>10</update_rate> + <topic_wind_speed>/vrx/debug/wind/speed</topic_wind_speed> + <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> + </plugin> + <scene> + <sky> + <clouds> + <speed>12</speed> + </clouds> + </sky> + <grid>0</grid> + <origin_visual>0</origin_visual> + <ambient> + +1 1 1 1 + </ambient> + </scene> + <include> + <uri>model://dock_2024</uri> + <pose>-480 190 0 0 0 1.0</pose> + </include> + <include> + <uri>model://robotx_light_buoy</uri> + <pose>-532 175 0.25 0 0 0</pose> + </include> + + <!-- Plugin to detect if the projectile entered the hole --> + <plugin filename="libContainPlugin.so" name="contain_placard1_big_plugin"> + <enabled>true</enabled> + <entity>blue_projectile::link</entity> + <namespace>vrx/dock_2024_placard1_big_hole</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard1::link_symbols">0.6 -0.27 1.33 0 0 0</pose> + <geometry> + <box> + <size>0.5 0.5 0.5</size> + </box> + </geometry> + </plugin> + <!-- Plugin to detect if the projectile entered the hole --> + <plugin filename="libContainPlugin.so" name="contain_placard1_small_plugin"> + <enabled>true</enabled> + <entity>blue_projectile::link</entity> + <namespace>vrx/dock_2024_placard1_small_hole</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard1::link_symbols">-0.6 -0.27 1.33 0 0 0</pose> + <geometry> + <box> + <size>0.25 0.5 0.25</size> + </box> + </geometry> + </plugin> + <!-- Plugin to detect if the projectile entered the hole --> + <plugin filename="libContainPlugin.so" name="contain_placard2_big_plugin"> + <enabled>true</enabled> + <entity>blue_projectile::link</entity> + <namespace>vrx/dock_2024_placard2_big_hole</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard2::link_symbols">0.6 -0.27 1.33 0 0 0</pose> + <geometry> + <box> + <size>0.5 0.5 0.5</size> + </box> + </geometry> + </plugin> + <!-- Plugin to detect if the projectile entered the hole --> + <plugin filename="libContainPlugin.so" name="contain_placard2_small_plugin"> + <enabled>true</enabled> + <entity>blue_projectile::link</entity> + <namespace>vrx/dock_2024_placard2_small_hole</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard2::link_symbols">-0.6 -0.27 1.33 0 0 0</pose> + <geometry> + <box> + <size>0.25 0.5 0.25</size> + </box> + </geometry> + </plugin> + <!-- Plugin to detect if the projectile entered the hole --> + <plugin filename="libContainPlugin.so" name="contain_placard3_big_plugin"> + <enabled>true</enabled> + <entity>blue_projectile::link</entity> + <namespace>vrx/dock_2024_placard3_big_hole</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard3::link_symbols">0.6 -0.27 1.33 0 0 0</pose> + <geometry> + <box> + <size>0.5 0.5 0.5</size> + </box> + </geometry> + </plugin> + <!-- Plugin to detect if the projectile entered the hole --> + <plugin filename="libContainPlugin.so" name="contain_placard3_small_plugin"> + <enabled>true</enabled> + <entity>blue_projectile::link</entity> + <namespace>vrx/dock_2024_placard3_small_hole</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard3::link_symbols">-0.6 -0.27 1.33 0 0 0</pose> + <geometry> + <box> + <size>0.25 0.5 0.25</size> + </box> + </geometry> + </plugin> + <!-- The scoring plugin --> + <plugin filename="libscan_dock_scoring_plugin.so" name="scan_dock__deliver_scoring_plugin"> + <!-- Parameters for scoring_plugin --> + <vehicle>wamv</vehicle> + <task_name>scan_dock_deliver</task_name> + <initial_state_duration>3</initial_state_duration> + <ready_state_duration>3</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 --> + <robot_namespace>vrx</robot_namespace> + <color_sequence_service>scan_dock_deliver/color_sequence</color_sequence_service> + <color_1>yellow</color_1> + <color_2>blue</color_2> + <color_3>green</color_3> + <!-- Dock checkers --> + <dock_bonus_points>15</dock_bonus_points> + <correct_dock_bonus_points>5</correct_dock_bonus_points> + <bays> + <bay> + <name>bay1</name> + <internal_activation_topic>/vrx/dock_2024/bay_1/contain</internal_activation_topic> + <external_activation_topic>/vrx/dock_2024/bay_1_external/contain</external_activation_topic> + <symbol_topic>/vrx/dock_2024_placard1/symbol</symbol_topic> + <min_dock_time>10.0</min_dock_time> + <dock_allowed>True</dock_allowed> + <symbol>red_square</symbol> + </bay> + <bay> + <name>bay2</name> + <internal_activation_topic>/vrx/dock_2024/bay_2/contain</internal_activation_topic> + <external_activation_topic>/vrx/dock_2024/bay_2_external/contain</external_activation_topic> + <symbol_topic>/vrx/dock_2024_placard2/symbol</symbol_topic> + <min_dock_time>10.0</min_dock_time> + <dock_allowed>False</dock_allowed> + <symbol>blue_square</symbol> + </bay> + <bay> + <name>bay3</name> + <internal_activation_topic>/vrx/dock_2024/bay_3/contain</internal_activation_topic> + <external_activation_topic>/vrx/dock_2024/bay_3_external/contain</external_activation_topic> + <symbol_topic>/vrx/dock_2024_placard3/symbol</symbol_topic> + <min_dock_time>10.0</min_dock_time> + <dock_allowed>False</dock_allowed> + <symbol>green_square</symbol> + </bay> + </bays> + <!-- Shooting targets --> + <targets> + <!-- Placard #1 --> + <target> + <topic>vrx/dock_2024_placard1_big_hole/contain</topic> + <bonus_points>5</bonus_points> + </target> + <target> + <topic>vrx/dock_2024_placard1_small_hole/contain</topic> + <bonus_points>10</bonus_points> + </target> + <!-- Placard #2 --> + <target> + <topic>vrx/dock_2024_placard2_big_hole/contain</topic> + <bonus_points>5</bonus_points> + </target> + <target> + <topic>vrx/dock_2024_placard2_small_hole/contain</topic> + <bonus_points>10</bonus_points> + </target> + <!-- Placard #3 --> + <target> + <topic>vrx/dock_2024_placard3_big_hole/contain</topic> + <bonus_points>5</bonus_points> + </target> + <target> + <topic>vrx/dock_2024_placard3_small_hole/contain</topic> + <bonus_points>10</bonus_points> + </target> + </targets> + </plugin> + <!-- Triggers a message when the vehicle enters and exits the bay #1 --> + <plugin filename="libContainPlugin.so" name="vehicle_docked_bay1"> + <entity>wamv::wamv/base_link</entity> + <namespace>vrx/dock_2024/bay_1</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard1::placard::link">0 -4.5 -1.5 0 0 0</pose> + <geometry> + <box> + <size>1.5 4 2</size> + </box> + </geometry> + </plugin> + <plugin filename="libContainPlugin.so" name="vehicle_docked_bay1_exterior"> + <entity>wamv::wamv/base_link</entity> + <namespace>vrx/dock_2024/bay_1_external</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard1::placard::link">0 -9.5 -1.5 0 0 0</pose> + <geometry> + <box> + <size>5.5 1.5 2</size> + </box> + </geometry> + </plugin> + <!-- Triggers a message when the vehicle enters and exits the bay #2 --> + <plugin filename="libContainPlugin.so" name="vehicle_docked_bay2"> + <entity>wamv::wamv/base_link</entity> + <namespace>vrx/dock_2024/bay_2</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard2::placard::link">0 -4.5 -1.5 0 0 0</pose> + <geometry> + <box> + <size>1.5 4 2</size> + </box> + </geometry> + </plugin> + <plugin filename="libContainPlugin.so" name="vehicle_docked_bay2_exterior"> + <entity>wamv::wamv/base_link</entity> + <namespace>vrx/dock_2024/bay_2_external</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard2::placard::link">0 -9.5 -1.5 0 0 0</pose> + <geometry> + <box> + <size>5.5 1.5 2</size> + </box> + </geometry> + </plugin> + <!-- Triggers a message when the vehicle enters and exits the bay #3 --> + <plugin filename="libContainPlugin.so" name="vehicle_docked_bay3"> + <entity>wamv::wamv/base_link</entity> + <namespace>vrx/dock_2024/bay_3</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard3::placard::link">0 -4.5 -1.5 0 0 0</pose> + <geometry> + <box> + <size>1.5 4 2</size> + </box> + </geometry> + </plugin> + <plugin filename="libContainPlugin.so" name="vehicle_docked_bay3_exterior"> + <entity>wamv::wamv/base_link</entity> + <namespace>vrx/dock_2024/bay_3_external</namespace> + <pose frame="robotx_dock_2024::dock_2024_placard3::placard::link">0 -9.5 -1.5 0 0 0</pose> + <geometry> + <box> + <size>5.5 1.5 2</size> + </box> + </geometry> + </plugin> + </world> +</sdf> diff --git a/ptk.txt b/ptk.txt new file mode 100644 index 000000000..00aee5efb --- /dev/null +++ b/ptk.txt @@ -0,0 +1,5 @@ +44957 +45260 +45563 +45897 +44931