Update connection helper (#1021)

* Update connection helper

Signed-off-by: Nate Koenig <[email protected]>

* Fix issue #984

Signed-off-by: Nate Koenig <[email protected]>

Co-authored-by: Nate Koenig <[email protected]>

Author: nkoenig

docker/subt_shell/Dockerfile

@@ -82,6 +82,7 @@ RUN sudo /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu
     ros-melodic-twist-mux \
     ros-melodic-rviz-imu-plugin \
     ros-melodic-rotors-control \
+    ros-melodic-control-toolbox \
  && sudo rosdep init \
  && sudo apt-get clean -qq
 

subt_ign/src/ConnectionHelper.cc

@@ -47,6 +47,7 @@ std::map<std::string, std::vector<ignition::math::Vector3d>>
     {"Tunnel Tile 6 Lights", {{0, -10, 0}, {0, 10, 5}}},
     {"Tunnel Tile 7", {{0, -10, 0}, {0, 10, 5}}},
     {"Tunnel Tile 7 Lights", {{0, -10, 0}, {0, 10, 5}}},
+    {"Tunnel Tile Blocker", {{0, 0, 0}}},
     {"Rough Tunnel Tile 90-degree Turn", {{0, -10, 0}, {10, 0, 0}}},
     {"Rough Tunnel Tile 90-degree Turn Lights", {{0, -10, 0}, {10, 0, 0}}},
     {"Rough Tunnel Tile Ramp", {{0, -10, 0}, {0, 10, 5}}},
@@ -94,6 +95,7 @@ std::map<std::string, std::vector<ignition::math::Vector3d>>
         {23.979, 3.906, 0.919}, {-23.979, 3.906, 0.919},
         {23.982, 11.743, 0.919}}},
     {"Urban Stairwell Platform", {{0, 20, 11.69}, {0, -20, 1.69}}},
+    {"Urban Stairwell Straight", {{20, 0, 0}, {-20, 0, 0}}},
     {"Urban Stairwell Platform Lights", {{0, 20, 11.69}, {0, -20, 1.69}}},
     {"Urban Stairwell Platform Centered", {{0, 20, 10}, {0, -20, 0}}},
     {"Urban Stairwell Platform Centered Lights", {{0, 20, 10}, {0, -20, 0}}},
@@ -110,6 +112,9 @@ std::map<std::string, std::vector<ignition::math::Vector3d>>
     {"Urban 2 Story Large Side 2", {{0, -20, 0}, {0, 20, 0}}},
     {"Urban Large Room Split", {{0, -20, 0}, {-20, 0, 0}, {0, 20, 0}}},
     {"Urban Large Room Split Lights", {{0, -20, 0}, {-20, 0, 0}, {0, 20, 0}}},
+    {"Urban Station", {{8.5, -26.5, -0.8}, {8.5, -26.5, -7}, {8.5, 26.5, -7}, {8.5, 26.5, -0.8},
+        {-8.5, -26.5, -0.8}, {-8.5, -26.5, -7}, {-8.5, 26.5, -7}, {-8.5, 26.5, -0.8}}},
+    {"Cave Tile Blocker", {{0, 0, 0}}},
     {"Cave Starting Area Type B", {{12.5, 0, 0}}},
     {"Cave Straight 01", {{0, 12.5, 0}, {0, -12.5, 0}}},
     {"Cave Straight 01 Type B", {{0, 12.5, 0}, {0, -12.5, 0}}},
@@ -154,6 +159,8 @@ std::map<std::string, std::vector<ignition::math::Vector3d>>
     {"Cave Corner 03 Type A Lights", {{-25.0, 0.0, 0.0}, {0.0, -25.0, 0.0}}},
     {"Cave Corner 04 Type A", {{0.0, 50.0, 25.0}, {-50.0, -50.0, 0.0}}},
     {"Cave Cap Type A", {{0.0, 0.0, 0.0}}},
+    {"Cave Cap Type B", {{0.0, 0.0, 0.0}}},
+    {"Cave Cap", {{0.0, 0.0, 0.0}}},
     {"Cave Elevation 01 Type A", {{0.0, -50.0, 0.0}, {0.0, 50.0, 75.0}}},
     {"Cave Elevation 02 Type A", {{-50.0, 0.0, 75.0}, {50.0, 0.0, 0.0}}},
     {"Cave Elevation Corner Type A", {{25.0, 75.0, 25.0}, {0.0, -100.0, 0.0}}},
