Skip to content

Commit

Permalink
Cave Type A tiles (#422)
Browse files Browse the repository at this point in the history 
* Cave Type A tiles

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Added DronePlatformX1

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Added Type A light tiles

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Reorder

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Added transition tile

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Removed old transition tile

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* sync changes

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* fix cave elevation straight lights connection point

Signed-off-by: Ian Chen <ichen@osrfoundation.org>

* Added transition tile with lights

Signed-off-by: Nate Koenig <nate@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>
Co-authored-by: Ian Chen <ichen@osrfoundation.org>
  • Loading branch information
@nkoenig @iche033
3 people committed May 20, 2020
1 parent f869fb0 commit 6d91304
Show file tree
Hide file tree
Showing 5 changed files with 195 additions and 7 deletions.
43 changes: 42 additions & 1 deletion docker/download_models.sh
View file
Expand Up @@ -48,6 +48,7 @@ ign fuel download --url "https://fuel.ignitionrobotics.org/openrobotics/models/E
ign fuel download --url "https://fuel.ignitionrobotics.org/openrobotics/models/COSTAR_HUSKY_SENSOR_CONFIG_1" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/openrobotics/models/SOPHISTICATED_ENGINEERING_X2_SENSOR_CONFIG_1" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/openrobotics/models/SOPHISTICATED_ENGINEERING_X4_SENSOR_CONFIG_1" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/openrobotics/models/DronePlatformX1" -v 4

# Download the tunnel circuit models
ign fuel download --url "https://fuel.ignitionrobotics.org/openrobotics/models/subt_tunnel_staging_area" -v 4
Expand Down Expand Up @@ -106,7 +107,47 @@ ign fuel download --url "https://fuel.ignitionrobotics.org/openrobotics/models/U
ign fuel download --url "https://fuel.ignitionrobotics.org/openrobotics/models/Urban Large Room Split" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/openrobotics/models/Urban Large Room Split Lights" -v 4

# Download cave circuit tiles
# Download cave circuit tiles (Type A)
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave 2 Way 01 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave 3 Way 01 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave 3 Way 02 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave 3 Way Elevation 01 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave 3 Way Elevation 02 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave 3 Way Elevation 03 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave 4 Way 01 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Cap Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Cavern Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Corner 01 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Corner 02 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Corner 03 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Corner 04 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Elevation 01 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Elevation 02 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Elevation Corner Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Elevation Straight Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Split Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Straight Shift Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Straight Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave U Turn 01 Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave U Turn Elevation Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Vertical Shaft Cantilevered Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Vertical Shaft Dead End Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Vertical Shaft Straight Bottom Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Vertical Shaft Straight Top Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Vertical Shaft Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave 3 Way Elevation 02 Lights Type A " -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave 4 Way 01 Lights Type A " -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Corner 01 Lights Type A " -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Corner 02 Lights Type A " -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Corner 04 Lights Type A " -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Elevation Straight Lights Type A " -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Straight Lights Type A " -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave U Turn Elevation Lights Type A " -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Vertical Shaft Straight Bottom Lights Type A" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Transition Type A to and from Type B" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Transition Type A to and from Type B Lights" -v 4

# Download cave circuit tiles (Type B)
ign fuel download --url "https://fuel.ignitionrobotics.org/openrobotics/models/Cave Cavern Split 01 Type B" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/openrobotics/models/Cave Cavern Split 02 Type B" -v 4
ign fuel download --url "https://fuel.ignitionrobotics.org/openrobotics/models/Cave Starting Area Type B" -v 4
Expand Down
8 changes: 4 additions & 4 deletions subt_ign/include/subt_ign/VisibilityTable.hh
View file
Expand Up @@ -40,16 +40,16 @@ namespace subt
public: static const int32_t kMaxX = 2100;

/// \brief Min Y to sample.
public: static const int32_t kMinY = -350;
public: static const int32_t kMinY = -850;

/// \brief Max Y to sample.
public: static const int32_t kMaxY = 350;
public: static const int32_t kMaxY = 850;

/// \brief Min Z to sample.
public: static const int32_t kMinZ = -50;
public: static const int32_t kMinZ = -250;

/// \brief Max Z to sample.
public: static const int32_t kMaxZ = 50;
public: static const int32_t kMaxZ = 100;

/// \brief Class constructor. Create the visibility table from a graph in
/// DOT format.
Expand Down
102 changes: 100 additions & 2 deletions subt_ign/src/ConnectionHelper.cc
View file
Expand Up @@ -97,7 +97,46 @@ std::map<std::string, std::vector<ignition::math::Vector3d>>
{"Cave Corner 30 D Type B", {{12.5, 0, 0}, {0, 12.5, 0}}},
{"Cave Corner 30 D Lights Type B", {{12.5, 0, 0}, {0, 12.5, 0}}},
{"Cave Corner 30F D Type B", {{12.5, 0, 0}, {0, -12.5, 0}}},
{"Cave Corner 30F D Lights Type B", {{12.5, 0, 0}, {0, -12.5, 0}}}
{"Cave Corner 30F D Lights Type B", {{12.5, 0, 0}, {0, -12.5, 0}}},

{"Cave 2 Way 01 Type A", {{-25.0, 0.0, 0.0}, {25.0, 0.0, 0.0}}},
{"Cave 3 Way 01 Type A", {{0.0, -25.0, 0.0}, {-25.0, 50.0, 0.0}, {50.0, 25.0, 0.0}}},
{"Cave 3 Way 02 Type A", {{-50.0, 25.0, 0.0}, {25.0, 50.0, 0.0}, {0.0, -25.0, 0.0}}},
{"Cave 3 Way Elevation 01 Type A", {{0.0, 50.0, 0.0}, {-100.0, -50.0, -25.0}, {0.0, -50.0, -25.0}}},
{"Cave 3 Way Elevation 02 Type A", {{0.0, -50.0, 0.0}, {-100.0, -50.0, -25.0}, {0.0, 50.0, 0.0}}},
{"Cave 3 Way Elevation 03 Type A", {{0.0, 50.0, 75.0}, {50.0, 0.0, -50.0}, {0.0, -50.0, 0.0}}},
{"Cave 4 Way 01 Type A", {{0.0, 25.0, 0.0}, {25.0, 0.0, 0.0}, {0.0, -25.0, 0.0}, {-25.0, 0.0, 0.0}}},
{"Cave Cavern Type A", {{-25.0, 0.0, 25.0}, {25.0, 0.0, 25.0}, {0.0, 25.0, 0.0}, {0.0, -25.0, 0.0}}},
{"Cave Corner 01 Type A", {{25.0, 0.0, 0.0}, {0.0, -25.0, 0.0}}},
{"Cave Corner 02 Type A", {{0.0, 25.0, 0.0}, {25.0, -50.0, 0.0}}},
{"Cave Corner 03 Type A", {{-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 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}}},
{"Cave Elevation Straight Type A", {{0.0, 50.0, 25.0}, {0.0, -50.0, 0.0}}},
{"Cave Split Type A", {{0.0, -50.0, 0.0}, {0.0, 50.0, 0.0}}},
{"Cave Straight Shift Type A", {{-25.0, 25.0, 0.0}, {25.0, -25.0, 0.0}}},
{"Cave Straight Type A", {{0.0, 25.0, 0.0}, {0.0, -25.0, 0.0}}},
{"Cave U Turn 01 Type A", {{-25.0, -25.0, 0.0}, {25.0, -25.0, 0.0}}},
{"Cave U Turn Elevation Type A", {{-50.0, -25.0, 0.0}, {0.0, -25.0, 25.0}}},
{"Cave Vertical Shaft Cantilevered Type A", {{0.0, -5.46392e-07, 10.0}, {0.0, 50.0, 25.0}}},
{"Cave Vertical Shaft Straight Bottom Type A", {{0.0, 0.0, 10.0}, {0.0, 25.0, 0.0}, {0.0, -25.0, 0.0}}},
{"Cave Vertical Shaft Straight Top Type A", {{0.0, 25.0, 0.0}, {0.0, -25.0, 0.0}, {0.0, 0.0, 0.0}}},
{"Cave Vertical Shaft Type A", {{0.0, 0.0, 10.0}, {0.0, 0.0, 25.0}}},
{"Cave Vertical Shaft Dead End Type A", {{0.0, 0.0, 10.0}}},

{"Cave 3 Way Elevation 02 Lights Type A", {{0.0, -50.0, 0.0}, {-100.0, -50.0, -25.0}, {0.0, 50.0, 0.0}}},
{"Cave 4 Way 01 Lights Type A", {{0.0, 25.0, 0.0}, {25.0, 0.0, 0.0}, {0.0, -25.0, 0.0}, {-25.0, 0.0, 0.0}}},
{"Cave Corner 01 Lights Type A", {{25.0, 0.0, 0.0}, {0.0, -25.0, 0.0}}},
{"Cave Corner 02 Lights Type A", {{0.0, 25.0, 0.0}, {25.0, -50.0, 0.0}}},
{"Cave Corner 04 Lights Type A", {{0.0, 50.0, 25.0}, {-50.0, -50.0, 0.0}}},
{"Cave Elevation Straight Lights Type A", {{0.0, 50.0, 25.0}, {0.0, -50.0, 0.0}}},
{"Cave Straight Lights Type A", {{0.0, 25.0, 0.0}, {0.0, -25.0, 0.0}}},
{"Cave U Turn Elevation Lights Type A", {{-50.0, -25.0, 0.0}, {0.0, -25.0, 25.0}}},
{"Cave Vertical Shaft Straight Bottom Lights Type A", {{0.0, 0.0, 10.0}, {0.0, 25.0, 0.0}, {0.0, -25.0, 0.0}}},
{"Cave Transition Type A to and from Type B", {{0.0, 25.0, 0.0}, {0.0, -25.0, 0.0}}},
{"Cave Transition Type A to and from Type B Lights", {{0.0, 25.0, 0.0}, {0.0, -25.0, 0.0}}}
};

