Cave feature release2 (#462)

* Add falling rocks to cave practice circuit 1 (#446) * Add rock fall 1 Signed-off-by: Addisu Z. Taddese <[email protected]> * Add rock fall 2 Signed-off-by: Addisu Z. Taddese <[email protected]> * Add rock fall 3 Signed-off-by: Addisu Z. Taddese <[email protected]> * Change small rock fall to medium, update positions of large rock falls Signed-off-by: Addisu Z. Taddese <[email protected]> * 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 <[email protected]> * Added dynamic rocks to cave practice world 2 Signed-off-by: Nate Koenig <[email protected]> * Remove debug output Signed-off-by: Nate Koenig <[email protected]> * Fix rock fall positions and types Signed-off-by: Nate Koenig <[email protected]> * Comments Signed-off-by: Nate Koenig <[email protected]> * Fix medium rock placements Signed-off-by: Nate Koenig <[email protected]> * Added rock fall, moved rocks out of collision Signed-off-by: Nate Koenig <[email protected]> * Doubled two rock falls, and reduced one to 3 deployments Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Arthur Schang <[email protected]> Co-authored-by: Nate Koenig <[email protected]> * Add falling rocks to cave practice 3 (#448) * Add falling rocks to cave practice 3 Signed-off-by: Nate Koenig <[email protected]> * Comments Signed-off-by: Nate Koenig <[email protected]> * Move rocks out of collision Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]> * Fix levels Signed-off-by: Nate Koenig <[email protected]> * Build the connection validator test (#455) * Build the connection validator test Signed-off-by: Nate Koenig <[email protected]> * Improve debug message Signed-off-by: Nate Koenig <[email protected]> * Removed probably Signed-off-by: Nate Koenig <[email protected]> * Make connection validator an executable Signed-off-by: Nate Koenig <[email protected]> * Update subt_ign/src/ConnectionValidatorPrivate.cc Co-authored-by: Michael Carroll <[email protected]> * 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 <[email protected]> Co-authored-by: Nate Koenig <[email protected]> Co-authored-by: Michael Carroll <[email protected]> Co-authored-by: Addisu Taddese <[email protected]> Co-authored-by: Arthur Schang <[email protected]> Co-authored-by: Nate Koenig <[email protected]> Co-authored-by: Michael Carroll <[email protected]>
osrf · Jun 10, 2020 · db5344b · db5344b
1 parent 69a4364
commit db5344b
Show file tree

Hide file tree

Showing 8 changed files with 694 additions and 11 deletions.
diff --git 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
@@ -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 @@
     <update_rate><%= updateRate %></update_rate>
     <%end%>
     <run>true</run>
-    <levels><%= levels %></levels>
+    <levels><%= $levels %></levels>
     <record>
       <enabled>true</enabled>
       <!-- This path is used by cloudsim, please do not change -->

diff --git 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 @@
     <update_rate><%= updateRate %></update_rate>
     <%end%>
     <run>true</run>
-    <levels><%= levels %></levels>
+    <levels><%= $levels %></levels>
     <record>
       <enabled>true</enabled>
       <!-- This path is used by cloudsim, please do not change -->

diff --git 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 → subt_ign/src/validate_connections.cc b/subt_ign/src/ConnectionValidator_TEST.cc → subt_ign/src/validate_connections.cc
@@ -15,8 +15,8 @@
  *
  */
 
-/// Helper test to evaluate graph population without starting gazebo.
-/// Call with TEST_connection_validator <world_name>
+/// Helper to evaluate graph population without starting gazebo.
+/// Call with `validate_connections` <world_name>
 #include "ConnectionValidatorPrivate.hh"
 
 #include <ignition/common/Console.hh>

diff --git a/subt_ign/worlds/cave_circuit_practice_01.sdf b/subt_ign/worlds/cave_circuit_practice_01.sdf
@@ -447,6 +447,140 @@
       <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Samsung J8 Black</uri>
     </include>
 
+    <model name="performer_detector_1">
+      <static>true</static>
+      <pose>287.47 -105.5 -19.67 0 0 0</pose>
+      <!--Uncomment <link> to preview detector region-->
+      <!--<link name='body'>-->
+      <!--  <visual name="v1">-->
+      <!--    <transparency>0.5</transparency>-->
+      <!--    <geometry>-->
+      <!--      <box>-->
+      <!--        <size>20 30 20</size>-->
+      <!--      </box>-->
+      <!--    </geometry>-->
+      <!--  </visual>-->
+      <!--  <material>-->
+      <!--    <ambient>0.0 1.0 0.0 1</ambient>-->
+      <!--    <diffuse>0.0 1.0 0.0 1</diffuse>-->
+      <!--    <specular>0.5 0.5 0.5 1</specular>-->
+      <!--  </material>-->
+      <!--  <cast_shadows>false</cast_shadows>-->
+      <!--</link>-->
+      <plugin filename="libignition-gazebo-performer-detector-system.so"
+        name="ignition::gazebo::systems::PerformerDetector">
+        <topic>/subt_performer_detector</topic>
+        <geometry>
+          <box>
+            <size>20 30 20</size>
+          </box>
+        </geometry>
+      </plugin>
+
+      <!--Triggered by performer_detector_1-->
+      <plugin name="ignition::gazebo::systems::TriggeredPublisher" filename="libignition-gazebo-triggered-publisher-system.so">
+        <input type="ignition.msgs.Pose" topic="/subt_performer_detector">
+          <match field='header.data'>{ key: "frame_id" value: "performer_detector_1" }</match>
+          <match field='header.data'>{ key: "state" value: "1" }</match>
+        </input>
+        <output type="ignition.msgs.Empty" topic="/model/medium_rock_fall_1/breadcrumbs/Rock/deploy"/>
+      </plugin>
+    </model>
+    <include>
+      <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Medium Rock Fall</uri>
+      <name>medium_rock_fall_1</name>
+      <pose>284.95 -104.381 -15.65 0 -1.0147 0</pose>
+    </include>
+    <model name="performer_detector_2">
+      <static>true</static>
+      <pose>101 -296 4 0 0 0</pose>
+      <!--Uncomment <link> to preview detector region-->
+      <!--<link name='body'>-->
+      <!--  <visual name="v1">-->
+      <!--    <transparency>0.5</transparency>-->
+      <!--    <geometry>-->
+      <!--      <box>-->
+      <!--        <size>16 16 15</size>-->
+      <!--      </box>-->
+      <!--    </geometry>-->
+      <!--  </visual>-->
+      <!--  <material>-->
+      <!--    <ambient>0.0 1.0 0.0 1</ambient>-->
+      <!--    <diffuse>0.0 1.0 0.0 1</diffuse>-->
+      <!--    <specular>0.5 0.5 0.5 1</specular>-->
+      <!--  </material>-->
+      <!--  <cast_shadows>false</cast_shadows>-->
+      <!--</link>-->
+      <plugin filename="libignition-gazebo-performer-detector-system.so"
+        name="ignition::gazebo::systems::PerformerDetector">
+        <topic>/subt_performer_detector</topic>
+        <geometry>
+          <box>
+            <size>16 16 15</size>
+          </box>
+        </geometry>
+      </plugin>
+
+      <!--Triggered by performer_detector_2-->
+      <plugin name="ignition::gazebo::systems::TriggeredPublisher" filename="libignition-gazebo-triggered-publisher-system.so">
+        <input type="ignition.msgs.Pose" topic="/subt_performer_detector">
+          <match field='header.data'>{ key: "frame_id" value: "performer_detector_2" }</match>
+          <match field='header.data'>{ key: "state" value: "0" }</match>
+        </input>
+        <output type="ignition.msgs.Empty" topic="/model/large_rock_fall_1/breadcrumbs/Rock/deploy"/>
+      </plugin>
+    </model>
+    <include>
+      <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Large Rock Fall</uri>
+      <name>large_rock_fall_1</name>
+      <pose>102.692000 -295.402000 3.005300 0 0.402990 -0.915400</pose>
+    </include>
+
+    <model name="performer_detector_3">
+      <static>true</static>
+      <pose>178 -270 -45 0 0 0</pose>
+      <!--Uncomment <link> to preview detector region-->
+      <!--<link name='body'>-->
+      <!--  <visual name="v1">-->
+      <!--    <transparency>0.5</transparency>-->
+      <!--    <geometry>-->
+      <!--      <box>-->
+      <!--        <size>16 16 15</size>-->
+      <!--      </box>-->
+      <!--    </geometry>-->
+      <!--  </visual>-->
+      <!--  <material>-->
+      <!--    <ambient>0.0 1.0 0.0 1</ambient>-->
+      <!--    <diffuse>0.0 1.0 0.0 1</diffuse>-->
+      <!--    <specular>0.5 0.5 0.5 1</specular>-->
+      <!--  </material>-->
+      <!--  <cast_shadows>false</cast_shadows>-->
+      <!--</link>-->
+      <plugin filename="libignition-gazebo-performer-detector-system.so"
+        name="ignition::gazebo::systems::PerformerDetector">
+        <topic>/subt_performer_detector</topic>
+        <geometry>
+          <box>
+            <size>16 16 15</size>
+          </box>
+        </geometry>
+      </plugin>
+
+      <!--Triggered by performer_detector_2-->
+      <plugin name="ignition::gazebo::systems::TriggeredPublisher" filename="libignition-gazebo-triggered-publisher-system.so">
+        <input type="ignition.msgs.Pose" topic="/subt_performer_detector">
+          <match field='header.data'>{ key: "frame_id" value: "performer_detector_3" }</match>
+          <match field='header.data'>{ key: "state" value: "0" }</match>
+        </input>
+        <output type="ignition.msgs.Empty" topic="/model/large_rock_fall_2/breadcrumbs/Rock/deploy"/>
+      </plugin>
+    </model>
+    <include>
+      <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Large Rock Fall</uri>
+      <name>large_rock_fall_2</name>
+      <pose>178.14 -270 -47.83 0.210254 0 1.57</pose>
+    </include>
+
     <!-- Auto generated: level_generator 161 182 140 40 -->
     <plugin name="ignition::gazebo" filename="dummy">
       <level name="level0">