@@ -212,16 +219,16 @@ std::map<std::string, std::vector<ignition::math::Vector3d>>
     {"Cave Tunnel Transition Lights", {{0, 25, 0}, {0, -25, 0}, {-25, 0, 0}}},
     {"Urban Cave Transition", {{0, -12, 0}, {0, 12, 0}, {-25, 0, 0}}},
     {"Urban Cave Transition Lights", {{0, -12, 0}, {0, 12, 0}, {-25, 0, 0}}},
-    {"Urban Cave Transition 2", {{0, -12, 0}, {0, 12, 0}, {-25, 0, 0}}},
+    {"Urban Cave Transition 2", {{0, 0, 0}, {0, -15, -5}}},
     {"Urban Cave Transition Straight", {{0, -5, 0}, {0, 5, 0}}},
     {"Urban Cave Transition Straight Large", {{0, -5, 0}, {0, 5, 0}}},
     {"Urban Cave Transition Straight Lights", {{0, -5, 0}, {0, 5, 0}}},
     {"Urban Tunnel Transition", {{0, 0, 0}, {0, -15, -5}}},
-    {"Urban Tunnel Transition 2", {{0, 0, 0}, {0, -15, -5}}},
+    {"Urban Tunnel Transition 2", {{5, 0, 0}, {-5, 0, 0}, {0, -5, 0}}},
     {"Urban Tunnel Transition Straight", {{0, -5, 0}, {0, 5, 0}}},
-    {"4-Way Finals Transition", {{8, 0, 0}, {-8, 0, 0}, {0, 8, 0}, {0, -8, 0}}},
-    {"4-Way Finals Transition 2", {{8, 0, 0}, {-8, 0, 0}, {0, 8, 0}, {0, -8, 0}}},
-    {"4-Way Finals Transition 2 Lights", {{8, 0, 0}, {-8, 0, 0}, {0, 8, 0}, {0, -8, 0}}},
+    {"4-Way Finals Transition", {{6, 0, 0}, {-7.5, 0, 0}, {0, 7, 0}, {0, -8.5, 0}}},
+    {"4-Way Finals Transition 2", {{7.5, 0, 0}, {-8.5, 0, 0}, {0, 8, 0}, {0, -8.5, 0}}},
+    {"4-Way Finals Transition 2 Lights", {{7.5, 0, 0}, {-8.5, 0, 0}, {0, 8, 0}, {0, -8.5, 0}}},
     {"Finals Staging Area", {{0, 5, 0}}},
     {"NIOSH Staging Connector Finals", {{0, 0, 0}, {-1, 6.5, 0}}},
   };
@@ -253,6 +260,7 @@ std::map<std::string, subt::ConnectionHelper::ConnectionType>
     {"Tunnel Tile 6 Lights", subt::ConnectionHelper::STRAIGHT},
     {"Tunnel Tile 7", subt::ConnectionHelper::TURN},
     {"Tunnel Tile 7 Lights", subt::ConnectionHelper::TURN},
+    {"Tunnel Tile Blocker", subt::ConnectionHelper::TURN},
     {"Rough Tunnel Tile 90-degree Turn", subt::ConnectionHelper::TURN},
     {"Rough Tunnel Tile 90-degree Turn Lights", subt::ConnectionHelper::TURN},
     {"Rough Tunnel Tile Ramp", subt::ConnectionHelper::STRAIGHT},
@@ -291,6 +299,7 @@ std::map<std::string, subt::ConnectionHelper::ConnectionType>
     {"Urban Platform", subt::ConnectionHelper::TURN},
     {"Urban Platform Open", subt::ConnectionHelper::TURN},
     {"Urban Stairwell Platform", subt::ConnectionHelper::STRAIGHT},
