From a8fd0bbc0fe628d195b5df9d043407f30223d156 Mon Sep 17 00:00:00 2001 From: Max Vu Laptop Date: Fri, 13 Sep 2024 14:15:58 -0400 Subject: [PATCH 1/3] test world for task 6 2024 created, shufflable placards for 2024 --- NaviGator/simulation/VRX/vrx | 2 +- .../worlds/scan_dock_fling_2024/default.world | 413 ++++++++++++++++++ ptk.txt | 5 + 3 files changed, 419 insertions(+), 1 deletion(-) create mode 100644 NaviGator/simulation/navigator_gazebo/worlds/scan_dock_fling_2024/default.world create mode 100644 ptk.txt diff --git a/NaviGator/simulation/VRX/vrx b/NaviGator/simulation/VRX/vrx index edecd95e3..987bc15ef 160000 --- a/NaviGator/simulation/VRX/vrx +++ b/NaviGator/simulation/VRX/vrx @@ -1 +1 @@ -Subproject commit edecd95e38ff68e59d66f647448415222a4529ce +Subproject commit 987bc15ef4b81845697fee4f9012c0dbc9effbc7 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..f11160ab5 --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/worlds/scan_dock_fling_2024/default.world @@ -0,0 +1,413 @@ + + + + + + + EARTH_WGS84 + ENU + -33.724223 + 150.679736 + 0.0 + 0.0 + + + + model://sun + + + + -478.101746 148.200836 13.203143 0.0 0.248344 2.936862 + orbit + + + + + + 0 0 0.2 0 0 0 + model://sydney_regatta + + + + post_0 + -535.916809 154.362869 0.675884 -0.17182 0.030464 -0.005286 + model://post + + + post_1 + -527.48999 153.854782 0.425844 -0.1365 0 0 + model://post + + + post_2 + -544.832825 156.671951 0.499025 -0.162625 0 0 + model://post + + + + -531.063721 147.668579 1.59471 -0.068142 0 -0.1 + model://antenna + + + + ground_station_0 + -540.796448 146.493744 1.671421 -0.00834 0.01824 1.301726 + model://ground_station + + + ground_station_1 + -537.622681 145.827896 1.681931 -0.00603 0.018667 1.301571 + model://ground_station + + + ground_station_2 + -534.550537 144.910400 1.720474 -0.004994 0.020798 1.301492 + model://ground_station + + + + blue_projectile + -545 60 0.03 0 0 0 + model://blue_projectile + + + true + + false + 30 + 1000 1000 + 50 50 + + PMS + 5.0 + 3 + 2.5 + 0.0 + 1.0 0.0 + 0.4 + 2.0 + 0.0 + + 0.0 + + + + + + true + 0.1 + 0.2 + 0.2 + + PMS + 5.0 + 3 + 2.5 + 0.0 + 1.0 0.0 + 0.4 + 2.0 + 0.0 + + 0.0 + + + + + 2.5 2.5 1 + model://ocean_waves/meshes/mesh.dae + + + + + + -1 + + + 0 0 -0.05 0 0 0 + + + + 2.5 2.5 1 + model://ocean_waves/meshes/mesh_below.dae + + + + + + -1 + + + + + + + + wamv + wamv/base_link + .5 .5 .33 + + + 240 + + 0.0 + 0 + 2 + 10 + + 10 + /vrx/debug/wind/speed + /vrx/debug/wind/direction + + + + + 12 + + + 0 + 0 + + +1 1 1 1 + + + + model://dock_2024 + -480 190 0 0 0 1.0 + + + model://robotx_light_buoy + -532 175 0.25 0 0 0 + + + + + true + blue_projectile::link + vrx/dock_2024_placard1_big_hole + 0.6 -0.27 1.33 0 0 0 + + + 0.5 0.5 0.5 + + + + + + true + blue_projectile::link + vrx/dock_2024_placard1_small_hole + -0.6 -0.27 1.33 0 0 0 + + + 0.25 0.5 0.25 + + + + + + true + blue_projectile::link + vrx/dock_2024_placard2_big_hole + 0.6 -0.27 1.33 0 0 0 + + + 0.5 0.5 0.5 + + + + + + true + blue_projectile::link + vrx/dock_2024_placard2_small_hole + -0.6 -0.27 1.33 0 0 0 + + + 0.25 0.5 0.25 + + + + + + true + blue_projectile::link + vrx/dock_2024_placard3_big_hole + 0.6 -0.27 1.33 0 0 0 + + + 0.5 0.5 0.5 + + + + + + true + blue_projectile::link + vrx/dock_2024_placard3_small_hole + -0.6 -0.27 1.33 0 0 0 + + + 0.25 0.5 0.25 + + + + + + + wamv + scan_dock_deliver + 3 + 3 + 300 + + + wamv_external_pivot_joint + + + wamv_external_riser + + + + vrx + scan_dock_deliver/color_sequence + yellow + blue + green + + 15 + 5 + + + bay1 + /vrx/dock_2024/bay_1/contain + /vrx/dock_2024/bay_1_external/contain + /vrx/dock_2024_placard1/symbol + 10.0 + True + red_square + + + bay2 + /vrx/dock_2024/bay_2/contain + /vrx/dock_2024/bay_2_external/contain + /vrx/dock_2024_placard2/symbol + 10.0 + False + blue_square + + + bay3 + /vrx/dock_2024/bay_3/contain + /vrx/dock_2024/bay_3_external/contain + /vrx/dock_2024_placard3/symbol + 10.0 + False + green_square + + + + + + + vrx/dock_2024_placard1_big_hole/contain + 5 + + + vrx/dock_2024_placard1_small_hole/contain + 10 + + + + vrx/dock_2024_placard2_big_hole/contain + 5 + + + vrx/dock_2024_placard2_small_hole/contain + 10 + + + + vrx/dock_2024_placard3_big_hole/contain + 5 + + + vrx/dock_2024_placard3_small_hole/contain + 10 + + + + + + wamv::wamv/base_link + vrx/dock_2024/bay_1 + 0 -4.5 -1.5 0 0 0 + + + 1.5 4 2 + + + + + wamv::wamv/base_link + vrx/dock_2024/bay_1_external + 0 -9.5 -1.5 0 0 0 + + + 5.5 1.5 2 + + + + + + wamv::wamv/base_link + vrx/dock_2024/bay_2 + 0 -4.5 -1.5 0 0 0 + + + 1.5 4 2 + + + + + wamv::wamv/base_link + vrx/dock_2024/bay_2_external + 0 -9.5 -1.5 0 0 0 + + + 5.5 1.5 2 + + + + + + wamv::wamv/base_link + vrx/dock_2024/bay_3 + 0 -4.5 -1.5 0 0 0 + + + 1.5 4 2 + + + + + wamv::wamv/base_link + vrx/dock_2024/bay_3_external + 0 -9.5 -1.5 0 0 0 + + + 5.5 1.5 2 + + + + + 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 From 2dda8347e511e613b4770c7844648770bd8d0bb6 Mon Sep 17 00:00:00 2001 From: Max Vu Laptop Date: Wed, 2 Oct 2024 14:13:09 -0400 Subject: [PATCH 2/3] scan dock fling world name change --- .../navigator_gazebo/worlds/scan_dock_fling_2024/default.world | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index f11160ab5..d4fbde58e 100644 --- a/NaviGator/simulation/navigator_gazebo/worlds/scan_dock_fling_2024/default.world +++ b/NaviGator/simulation/navigator_gazebo/worlds/scan_dock_fling_2024/default.world @@ -1,7 +1,7 @@ - + From 47d74d55783daa04a5e5da8ebd1b95868d222b34 Mon Sep 17 00:00:00 2001 From: Max Vu Laptop Date: Fri, 4 Oct 2024 15:28:29 -0400 Subject: [PATCH 3/3] pulled my files out of VRX submodule, now moved into navigator_gazebo --- .../models/dock_2024/model.config | 20 + .../models/dock_2024/model.sdf | 349 ++++++++++ .../models/dock_2024/model.sdf.erb | 215 +++++++ .../models/dock_2024_base/model.config | 20 + .../models/dock_2024_base/model.sdf | 142 ++++ .../materials/textures/Placard.png | Bin 0 -> 1637 bytes .../placard_2024/meshes/placard_2024.dae | 607 ++++++++++++++++++ .../models/placard_2024/model.config | 20 + .../models/placard_2024/model.sdf | 72 +++ .../models/symbol_square/model.config | 20 + .../models/symbol_square/model.sdf | 17 + .../src/placard_plugin_2024.cc | 260 ++++++++ 12 files changed, 1742 insertions(+) create mode 100644 NaviGator/simulation/navigator_gazebo/models/dock_2024/model.config create mode 100644 NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf create mode 100644 NaviGator/simulation/navigator_gazebo/models/dock_2024/model.sdf.erb create mode 100644 NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.config create mode 100644 NaviGator/simulation/navigator_gazebo/models/dock_2024_base/model.sdf create mode 100755 NaviGator/simulation/navigator_gazebo/models/placard_2024/materials/textures/Placard.png create mode 100644 NaviGator/simulation/navigator_gazebo/models/placard_2024/meshes/placard_2024.dae create mode 100644 NaviGator/simulation/navigator_gazebo/models/placard_2024/model.config create mode 100644 NaviGator/simulation/navigator_gazebo/models/placard_2024/model.sdf create mode 100644 NaviGator/simulation/navigator_gazebo/models/symbol_square/model.config create mode 100644 NaviGator/simulation/navigator_gazebo/models/symbol_square/model.sdf create mode 100644 NaviGator/simulation/navigator_gazebo/src/placard_plugin_2024.cc 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 0000000000000000000000000000000000000000..8664df705134268b89fb23bd2163a0c0e852723b GIT binary patch literal 1637 zcmbVNJ#W)M7+5p7P+SN0$oq*ypYF-wV7huESCm6XifcxB^!TWv?0X)yHpE771rZ(_nDRC=25~CU zin4fqkYcjQgxY4EDDky#&tGV2M1AeL6Tl!{X5DCYCu7%k)>>p|lXz4+e@uwGl6p^I{oj>Ifn>eQl{QsBQ#Jb(v>Obq%N!2%+kFhG{!4@z`0_f|d!O z3(T4hA+kN>%&X<0Npl$u(R%$-X-q!(T33h^0nqRFjlOO0tOHEX^8mDfW$6;3=fgzc zfu7{YCk%C#lPpR_#1pk(#BIJMd`-GKjv-Df+9WUYBr^sEI0dEwiG%Q77G)b%K#yq}*gtdtvA9Si1bg=#un&~3A2 z+DM-EybfIi;TRgoLZDdSsbB{|=NB-iQ8@f7loAy3EXFcRQH(nbq)BIFEeKE}$puab zYt(&Bx@trbMf0xfVY>}=8(X$c!&<0gYa!Iz4BKAKbUkNb?5*5i=VYr;aI&Au{R^Q9 zLl2uet%X#t*)^)Si94@5wKilPtYO-ACibspk^F>k@Beu!h0rqZ@hsp}X4x+4cT`7c zHOi$}!}5!`#%`ChUZjpn2VqiFx37^xRz|gQIhvB0e~WBP+wU?-x{KUnSk6Py#~EAd z$SnS~oWP%)&++!|uK$~;ySV;y@%@kMWm-uWCmkkxCD4jsK+#mjqJZD^8om3e0Zrht zs}<9#pUQ`-BeO(OXR_bEdUOAw?A6UieW}&`@b$Ghrzi*ZK0EmGg?sY}KH2#4 + + + + + 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)