std::map<std::string, subt::ConnectionHelper::ConnectionType>
Expand Down Expand Up @@ -169,7 +208,45 @@ std::map<std::string, subt::ConnectionHelper::ConnectionType>
{"Cave Corner 30 D Type B", subt::ConnectionHelper::TURN},
{"Cave Corner 30 D Lights Type B", subt::ConnectionHelper::TURN},
{"Cave Corner 30F D Type B", subt::ConnectionHelper::TURN},
{"Cave Corner 30F D Lights Type B", subt::ConnectionHelper::TURN}
{"Cave Corner 30F D Lights Type B", subt::ConnectionHelper::TURN},

{"Cave 2 Way 01 Type A", subt::ConnectionHelper::STRAIGHT},
{"Cave 3 Way 01 Type A", subt::ConnectionHelper::TURN},
{"Cave 3 Way 02 Type A", subt::ConnectionHelper::TURN},
{"Cave 3 Way Elevation 01 Type A", subt::ConnectionHelper::TURN},
{"Cave 3 Way Elevation 02 Type A", subt::ConnectionHelper::TURN},
{"Cave 3 Way Elevation 03 Type A", subt::ConnectionHelper::TURN},
{"Cave 4 Way 01 Type A", subt::ConnectionHelper::TURN},
{"Cave Cavern Type A", subt::ConnectionHelper::TURN},
{"Cave Corner 01 Type A", subt::ConnectionHelper::TURN},
{"Cave Corner 02 Type A", subt::ConnectionHelper::TURN},
{"Cave Corner 03 Type A", subt::ConnectionHelper::TURN},
{"Cave Corner 04 Type A", 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},
{"Cave Elevation Straight Type A", subt::ConnectionHelper::TURN},
{"Cave Split Type A", subt::ConnectionHelper::TURN},
{"Cave Straight Shift Type A", subt::ConnectionHelper::STRAIGHT},
{"Cave Straight Type A", subt::ConnectionHelper::STRAIGHT},
{"Cave U Turn 01 Type A", subt::ConnectionHelper::TURN},
{"Cave U Turn Elevation Type A", subt::ConnectionHelper::TURN},
{"Cave Vertical Shaft Cantilevered Type A", subt::ConnectionHelper::TURN},
{"Cave Vertical Shaft Straight Bottom Type A", subt::ConnectionHelper::TURN},
{"Cave Vertical Shaft Straight Top Type A", subt::ConnectionHelper::TURN},
{"Cave Vertical Shaft Type A", subt::ConnectionHelper::STRAIGHT},
{"Cave Vertical Shaft Dead End Type A", subt::ConnectionHelper::STRAIGHT},
{"Cave 3 Way Elevation 02 Lights Type A", subt::ConnectionHelper::TURN},
{"Cave 4 Way 01 Lights Type A", subt::ConnectionHelper::TURN},
{"Cave Corner 01 Lights Type A", subt::ConnectionHelper::TURN},
{"Cave Corner 02 Lights Type A", subt::ConnectionHelper::TURN},
{"Cave Corner 04 Lights Type A", subt::ConnectionHelper::TURN},
{"Cave Elevation Straight Lights Type A", subt::ConnectionHelper::TURN},
{"Cave Straight Lights Type A", subt::ConnectionHelper::STRAIGHT},
{"Cave U Turn Elevation Lights Type A", subt::ConnectionHelper::TURN},
{"Cave Vertical Shaft Straight Bottom Lights Type A", subt::ConnectionHelper::TURN},
{"Cave Transition Type A to and from Type B", subt::ConnectionHelper::STRAIGHT},
{"Cave Transition Type A to and from Type B Lights", subt::ConnectionHelper::STRAIGHT}
};

