Skip to content

Commit

Permalink
reuse connection helper code in connection validator
Browse files Browse the repository at this point in the history
  • Loading branch information
@iche033
iche033 committed Apr 11, 2020
1 parent 82f6982 commit 1d1c233
Show file tree
Hide file tree
Showing 2 changed files with 2 additions and 142 deletions.
113 changes: 1 addition & 112 deletions subt_ign/src/ConnectionValidatorPrivate.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -287,7 +287,7 @@ void ConnectionValidatorPrivate::PopulateConnections()
c.tile1 = &conn1->second;
c.tile2 = &conn2->second;
// Only add to the list if a connection point can be computed.
if(ComputePoint(c.tile1, c.tile2, c.connectionPoint))
if(ConnectionHelper::ComputePoint(c.tile1, c.tile2, c.connectionPoint))
{
connData.push_back(c);
}
Expand All @@ -297,114 +297,3 @@ void ConnectionValidatorPrivate::PopulateConnections()

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

/////////////////////////////////////////////////
bool subt::ComputePoint(VertexData *_tile1, VertexData *_tile2,
ignition::math::Vector3d& _pt)
{
std::map<std::string,
std::vector<ignition::math::Vector3d>> connectionPoints = {
{"Urban Straight", {{0, 20, 0}, {0, -20, 0}}},
{"Urban Bend Right", {{0, -20, 0}, {20, 0, 0}}},
{"Urban Bend Left", {{0, -20, 0}, {-20, 0, 0}}},
{"Urban Superpose", {{0, 20, 0}, {0, -20, 0}, {-20, 0, 10}, {20, 0, 10}}},
{"Urban 3-Way Right Intersection", {{0, 20, 0}, {0, -20, 0}, {20, 0, 0}}},
{"Urban Straight Door Right", {{20, 0, 0}, {-20, 0, 0}, {-16.021, 3.94, 0.94}}},
{"Urban Straight Door Left", {{20, 0, 0}, {-20, 0, 0}, {-16.021, -3.94, 0.94}}},
{"Urban Straight Door Right Flipped", {{20, 0, 0}, {-20, 0, 0}, {-16.021, 3.94, 0.94}}},
{"Urban Straight Door Left Flipped", {{20, 0, 0}, {-20, 0, 0}, {-16.021, -3.94, 0.94}}},
{"Urban Straight Door Right Extension", {{20, 0, 0}, {-20, 0, 0}, {0, 20, 0}}},
{"Urban Service Room Centered", {{0, 20, 0}}},
{"Urban Service Room", {{-16.023, 3.906, 0.919}}},
{"Urban Service Room Straight", {{0, -20, 0}, {0, 20, 0}}},
{"Urban Platform", {{20, 0, 0}, {-20, 0, 0}, {0, 20, 1.7}, {23.979, 3.906, 0.94}, {-23.979, 3.906, 0.94}}},
{"Urban Platform Open", {{20, 0, 0}, {-20, 0, 0}, {0, 20, 0},
{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 Platform Centered", {{0, 20, 10}, {0, -20, 0}}},
{"Urban Starting Area", {{-16.021, 3.94, 0.919}}},
{"Urban Elevation Up", {{0, 20, 5}, {0, -20, 0}}},
{"Urban Elevation Down", {{0, 20, 0}, {0, -20, 5}}},
{"Urban 2 Story", {{0, 20, 10}, {0, -20, 0}, {-20, 0, 10}, {20, 0, 0}}},
{"Urban 2 Story Large Side 1", {{0, 20, 10}, {0, -20, 0}}},
{"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}}},
{"Cave Starting Area", {{12.5, 0, 0}}},
{"Cave Straight 01", {{0, 12.5, 0}, {0, -12.5, 0}}},
{"Cave Straight 02", {{0, 12.5, 0}, {0, -12.5, 0}}},
{"Cave Straight 03", {{0, 12.5, 0}, {0, -12.5, 0}}},
{"Cave Straight 04", {{0, 12.5, 0}, {0, -12.5, 0}}},
{"Cave Straight 05", {{0, 12.5, 0}, {0, -12.5, 0}}},
{"Cave Corner 01", {{-12.5, 0, 0}, {0, 12.5, 0}}},
{"Cave Corner 02", {{-12.5, 0, 0}, {0, 12.5, 0}}},
{"Cave 3 Way 01", {{12.5, 0, 0}, {-12.5, 0, 0}, {0, 12.5, 0}}},
{"Cave Elevation", {{0, 12.5, 0}, {0, -12.5, 10}}},
{"Cave Vertical Shaft", {{0, 12.5, 20}, {0, -12.5, 0}}},
{"Cave Cavern Split 01", {{0, 12.5, 12.5}, {12.5, 0, 0}, {-12.5, 0, 0}}},
{"Cave Cavern Split 02", {{12.5, 0, 0}, {-12.5, 0, 0}}},
};

std::vector<std::string> lights = {
"Urban Straight",
"Urban Bend Left",
"Urban Straight Door Right Extension",
"Urban Service Room Centered",
"Urban Service Room",
"Urban Service Room Straight",
"Urban Stairwell Platform",
"Urban Stairwell Platform Centered",
"Urban Elevation Up",
"Urban 2 Story",
"Urban 2 Story Large Side 1",
"Urban 2 Story Large Side 2",
"Urban Large Room Split",
"Urban Straight Door Right Flipped"
};

for (auto ll : lights)
{
connectionPoints[ll + " Lights"] = connectionPoints[ll];
}

if (!connectionPoints.count(_tile1->tileType))
{
ignwarn << "No connection information for: " << _tile1->tileType << std::endl;
return false;
}

if (!connectionPoints.count(_tile2->tileType))
{
ignwarn << "No connection information for: " << _tile2->tileType << std::endl;
return false;
}

for (const auto& pt1 : connectionPoints[_tile1->tileType])
{
auto pt1tf = _tile1->model.Pose().CoordPositionAdd(pt1);
for (const auto& pt2 : connectionPoints[_tile2->tileType])
{
auto pt2tf = _tile2->model.Pose().CoordPositionAdd(pt2);
if (pt1tf.Equal(pt2tf, 1))
{
_pt = pt1tf;
return true;
}
}
}

ignwarn << "Failed to connect: " << _tile1->tileType << " " << _tile2->tileType << std::endl;

for (const auto& pt1 : connectionPoints[_tile1->tileType])
{
auto pt1tf = _tile1->model.Pose().CoordPositionAdd(pt1);
for (const auto& pt2 : connectionPoints[_tile2->tileType])
{
auto pt2tf = _tile2->model.Pose().CoordPositionAdd(pt2);
igndbg <<
_tile1->tileType << " [" << _tile1->model.Pose() << "] -- " <<
_tile2->tileType << " [" << _tile2->model.Pose() << "] [" << pt1tf << "] [" << pt2tf << "]" << std::endl;
}
}

return false;
}
31 changes: 1 addition & 30 deletions subt_ign/src/ConnectionValidatorPrivate.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,32 +20,14 @@

#include <string>

#include <sdf/Root.hh>
#include <sdf/Model.hh>

#include <ignition/msgs/stringmsg.pb.h>
#include <ignition/transport/Node.hh>

#include <subt_ign/SimpleDOTParser.hh>
#include "ConnectionHelper.hh"

namespace subt
{
/// \brief Data about vertex (tile) in the environment
struct VertexData
{
/// \brief ID of the vertex (from dot graph)
int id;

/// \brief Type of the tile (eg "Urban Straight")
std::string tileType;

/// \brief Name of the tile as it appears in the SDF file. (eg "tile_1")
std::string tileName;

/// \brief SDF model content for the tile
sdf::Model model;
};

/// \brief Data about connections between tiles in the environment
struct Connection
{
Expand Down Expand Up @@ -122,17 +104,6 @@ namespace subt
/// Used in support of next/prev service calls
public: std::vector<Connection>::iterator connDataIter;
};

/// \brief Compute the connection point between two tiles.
/// This uses a reference list of connection points for each tile type.
/// The function iterates over the connection points transformed into the
/// world coordinate frame to find the location where the tiles meet.
/// \param[in] _tile1 vertex data for first connected tile
/// \param[in] _tile2 vertex data for second connected tile
/// \param[out] _pt Populated point data
/// \return True if connection point was found, false otherwise.
bool ComputePoint(VertexData *_tile1, VertexData *_tile2,
ignition::math::Vector3d& _pt);
}

#endif // SUBT_IGN_CONNECTIONVALIDATORPRIVATE_HH_
Expand Down

0 comments on commit 1d1c233

Please sign in to comment.