From db5344b1c7a3ae80add62ef8f875a82be9005a6d Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 10 Jun 2020 13:05:42 -0700 Subject: [PATCH] Cave feature release2 (#462) * Add falling rocks to cave practice circuit 1 (#446) * Add rock fall 1 Signed-off-by: Addisu Z. Taddese * Add rock fall 2 Signed-off-by: Addisu Z. Taddese * Add rock fall 3 Signed-off-by: Addisu Z. Taddese * Change small rock fall to medium, update positions of large rock falls Signed-off-by: Addisu Z. Taddese * Add falling rocks to cave practice circuit 2 (#447) * Adjustment of fading exponent from 2.5 to 1.5 for the cave circuit. * Add fading exponent to cloudsim launch files Signed-off-by: Nate Koenig * Added dynamic rocks to cave practice world 2 Signed-off-by: Nate Koenig * Remove debug output Signed-off-by: Nate Koenig * Fix rock fall positions and types Signed-off-by: Nate Koenig * Comments Signed-off-by: Nate Koenig * Fix medium rock placements Signed-off-by: Nate Koenig * Added rock fall, moved rocks out of collision Signed-off-by: Nate Koenig * Doubled two rock falls, and reduced one to 3 deployments Signed-off-by: Nate Koenig Co-authored-by: Arthur Schang Co-authored-by: Nate Koenig * Add falling rocks to cave practice 3 (#448) * Add falling rocks to cave practice 3 Signed-off-by: Nate Koenig * Comments Signed-off-by: Nate Koenig * Move rocks out of collision Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig * Fix levels Signed-off-by: Nate Koenig * Build the connection validator test (#455) * Build the connection validator test Signed-off-by: Nate Koenig * Improve debug message Signed-off-by: Nate Koenig * Removed probably Signed-off-by: Nate Koenig * Make connection validator an executable Signed-off-by: Nate Koenig * Update subt_ign/src/ConnectionValidatorPrivate.cc Co-authored-by: Michael Carroll * Use a wider cap tolerance as a fallback This is because some of the cave caps have a non-zero connection point, but we do not propagate enough information to be able to compute the true connection point at this time. Signed-off-by: Michael Carroll Co-authored-by: Nate Koenig Co-authored-by: Michael Carroll Co-authored-by: Addisu Taddese Co-authored-by: Arthur Schang Co-authored-by: Nate Koenig Co-authored-by: Michael Carroll --- subt_ign/CMakeLists.txt | 24 ++ subt_ign/launch/cave_circuit.ign | 10 +- subt_ign/launch/cloudsim_sim.ign | 10 +- subt_ign/src/ConnectionValidatorPrivate.cc | 15 +- ...idator_TEST.cc => validate_connections.cc} | 4 +- subt_ign/worlds/cave_circuit_practice_01.sdf | 134 +++++++++ subt_ign/worlds/cave_circuit_practice_02.sdf | 260 ++++++++++++++++++ subt_ign/worlds/cave_circuit_practice_03.sdf | 248 +++++++++++++++++ 8 files changed, 694 insertions(+), 11 deletions(-) rename subt_ign/src/{ConnectionValidator_TEST.cc => validate_connections.cc} (87%) diff --git a/subt_ign/CMakeLists.txt b/subt_ign/CMakeLists.txt index 8bb7541e..bc14702b 100644 --- a/subt_ign/CMakeLists.txt +++ b/subt_ign/CMakeLists.txt @@ -153,6 +153,30 @@ install(TARGETS ${connection_validator_name} LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +add_executable(validate_connections + src/validate_connections.cc + src/ConnectionValidator.cc + src/ConnectionValidatorPrivate.cc + src/ConnectionHelper.cc + src/SimpleDOTParser.cc) +target_link_libraries(validate_connections + PRIVATE + ignition-gazebo${IGN_GAZEBO_VER}::core + ignition-common3::ignition-common3 + ignition-launch1::ignition-launch1 + ignition-math6::ignition-math6 + ignition-msgs4::ignition-msgs4 + ignition-plugin1::loader + ignition-transport7::ignition-transport7 + ${catkin_LIBRARIES} + ${protobuf_lib_name} +) +install(TARGETS validate_connections + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + + # Create the libCommsBrokerPlugin.so library. set(comms_broker_plugin_name CommsBrokerPlugin) add_library(${comms_broker_plugin_name} diff --git a/subt_ign/launch/cave_circuit.ign b/subt_ign/launch/cave_circuit.ign index a7ab6f39..ae6f19f3 100644 --- a/subt_ign/launch/cave_circuit.ign +++ b/subt_ign/launch/cave_circuit.ign @@ -196,9 +196,13 @@ <% # disable levels for simple cave worlds - levels = "true" + $levels = "true" if $worldName.include?('simple_cave_') - levels = "false" + $levels = "false" + end + + if defined?(levels) && levels != nil && !levels.empty? + $levels = levels.downcase == "true" end %> @@ -232,7 +236,7 @@ <%= updateRate %> <%end%> true - <%= levels %> + <%= $levels %> true diff --git a/subt_ign/launch/cloudsim_sim.ign b/subt_ign/launch/cloudsim_sim.ign index b40b81df..724ee75b 100644 --- a/subt_ign/launch/cloudsim_sim.ign +++ b/subt_ign/launch/cloudsim_sim.ign @@ -208,9 +208,13 @@ <% # disable levels for simple cave worlds - levels = "true" + $levels = "true" if $worldName.include?('simple_cave_') - levels = "false" + $levels = "false" + end + + if defined?(levels) && levels != nil && !levels.empty? + $levels = levels.downcase == "true" end %> @@ -238,7 +242,7 @@ <%= updateRate %> <%end%> true - <%= levels %> + <%= $levels %> true diff --git a/subt_ign/src/ConnectionValidatorPrivate.cc b/subt_ign/src/ConnectionValidatorPrivate.cc index 5cf2bf87..daa7e4ce 100644 --- a/subt_ign/src/ConnectionValidatorPrivate.cc +++ b/subt_ign/src/ConnectionValidatorPrivate.cc @@ -327,18 +327,27 @@ void ConnectionValidatorPrivate::PopulateConnections() { for (auto cap: caps) { - if (point.Equal(cap, 1)) + if (point.Equal(cap, 1.0)) { igndbg << "Found cap!" << std::endl; found_caps += 1; } + else if (point.Equal(cap, 12.6)) + { + igndbg << "Found cap (with expanded tolerance)" << std::endl; + found_caps += 1; + } } } if (actual + found_caps != data) { - igndbg << name << " " << actualConnections[name] << "/" << data << - " (" << this->vertData[name].tileType << ")" << std::endl; + igndbg << "Actual connections[" << actualConnections[name] + << "] do not match expected connections[" << data + << "] for tile name[" << name << "] with type[" + << this->vertData[name].tileType << "]. " + << "Missing an end cap or connecting tile." + << std::endl; } } } diff --git a/subt_ign/src/ConnectionValidator_TEST.cc b/subt_ign/src/validate_connections.cc similarity index 87% rename from subt_ign/src/ConnectionValidator_TEST.cc rename to subt_ign/src/validate_connections.cc index d52003a6..582cc817 100644 --- a/subt_ign/src/ConnectionValidator_TEST.cc +++ b/subt_ign/src/validate_connections.cc @@ -15,8 +15,8 @@ * */ -/// Helper test to evaluate graph population without starting gazebo. -/// Call with TEST_connection_validator +/// Helper to evaluate graph population without starting gazebo. +/// Call with `validate_connections` #include "ConnectionValidatorPrivate.hh" #include diff --git a/subt_ign/worlds/cave_circuit_practice_01.sdf b/subt_ign/worlds/cave_circuit_practice_01.sdf index db75330c..de01f15c 100644 --- a/subt_ign/worlds/cave_circuit_practice_01.sdf +++ b/subt_ign/worlds/cave_circuit_practice_01.sdf @@ -447,6 +447,140 @@ https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Samsung J8 Black + + true + 287.47 -105.5 -19.67 0 0 0 + + + + + + + + + + + + + + + + + + + /subt_performer_detector + + + 20 30 20 + + + + + + + + { key: "frame_id" value: "performer_detector_1" } + { key: "state" value: "1" } + + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Medium Rock Fall + medium_rock_fall_1 + 284.95 -104.381 -15.65 0 -1.0147 0 + + + true + 101 -296 4 0 0 0 + + + + + + + + + + + + + + + + + + + /subt_performer_detector + + + 16 16 15 + + + + + + + + { key: "frame_id" value: "performer_detector_2" } + { key: "state" value: "0" } + + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Large Rock Fall + large_rock_fall_1 + 102.692000 -295.402000 3.005300 0 0.402990 -0.915400 + + + + true + 178 -270 -45 0 0 0 + + + + + + + + + + + + + + + + + + + /subt_performer_detector + + + 16 16 15 + + + + + + + + { key: "frame_id" value: "performer_detector_3" } + { key: "state" value: "0" } + + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Large Rock Fall + large_rock_fall_2 + 178.14 -270 -47.83 0.210254 0 1.57 + + diff --git a/subt_ign/worlds/cave_circuit_practice_02.sdf b/subt_ign/worlds/cave_circuit_practice_02.sdf index ddc5dc73..b64517b3 100644 --- a/subt_ign/worlds/cave_circuit_practice_02.sdf +++ b/subt_ign/worlds/cave_circuit_practice_02.sdf @@ -14,6 +14,266 @@ false + + + + true + 314.608 217.222 -21.79 0.0 0.0 0.0 + + + + /subt_performer_detector + + + 20 30 10 + + + + + + + + + { + key: "frame_id" + value: "performer_detector_1" + } + + + { + key: "state" + value: "1" + } + + + + + + + + + dynamic_rocks_1 + 318.415 217.602 -14.9601 0.0 0.0 0.0 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Medium Rock Fall 3 + + + + + + + true + 785.201000 -25.106900 -47.35 0.0 0.0 + + + + /subt_performer_detector + + + 16 12 15 + + + + + + + + + { + key: "frame_id" + value: "performer_detector_2" + } + + + { + key: "state" + value: "1" + } + + + + + + + + + dynamic_rocks_2 + 785.201000 -25.117000 -45.6989 0 0 0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Medium Rock Fall + + + + + + + + true + 511.806000 363.010000 -43.3108 0 0 0 + + + + /subt_performer_detector + + + 16 16 15 + + + + + + + + { + key: "frame_id" + value: "performer_detector_3" + } + + + { + key: "state" + value: "0" + } + + + + + + + + + + dynamic_rocks_3 + 511.806000 363.010000 -47.723600 0 0.310095 0 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Large Rock Fall + + + dynamic_rocks_3b + 514.233000 366.052000 -47.517700 0 0.310095 0 + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Large Rock Fall + + + + + + + + true + 741.101000 -557.923000 -20.670900 0 0 0 + + + + /subt_performer_detector + + + 16 16 15 + + + --> + + + + + { + key: "frame_id" + value: "performer_detector_4" + } + + + { + key: "state" + value: "0" + } + + + + + + + + + + dynamic_rocks_4 + 741.681 -556.875 -22.4492 0 -0.337194 1.491650 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Large Rock Fall + + + + dynamic_rocks_4b + 744.311000 -554.854000 -22.889000 0 0 1.491650 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Large Rock Fall + + + + + true diff --git a/subt_ign/worlds/cave_circuit_practice_03.sdf b/subt_ign/worlds/cave_circuit_practice_03.sdf index dacfaf9f..42fa61bd 100644 --- a/subt_ign/worlds/cave_circuit_practice_03.sdf +++ b/subt_ign/worlds/cave_circuit_practice_03.sdf @@ -14,6 +14,254 @@ false + + + + true + 348.293000 224.719000 5.290740 0.0 0.0 0.0 + + + + /subt_performer_detector + + + 16 25 15 + + + + + + + + + { + key: "frame_id" + value: "performer_detector_1" + } + + + { + key: "state" + value: "1" + } + + + + + + + + + dynamic_rocks_1 + 348.293000 222.773000 10.127600 0.0 0.0 0.0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Medium Rock Fall + + + + + + + + true + 847.566000 125.260000 24.790600 0.0 0.0 0.0 + + + + /subt_performer_detector + + + 16 25 15 + + + + + + + + + { + key: "frame_id" + value: "performer_detector_2" + } + + + { + key: "state" + value: "1" + } + + + + + + + + + dynamic_rocks_2 + 847.566000 126.873000 29.7217 0.0 0.0 0.0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Medium Rock Fall + + + + + + + + true + 153.975000 4.009890 -3.875 0.0 0.0 0.0 + + + + /subt_performer_detector + + + 25 16 15 + + + + + + + + + { + key: "frame_id" + value: "performer_detector_3" + } + + + { + key: "state" + value: "0" + } + + + + + + + + + dynamic_rocks_3 + 153.94 4.009890 -6.251890 0.0 0.0 0.0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Large Rock Fall + + + + + + + + true + 626.056000 300.805000 12.145300 0.0 0.0 0.0 + + + + /subt_performer_detector + + + 25 16 15 + + + + + + + + + { + key: "frame_id" + value: "performer_detector_4" + } + + + { + key: "state" + value: "0" + } + + + + + + + + + dynamic_rocks_4 + 626.056000 300.805000 11.451700 0.0 0.0 0.0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Large Rock Fall + + + + true