using namespace ignition;
Expand Down Expand Up @@ -227,3 +304,24 @@ bool ConnectionHelper::ComputePoint(VertexData *_tile1, VertexData *_tile2,

return false;
}

/////////////////////////////////////////////////
std::vector<ignition::math::Vector3d> ConnectionHelper::GetConnectionPoints(VertexData *_tile1)
{
auto ret = std::vector<ignition::math::Vector3d>();

if (!ConnectionHelper::connectionPoints.count(_tile1->tileType))
{
ignwarn << "No connection information for: " << _tile1->tileType
<< std::endl;
}
else
{
for (const auto& pt1 : ConnectionHelper::connectionPoints[_tile1->tileType])
{
auto pt1tf = _tile1->model.Pose().CoordPositionAdd(pt1);
ret.push_back(pt1tf);
}
}
return ret;
}
2 changes: 2 additions & 0 deletions subt_ign/src/ConnectionHelper.hh
View file
Expand Up @@ -67,6 +67,8 @@ namespace subt
public: static bool ComputePoint(VertexData *_tile1, VertexData *_tile2,
ignition::math::Vector3d& _pt);

public: static std::vector<ignition::math::Vector3d> GetConnectionPoints(VertexData *_tile1);

/// \brief Map of tile type to a vector of connection points
public: static std::map<std::string, std::vector<ignition::math::Vector3d>>
connectionPoints;
Expand Down
47 changes: 47 additions & 0 deletions subt_ign/src/ConnectionValidatorPrivate.cc
View file
Expand Up @@ -240,6 +240,8 @@ void ConnectionValidatorPrivate::PopulateConnections()
// Assuming 1 world per SDF, which is true for SubT.
auto world = this->sdfRoot.WorldByIndex(0);

