From 7313fd1c43ae984aac5222c08ce54a48ac9e0e25 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 6 Apr 2020 07:48:44 -0700 Subject: [PATCH 1/2] Connection validator --- subt_ign/CMakeLists.txt | 22 + .../include/subt_ign/ConnectionValidator.hh | 55 +++ subt_ign/launch/validate_connections.ign | 392 +++++++++++++++++ subt_ign/src/ConnectionValidator.cc | 74 ++++ subt_ign/src/ConnectionValidatorPrivate.cc | 410 ++++++++++++++++++ subt_ign/src/ConnectionValidatorPrivate.hh | 139 ++++++ subt_ign/src/ConnectionValidator_TEST.cc | 33 ++ subt_ros/launch/validator_topics.launch | 32 ++ subt_ros/rviz/connection_validator.rviz | 151 +++++++ 9 files changed, 1308 insertions(+) create mode 100644 subt_ign/include/subt_ign/ConnectionValidator.hh create mode 100644 subt_ign/launch/validate_connections.ign create mode 100644 subt_ign/src/ConnectionValidator.cc create mode 100644 subt_ign/src/ConnectionValidatorPrivate.cc create mode 100644 subt_ign/src/ConnectionValidatorPrivate.hh create mode 100644 subt_ign/src/ConnectionValidator_TEST.cc create mode 100644 subt_ros/launch/validator_topics.launch create mode 100644 subt_ros/rviz/connection_validator.rviz diff --git a/subt_ign/CMakeLists.txt b/subt_ign/CMakeLists.txt index 6347b3a8..2ce8de72 100644 --- a/subt_ign/CMakeLists.txt +++ b/subt_ign/CMakeLists.txt @@ -130,6 +130,28 @@ install(TARGETS ${artifact_validator_name} LIBRARY DESTINATION lib RUNTIME DESTINATION bin) +set(connection_validator_name ConnectionValidator) +add_library(${connection_validator_name} SHARED + src/ConnectionValidator.cc + src/ConnectionValidatorPrivate.cc + src/SimpleDOTParser.cc) +target_link_libraries(${connection_validator_name} + 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 ${connection_validator_name} + 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/include/subt_ign/ConnectionValidator.hh b/subt_ign/include/subt_ign/ConnectionValidator.hh new file mode 100644 index 00000000..358874d9 --- /dev/null +++ b/subt_ign/include/subt_ign/ConnectionValidator.hh @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef SUBT_IGN_CONNECTIONVALIDATOR_HH_ +#define SUBT_IGN_CONNECTIONVALIDATOR_HH_ + +#include + +#include + +namespace subt +{ + class ConnectionValidatorPrivate; + + class ConnectionValidator: + public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPostUpdate + { + /// \brief Constructor + public: ConnectionValidator(); + + /// \brief Destructor + public: ~ConnectionValidator(); + + // Documentation inherited + public: void Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &_eventMgr) override; + + // Documentation inherited + public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; +} + +#endif // SUBT_IGN_CONNECTIONVALIDATOR_HH_ diff --git a/subt_ign/launch/validate_connections.ign b/subt_ign/launch/validate_connections.ign new file mode 100644 index 00000000..4befbf4a --- /dev/null +++ b/subt_ign/launch/validate_connections.ign @@ -0,0 +1,392 @@ + + + +<% + # Check if worldName is not defined or is empty/nil + if !defined?(worldName) || worldName == nil || worldName.empty? + $worldName = 'tunnel_circuit_practice_01' + else + $worldName = worldName + end + + if !defined?(headless) || headless == nil || headless.empty? + $headless = false + elsif headless == 'true' || headless == 'True' || headless == '1' + $headless = true + end + + worldNumber = $worldName.split('_').last +%> + +<% + # disable levels for simple cave worlds + levels = "true" + if $worldName.include?('simple_cave_') + levels = "false" + end +%> + + + + IGN_GAZEBO_SYSTEM_PLUGIN_PATH + $LD_LIBRARY_PATH + + + + roslaunch subt_ros validator_topics.launch world_name:=<%= $worldName %> name:=validator + + + + <% if $worldName.include?('tunnel_circuit_') && + !$worldName.include?('practice') %> + tunnel_circuit/<%= worldNumber %>/<%= $worldName %>.sdf + <% elsif $worldName.include?('urban_circuit_') && + !$worldName.include?('practice') %> + urban_circuit/<%= worldNumber %>/<%= $worldName %>.sdf + <% else %> + <%= $worldName %>.sdf + <% end %> + true + <%= levels %> + <%if defined?(seed) && seed != nil && !seed.empty?%> + <%= seed %> + <%end%> + + + + + ogre2 + + + + + + + + + <%= $worldName %> + + + + + <%if !$headless %> + + + <%= $worldName %> + SubT Simulator + <%= ENV['SUBT_IMAGES_PATH'] %>/SubT_logo.svg + + + 3D View + false + docked + + + ogre2 + scene + 0.2 0.2 0.1 + 0.8 0.8 0.8 + -6.3 -4.2 3.6 0 0.268 0.304 + /world/<%= $worldName %>/scene/info + /world/<%= $worldName %>/pose/info + /world/<%= $worldName %>/scene/info + /world/<%= $worldName %>/scene/deletion + + 0.05 + validator + -4 0 15 + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/<%= $worldName %>/control + /world/<%= $worldName %>/stats + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/<%= $worldName %>/stats + + + + + Angled + docked + + /validate/angled + false + + + + + Top-down + docked + + /validate/top_down + false + + + + <%end%> + + + validator + false + 0 0 0 0 0 0 + true + + + true + + + 0 0 5 0 0 0 + + + 1.0 1.0 1.0 + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + 2.5 2.5 7.0 0 0 0 + 1 0 0 1 + 1 0 0 1 + + 5 + 0 + 1 + + false + + + -2.5 2.5 7.0 0 0 0 + 1 0 0 1 + 1 0 0 1 + + 5 + 0 + 1 + + false + + + 2.5 -2.5 7.0 0 0 0 + 1 0 0 1 + 1 0 0 1 + + 5 + 0 + 1 + + false + + + -2.5 -2.5 7.0 0 0 0 + 1 0 0 1 + 1 0 0 1 + + 5 + 0 + 1 + + false + + + 0 0 1.0 0 0 0 + + 50 + 0 + 0.1 + 0.0025 + + 0.8 0.8 0.5 1 + 0.8 0.8 0.5 1 + + 1 + 1.5 + 1 + + 0 0 -1 + false + + + /validate/top_down + 0.0 0 4 0 1.570 0 + 1 + 20 + + 0 0 0 0 0 0 + 1.0472 + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 +
0.5 0.5
+ + + 640 + 480 + R8G8B8 + + + 0.01 + 300 + + + + + /validate/angled + -4 0 4 0 0.78 0.0 + 1 + 20 + + 0 0 0 0 0 0 + 1.0472 + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 +
0.5 0.5
+ + + 640 + 480 + R8G8B8 + + + 0.01 + 300 + + + + + 0 0 2 1.54 1.54 0 + 0 + 1 + + + + + 200 + 0.1 + -3.1459 + 3.1459 + + + 80 + 0.1 + -1.4 + 1.4 + + + + 0.01 + 10 + 0.035 + + + gaussian + 0 + 0.0 + + + + + + true + true + false + false + true + + + + + diff --git a/subt_ign/src/ConnectionValidator.cc b/subt_ign/src/ConnectionValidator.cc new file mode 100644 index 00000000..69b9332e --- /dev/null +++ b/subt_ign/src/ConnectionValidator.cc @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "subt_ign/ConnectionValidator.hh" +#include "ConnectionValidatorPrivate.hh" + +#include +#include +#include + +#include "subt_ign/CommonTypes.hh" + +IGNITION_ADD_PLUGIN( + subt::ConnectionValidator, + ignition::gazebo::System, + subt::ConnectionValidator::ISystemConfigure, + subt::ConnectionValidator::ISystemPostUpdate +) + +using namespace ignition; +using namespace gazebo; +using namespace systems; +using namespace subt; + + +///////////////////////////////////////////////// +ConnectionValidator::ConnectionValidator() + : dataPtr(new ConnectionValidatorPrivate) +{ +} + +///////////////////////////////////////////////// +ConnectionValidator::~ConnectionValidator() = default; + +///////////////////////////////////////////////// +void ConnectionValidator::Configure(const ignition::gazebo::Entity & /*_entity*/, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager & /*_ecm*/, + ignition::gazebo::EventManager & /*_eventMgr*/) +{ + this->dataPtr->worldName = const_cast( + _sdf.get())->Get("world_name", "world_name").first; + + igndbg << "Set world name: " << this->dataPtr->worldName << std::endl; + + this->dataPtr->Load(this->dataPtr->worldName); + this->dataPtr->PopulateConnections(); + + this->dataPtr->node.Advertise("/connection/next", + &ConnectionValidatorPrivate::OnNext, this->dataPtr.get()); + this->dataPtr->node.Advertise("/connection/prev", + &ConnectionValidatorPrivate::OnPrev, this->dataPtr.get()); +} + +////////////////////////////////////////////////// +void ConnectionValidator::PostUpdate( + const ignition::gazebo::UpdateInfo &/*_info*/, + const ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ +} diff --git a/subt_ign/src/ConnectionValidatorPrivate.cc b/subt_ign/src/ConnectionValidatorPrivate.cc new file mode 100644 index 00000000..73690c78 --- /dev/null +++ b/subt_ign/src/ConnectionValidatorPrivate.cc @@ -0,0 +1,410 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "ConnectionValidatorPrivate.hh" + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include + +using namespace subt; +using namespace ignition; + +///////////////////////////////////////////////// +bool ConnectionValidatorPrivate::Load(const std::string &_worldName) +{ + std::string worldsDirectory = SUBT_INSTALL_WORLD_DIR; + + // Modifications for the tunnel circuit. + const std::string tunnelPrefix = "tunnel_circuit_"; + const std::string urbanPrefix = "urban_circuit_"; + const std::string cavePrefix = "cave_circuit_"; + if (0 == _worldName.compare(0, tunnelPrefix.size(), tunnelPrefix)) + { + std::string suffix = _worldName.substr(tunnelPrefix.size()); + // don't use a subfolder for practice worlds + if (0 != suffix.compare(0, 9, "practice_")) + { + worldsDirectory = ignition::common::joinPaths(worldsDirectory, + "tunnel_circuit", suffix); + } + } + else if (0 == _worldName.compare(0, urbanPrefix.size(), urbanPrefix)) + { + std::string suffix = _worldName.substr(urbanPrefix.size()); + // don't use a subfolder for practice worlds + if (0 != suffix.compare(0, 9, "practice_")) + { + worldsDirectory = ignition::common::joinPaths(worldsDirectory, + "urban_circuit", suffix); + } + } + else if (0 == _worldName.compare(0, cavePrefix.size(), cavePrefix)) + { + std::string suffix = _worldName.substr(cavePrefix.size()); + // don't use a subfolder for practice worlds + if (0 != suffix.compare(0, 9, "practice_")) + { + worldsDirectory = ignition::common::joinPaths(worldsDirectory, + "cave_circuit", suffix); + } + } + else if (_worldName.find("simple") == std::string::npos) + { + ignwarn << "Unable to determine circuit number from[" + << _worldName << "].\n"; + } + + std::string worldPath = common::joinPaths( + worldsDirectory, _worldName + ".sdf"); + + std::string graphPath = common::joinPaths( + worldsDirectory, _worldName + ".dot"); + + auto ret = LoadSdf(worldPath) && LoadDot(graphPath); + if (ret) + this->worldName = _worldName; + return ret; +} + +///////////////////////////////////////////////// +bool ConnectionValidatorPrivate::LoadSdf(const std::string &_fpath) +{ + // Configure the fuel client + fuel_tools::ClientConfig config; + fuel_tools::FuelClient fuelClient(config); + + sdf::setFindCallback([&](const std::string &_uri) { + return fuel_tools::fetchResourceWithClient(_uri, fuelClient); + }); + + igndbg << "Parsing [" << _fpath << "]" << std::endl; + + auto errors = sdfRoot.Load(_fpath); + if (!errors.empty()) + { + for (auto &err: errors) + ignerr << err << "\n"; + return false; + } + return true; +} + +///////////////////////////////////////////////// +bool ConnectionValidatorPrivate::LoadDot(const std::string &_fpath) +{ + std::filebuf fb; + if (!fb.open(_fpath, std::ios::in)) + { + ignerr << "Unable to read [" << _fpath << "] file" << std::endl; + return false; + } + + igndbg << "Parsing [" << _fpath << "]" << std::endl; + + std::istream is(&fb); + subt::SimpleDOTParser dotParser; + if (!dotParser.Parse(is)) + return false; + + this->graph = dotParser.Graph(); + + return true; +} + +///////////////////////////////////////////////// +bool ConnectionValidatorPrivate::Move(const ignition::math::Vector3d& pt) +{ + ignition::msgs::Pose req; + req.set_name("validator"); + req.mutable_position()->set_x(pt.X()); + req.mutable_position()->set_y(pt.Y()); + req.mutable_position()->set_z(pt.Z()); + + msgs::Boolean res; + bool result; + unsigned int timeout = 1000; + std::string service {"/world/" + this->worldName + "/set_pose"}; + node.Request(service, req, timeout, res, result); + igndbg << "Result: " << res.data() << " " << result << std::endl; + return result; +} + +///////////////////////////////////////////////// +bool ConnectionValidatorPrivate::OnNext(const ignition::msgs::StringMsg& /*_req*/, + ignition::msgs::StringMsg& _rep) +{ + this->connDataIter++; + + if (this->connDataIter >= this->connData.end()) + { + this->connDataIter = this->connData.begin(); + } + + std::stringstream ss; + ss << "Moving to: " + << "[(" << this->connDataIter->tile1->id << ") " + << this->connDataIter->tile1->tileName << "(" << this->connDataIter->tile1->tileType << ")]" + << " -- " + << "[(" << this->connDataIter->tile2->id << ") " + << this->connDataIter->tile2->tileName << "(" << this->connDataIter->tile2->tileType << ")]\n" + << "[" << this->connDataIter->connectionPoint << "]" << std::endl; + + _rep.set_data(ss.str()); + + igndbg << ss.str() << std::endl; + return this->Move(this->connDataIter->connectionPoint); +} + +///////////////////////////////////////////////// +bool ConnectionValidatorPrivate::OnPrev(const ignition::msgs::StringMsg& /*_req*/, + ignition::msgs::StringMsg& _rep) +{ + this->connDataIter--; + + std::stringstream ss; + ss << "Moving to: " + << "[(" << this->connDataIter->tile1->id << ") " + << this->connDataIter->tile1->tileName << "(" << this->connDataIter->tile1->tileType << ")]" + << " -- " + << "[(" << this->connDataIter->tile2->id << ") " + << this->connDataIter->tile2->tileName << "(" << this->connDataIter->tile2->tileType << ")]\n" + << "[" << this->connDataIter->connectionPoint << "]" << std::endl; + + _rep.set_data(ss.str()); + + igndbg << ss.str() << std::endl; + auto ret = this->Move(this->connDataIter->connectionPoint); + + if (this->connDataIter == this->connData.begin()) + { + this->connDataIter = this->connData.end(); + this->connDataIter--; + } + return ret; +} + +///////////////////////////////////////////////// +void ConnectionValidatorPrivate::PopulateConnections() +{ + // Pull vertex data from the DOT graph file. + auto verts = this->graph.Vertices(); + for (const auto vert : verts) + { + auto vv = vert.second.get(); + auto data = ignition::common::Split( + vv.Data().substr(1, vv.Data().length() - 2), ':'); + VertexData dd; + + dd.id = stoi(data[0]); + dd.tileType = data[2]; + dd.tileName = data[4]; + + if (dd.tileType == "base_station") + { + if (this->worldName.find("urban") != std::string::npos) + dd.tileType = "Urban Starting Area"; + else if (this->worldName.find("cave") != std::string::npos) + dd.tileType = "Cave Starting Area"; + dd.tileName = "staging_area"; + } + + this->vertData[data[4]] = dd; + } + + // Add SDF::Model data to each of the vertices + // Assuming 1 world per SDF, which is true for SubT. + auto world = this->sdfRoot.WorldByIndex(0); + + for (uint64_t modelIndex = 0; modelIndex < world->ModelCount(); ++modelIndex) + { + auto model = world->ModelByIndex(modelIndex); + const auto pose = model->Pose(); + const auto name = model->Name(); + + if (this->vertData.count(name)) + { + this->vertData[name].model = *model; + } + } + + // Pull edge data from the DOT graph file. + auto edges = this->graph.Edges(); + + for (const auto [id, edge]: edges) + { + auto conn1 = std::find_if(this->vertData.begin(), + this->vertData.end(), + [&]( const auto & vert ) { + return vert.second.id == static_cast(edge.get().Vertices().first); + }); + + auto conn2 = std::find_if(this->vertData.begin(), + this->vertData.end(), + [&]( const auto & vert ) { + return vert.second.id == static_cast(edge.get().Vertices().second); + }); + + if (conn1 == this->vertData.end()) + { + ignerr << "Could not find vertex information for ID: " + << edge.get().Vertices().first << std::endl; + continue; + } + + if (conn2 == this->vertData.end()) + { + ignerr << "Could not find vertex information for ID: " + << edge.get().Vertices().second << std::endl; + continue; + } + + Connection c; + c.id = id; + 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)) + { + connData.push_back(c); + } + } + + igndbg << "Populated " << connData.size() << "/" << graph.Edges().size() << std::endl; + + this->connDataIter = this->connData.end(); +} + +///////////////////////////////////////////////// +bool subt::ComputePoint(VertexData *_tile1, VertexData *_tile2, + ignition::math::Vector3d& _pt) +{ + std::map> 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 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; +} diff --git a/subt_ign/src/ConnectionValidatorPrivate.hh b/subt_ign/src/ConnectionValidatorPrivate.hh new file mode 100644 index 00000000..c6cd1506 --- /dev/null +++ b/subt_ign/src/ConnectionValidatorPrivate.hh @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef SUBT_IGN_CONNECTIONVALIDATORPRIVATE_HH_ +#define SUBT_IGN_CONNECTIONVALIDATORPRIVATE_HH_ + +#include + +#include +#include + +#include +#include + +#include + +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 + { + /// \brief ID of the connection (from dot graph) + int id; + + /// \brief Pointer to first + VertexData *tile1; + + /// \brief Pointer to second tile + VertexData *tile2; + + /// \brief Computed connection point between the tiles. + ignition::math::Vector3d connectionPoint; + }; + + /// \brief Private data for the connection validator + class ConnectionValidatorPrivate + { + /// \brief Load both the SDF and DOT graph content for a given world + /// \param[in] _worldName name of the world to load (eg "urban_qual") + /// \return True when all content is succesfully loaded, false otherwise. + public: bool Load(const std::string &_worldName); + + /// \brief Load SDF content for a given world + /// \param[in] _worldName name of the world to load (eg "urban_qual") + /// \return True when all content is succesfully loaded, false otherwise. + private: bool LoadSdf(const std::string &_fname); + + /// \brief Load connectivity graph content for a given world + /// \param[in] _worldName name of the world to load (eg "urban_qual") + /// \return True when all content is succesfully loaded, false otherwise. + private: bool LoadDot(const std::string &_fname); + + /// \brief Extract connection data from the graph/sdf world. + public: void PopulateConnections(); + + /// \brief Move the validator actor to a given point + /// \param[in] _pt Point to move artifact to + /// \return True when service call was successful, false otherwise. + public: bool Move(const ignition::math::Vector3d& _pt); + + /// \brief Callback fired when the next service is called. + /// \param[in] _msg Service request + /// \param[out] _rep Service response + public: bool OnNext(const ignition::msgs::StringMsg &_msg, + ignition::msgs::StringMsg &_rep); + + /// \brief Callback fired when the next service is called. + /// \param[in] _msg Service request + /// \param[out] _rep Service response + public: bool OnPrev(const ignition::msgs::StringMsg &_msg, + ignition::msgs::StringMsg &_rep); + + /// \brief Name of the world. + public: std::string worldName; + + /// \brief SDF content of the world. + public: sdf::Root sdfRoot; + + /// \brief Visibility graph content of the world. + public: VisibilityGraph graph; + + /// \brief Ignition transport node. + public: ignition::transport::Node node; + + /// \brief Vertex data from SDF/dot, key is tile name (eg "tile_1") + public: std::map vertData; + + /// \brief Connection data from SDF/dot + public: std::vector connData; + + /// \brief Iterator to current location in the connection data. + /// Used in support of next/prev service calls + public: std::vector::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_ + diff --git a/subt_ign/src/ConnectionValidator_TEST.cc b/subt_ign/src/ConnectionValidator_TEST.cc new file mode 100644 index 00000000..d52003a6 --- /dev/null +++ b/subt_ign/src/ConnectionValidator_TEST.cc @@ -0,0 +1,33 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +/// Helper test to evaluate graph population without starting gazebo. +/// Call with TEST_connection_validator +#include "ConnectionValidatorPrivate.hh" + +#include + +int main(int /*argc*/, char** argv) +{ + ignition::common::Console::SetVerbosity(4); + + auto cv = subt::ConnectionValidatorPrivate(); + cv.Load(argv[1]); + + cv.PopulateConnections(); +} + diff --git a/subt_ros/launch/validator_topics.launch b/subt_ros/launch/validator_topics.launch new file mode 100644 index 00000000..b88307fc --- /dev/null +++ b/subt_ros/launch/validator_topics.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/subt_ros/rviz/connection_validator.rviz b/subt_ros/rviz/connection_validator.rviz new file mode 100644 index 00000000..af45c22f --- /dev/null +++ b/subt_ros/rviz/connection_validator.rviz @@ -0,0 +1,151 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21/Status1 + Splitter Ratio: 0.4095693826675415 + Tree Height: 1103 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 6.002923488616943 + Min Value: 0.9104812145233154 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 100 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /validator/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: urban_circuit_04 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 132.28228759765625 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -2.6620118618011475 + Y: -0.4445297122001648 + Z: 5.099700927734375 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4053986966609955 + Target Frame: validator + Value: Orbit (rviz) + Yaw: 3.944934844970703 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1383 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001c4000004d4fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000034000004d4000000b900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000025c0000015a0000000000000000fb0000000a0049006d00610067006501000003bb0000014d000000000000000000000001000000fe000004d4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000034000004d40000009600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009fc00000040fc0100000002fb0000000800540069006d00650100000000000009fc0000021900fffffffb0000000800540069006d0065010000000000000450000000000000000000000730000004d400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2556 + X: 2560 + Y: 36 From 9108050ecf1c0775ce1e9d3d7804015b5cc45189 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 6 Apr 2020 16:17:18 +0000 Subject: [PATCH 2/2] Close branch connection_validator