Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Reuse ConnectionHelper code in ConnectionValidator #390

Merged
merged 1 commit into from
Apr 14, 2020
Merged

Reuse ConnectionHelper code in ConnectionValidator #390

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
113 changes: 1 addition & 112 deletions subt_ign/src/ConnectionValidatorPrivate.cc
View file
Open in desktop
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
Open in desktop
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