std::vector<ignition::math::Vector3d> caps;

for (uint64_t modelIndex = 0; modelIndex < world->ModelCount(); ++modelIndex)
{
auto model = world->ModelByIndex(modelIndex);
Expand All @@ -250,6 +252,20 @@ void ConnectionValidatorPrivate::PopulateConnections()
{
this->vertData[name].model = *model;
}
else if (name.find("cap") != std::string::npos)
{
caps.push_back(pose.Pos());
}
}

std::map<std::string, int> expectedConnections;
std::map<std::string, int> actualConnections;

for (const auto [name, data]: vertData)
{
auto points = ConnectionHelper::connectionPoints[data.tileType];
expectedConnections[name] = points.size();
actualConnections[name] = 0;
}

// Pull edge data from the DOT graph file.
Expand Down Expand Up @@ -290,11 +306,42 @@ void ConnectionValidatorPrivate::PopulateConnections()
// Only add to the list if a connection point can be computed.
if(ConnectionHelper::ComputePoint(c.tile1, c.tile2, c.connectionPoint))
{
actualConnections[conn1->first]++;
actualConnections[conn2->first]++;
connData.push_back(c);
}
}

igndbg << "Populated " << connData.size() << "/" << graph.Edges().size() << std::endl;

for (const auto [name, data]: expectedConnections)
{
auto actual = actualConnections[name];

if (actual != data)
{
// Check against caps
int found_caps = 0;
auto points = ConnectionHelper::GetConnectionPoints(&this->vertData[name]);
for (auto point: points)
{
for (auto cap: caps)
{
if (point.Equal(cap, 1))
{
igndbg << "Found cap!" << std::endl;
found_caps += 1;
}
}
}

if (actual + found_caps != data)
{
igndbg << name << " " << actualConnections[name] << "/" << data <<
" (" << this->vertData[name].tileType << ")" << std::endl;
}
}
}

this->connDataIter = this->connData.end();
}

0 comments on commit 6d91304

Please sign in to comment.