+    {"Urban Stairwell Straight", subt::ConnectionHelper::STRAIGHT},
     {"Urban Stairwell Platform Lights", subt::ConnectionHelper::STRAIGHT},
     {"Urban Stairwell Platform Centered", subt::ConnectionHelper::STRAIGHT},
     {"Urban Stairwell Platform Centered Lights",
@@ -307,7 +316,9 @@ std::map<std::string, subt::ConnectionHelper::ConnectionType>
     {"Urban 2 Story Large Side 2", subt::ConnectionHelper::TURN},
     {"Urban Large Room Split", subt::ConnectionHelper::TURN},
     {"Urban Large Room Split Lights", subt::ConnectionHelper::TURN},
+    {"Urban Station", subt::ConnectionHelper::STRAIGHT},
     {"Cave Starting Area Type B", subt::ConnectionHelper::STRAIGHT},
+    {"Cave Tile Blocker", subt::ConnectionHelper::STRAIGHT},
     {"Cave Straight 01 Type B", subt::ConnectionHelper::STRAIGHT},
     {"Cave Straight 01 Lights Type B", subt::ConnectionHelper::STRAIGHT},
     {"Cave Straight 02 Type B", subt::ConnectionHelper::STRAIGHT},
@@ -350,6 +361,8 @@ std::map<std::string, subt::ConnectionHelper::ConnectionType>
     {"Cave Corner 03 Type A Lights", subt::ConnectionHelper::TURN},
     {"Cave Corner 04 Type A", subt::ConnectionHelper::TURN},
     {"Cave Cap Type A", subt::ConnectionHelper::TURN},
+    {"Cave Cap Type B", subt::ConnectionHelper::TURN},
+    {"Cave Cap", subt::ConnectionHelper::TURN},
     {"Cave Elevation 01 Type A", subt::ConnectionHelper::STRAIGHT},
     {"Cave Elevation 02 Type A", subt::ConnectionHelper::STRAIGHT},
     {"Cave Elevation Corner Type A", subt::ConnectionHelper::TURN},
@@ -447,6 +460,7 @@ std::map<std::string, subt::ConnectionHelper::ConnectionType>
     {"Tunnel Tile 6 Lights", subt::ConnectionHelper::TUNNEL},
     {"Tunnel Tile 7", subt::ConnectionHelper::TUNNEL},
     {"Tunnel Tile 7 Lights", subt::ConnectionHelper::TUNNEL},
+    {"Tunnel Tile Blocker", subt::ConnectionHelper::TUNNEL},
     {"Rough Tunnel Tile 90-degree Turn", subt::ConnectionHelper::TUNNEL},
     {"Rough Tunnel Tile 90-degree Turn Lights", subt::ConnectionHelper::TUNNEL},
     {"Rough Tunnel Tile Ramp", subt::ConnectionHelper::TUNNEL},
@@ -482,6 +496,7 @@ std::map<std::string, subt::ConnectionHelper::ConnectionType>
     {"Urban Platform", subt::ConnectionHelper::URBAN},
     {"Urban Platform Open", subt::ConnectionHelper::URBAN},
     {"Urban Stairwell Platform", subt::ConnectionHelper::URBAN},
+    {"Urban Stairwell Straight", subt::ConnectionHelper::URBAN},
     {"Urban Stairwell Platform Lights", subt::ConnectionHelper::URBAN},
     {"Urban Stairwell Platform Centered", subt::ConnectionHelper::URBAN},
     {"Urban Stairwell Platform Centered Lights",
@@ -500,7 +515,9 @@ std::map<std::string, subt::ConnectionHelper::ConnectionType>
     {"Urban Large Room Split Lights", subt::ConnectionHelper::URBAN},
     {"Urban Straight Door Left Extension", subt::ConnectionHelper::URBAN},
     {"Urban Bend Mixed", subt::ConnectionHelper::URBAN},
+    {"Urban Station", subt::ConnectionHelper::URBAN},
     {"Cave Starting Area Type B", subt::ConnectionHelper::CAVE},
+    {"Cave Tile Blocker", subt::ConnectionHelper::CAVE},
     {"Cave Straight 01", subt::ConnectionHelper::CAVE},
     {"Cave Straight 01 Type B", subt::ConnectionHelper::CAVE},
     {"Cave Straight 01 Lights Type B", subt::ConnectionHelper::CAVE},
@@ -544,6 +561,8 @@ std::map<std::string, subt::ConnectionHelper::ConnectionType>
     {"Cave Corner 03 Type A Lights", subt::ConnectionHelper::CAVE},
     {"Cave Corner 04 Type A", subt::ConnectionHelper::CAVE},
     {"Cave Cap Type A", subt::ConnectionHelper::CAVE},
+    {"Cave Cap Type B", subt::ConnectionHelper::CAVE},
+    {"Cave Cap", subt::ConnectionHelper::CAVE},
     {"Cave Elevation 01 Type A", subt::ConnectionHelper::CAVE},
     {"Cave Elevation 02 Type A", subt::ConnectionHelper::CAVE},
     {"Cave Elevation Corner Type A", subt::ConnectionHelper::CAVE},