Add level generator (#416)

* add level generator Signed-off-by: Ian Chen <[email protected]> * comment and simplify code Signed-off-by: Ian Chen <[email protected]> * Tweaks to the level generator (#423) Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]> Co-authored-by: Ian Chen <[email protected]> * fix typo Signed-off-by: Ian Chen <[email protected]> Co-authored-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]>
osrf · May 20, 2020 · e31ae65 · e31ae65
1 parent a1837a1
commit e31ae65
Show file tree

Hide file tree

Showing 5 changed files with 486 additions and 89 deletions.
diff --git a/subt_ign/CMakeLists.txt b/subt_ign/CMakeLists.txt
@@ -277,7 +277,7 @@ target_link_libraries(log_checker
 install(TARGETS log_checker
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
-add_executable(dot_generator src/dot_generator.cc src/ConnectionHelper.cc)
+add_executable(dot_generator src/dot_generator.cc src/ConnectionHelper.cc src/SdfParser.cc)
 target_link_libraries(dot_generator
   ignition-math6::ignition-math6
   ignition-common3::ignition-common3
@@ -287,6 +287,17 @@ target_include_directories(dot_generator
 install(TARGETS dot_generator
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
+add_executable(level_generator src/level_generator.cc src/ConnectionHelper.cc src/SdfParser.cc)
+target_link_libraries(level_generator
+  ignition-math6::ignition-math6
+  ignition-common3::ignition-common3
+  sdformat8::sdformat8)
+target_include_directories(level_generator
+  PRIVATE ${CATKIN_DEVEL_PREFIX}/include)
+install(TARGETS level_generator
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+
 ###########
 ## Tests ##
 ###########

diff --git a/subt_ign/src/SdfParser.cc b/subt_ign/src/SdfParser.cc
@@ -0,0 +1,100 @@
+/*
+ * 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 "ConnectionHelper.hh"
+#include "SdfParser.hh"
+
+using namespace ignition;
+using namespace subt;
+
+//////////////////////////////////////////////////
+std::string SdfParser::Parse(const std::string &_key, const std::string &_str,
+     size_t &_endPos)
+{
+  std::string elemStartStr = "<" + _key + ">";
+  std::string elemEndStr = "</" + _key + ">";
+
+  size_t start = _str.find(elemStartStr);
+  if (start == std::string::npos)
+    return std::string();
+  size_t startIdx = start + elemStartStr.size();
+  size_t end = _str.find(elemEndStr, startIdx);
+  if (end == std::string::npos)
+    return std::string();
+
+  _endPos = end + elemEndStr.size();
+  std::string result = _str.substr(startIdx, end - startIdx);
+  return result;
+}
+
+//////////////////////////////////////////////////
+std::string SdfParser::Parse(const std::string &_key, const std::string &_str)
+{
+  size_t endPos;
+  return Parse(_key, _str, endPos);
+}
+
+//////////////////////////////////////////////////
+bool SdfParser::FillVertexData(const std::string &_includeStr, VertexData &_vd,
+  std::function<bool(const std::string &, const std::string &)> &_filter)
+{
+  // parse name
+  std::string name = Parse("name", _includeStr);
+
+  // parse pose
+  std::string poseStr = Parse("pose", _includeStr);
+  math::Pose3d pose;
+  std::stringstream ss(poseStr);
+  ss >> pose;
+
+  // parse uri and get model type
+  std::string uri = Parse("uri", _includeStr);
+  std::string fuelStr =
+      "https://fuel.ignitionrobotics.org/1.0/openrobotics/models/";
+  size_t fuelIdx = uri.find(fuelStr);
+  std::string modelType;
+  if (fuelIdx == std::string::npos)
+    return false;
+  modelType = uri.substr(fuelIdx + fuelStr.size());
+
+  // check if model type is recognized
+  if (_filter && _filter(name, modelType))
+    return false;
+  sdf::Model modelSdf;
+  modelSdf.SetName(name);
+  modelSdf.SetPose(pose);
+
+  static int tileId = 0;
+  // Try getting the tile id from the tile name first.
+  try
+  {
+    int numIndex = name.rfind("_");
+    _vd.id = std::stoi(name.substr(numIndex+1));
+  }
+  catch (...)
+  {
+    _vd.id = tileId++;
+  }
+  _vd.tileType = modelType;
+  _vd.tileName = name;
+  _vd.model = modelSdf;
+
+  return true;
+}
+
+
+
diff --git a/subt_ign/src/SdfParser.hh b/subt_ign/src/SdfParser.hh
@@ -0,0 +1,48 @@
+/*
+ * 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 "ConnectionHelper.hh"
+
+namespace subt
+{
+
+class SdfParser
+{
+  /// \brief Parse contents of an sdf element
+  /// \param[in] _key SDF element key
+  /// \param[in] _str String content of the sdf element
+  /// \param[out] _endPos end position of the sdf element string
+  /// \return Value for the input key
+  public: static std::string Parse(const std::string &_key, const std::string &_str,
+       size_t &_endPos);
+
+  /// \brief Parse contents of an sdf element
+  /// \param[in] _key SDF element key
+  /// \param[in] _str String content of the sdf element
+  /// \return Value for the input key
+  public: static std::string Parse(const std::string &_key,
+      const std::string &_str);
+
+  /// \brief Fill VertexData from string
+  /// \param[in] _includeStr input <include> string
+  /// \param[out] _vd Vertex data to be filled
+  /// \return True if vertex data is successfully filled, false otherwise
+  public: static bool FillVertexData(const std::string &_includeStr,
+    VertexData &_vd,
+    std::function<bool(const std::string &, const std::string &)> &_filter);
+};
+}
diff --git a/subt_ign/src/dot_generator.cc b/subt_ign/src/dot_generator.cc
@@ -17,43 +17,10 @@
 
 #include <ignition/common/Console.hh>
 #include "ConnectionHelper.hh"
+#include "SdfParser.hh"
 
-using namespace ignition;
 using namespace subt;
-
-/// \brief Parse contents of an sdf element
-/// \param[in] _key SDF element key
-/// \param[in] _str String content of the sdf element
-/// \param[out] _endPos end position of the sdf element string
-/// \return Value for the input key
-std::string parse(const std::string &_key, const std::string &_str,
-     size_t &_endPos)
-{
-  std::string elemStartStr = "<" + _key + ">";
-  std::string elemEndStr = "</" + _key + ">";
-
-  size_t start = _str.find(elemStartStr);
-  if (start == std::string::npos)
-    return std::string();
-  size_t startIdx = start + elemStartStr.size();
-  size_t end = _str.find(elemEndStr, startIdx);
-  if (end == std::string::npos)
-    return std::string();
-
-  _endPos = end + elemEndStr.size();
-  std::string result = _str.substr(startIdx, end - startIdx);
-  return result;
-}
-
-/// \brief Parse contents of an sdf element
-/// \param[in] _key SDF element key
-/// \param[in] _str String content of the sdf element
-/// \return Value for the input key
-std::string parse(const std::string &_key, const std::string &_str)
-{
-  size_t endPos;
-  return parse(_key, _str, endPos);
-}
+using namespace ignition;
 
 /// \brief Print the DOT file
 /// \param[in] _vertexData vector of vertex data containing
@@ -140,57 +107,7 @@ void printGraph(std::vector<VertexData> &_vertexData)
   std::cout << out.str() << std::endl;
 }
 
-/// \brief Fill VertexData from string
-/// \param[in] _includeStr input <include> string
-/// \param[out] _vd Vertex data to be filled
-/// \return True if vertex data is successfully filled, false otherwise
-bool fillVertexData(const std::string &_includeStr, VertexData &_vd)
-{
-  // parse name
-  std::string name = parse("name", _includeStr);
-
-  // parse pose
-  std::string poseStr = parse("pose", _includeStr);
-  math::Pose3d pose;
-  std::stringstream ss(poseStr);
-  ss >> pose;
-
-  // parse uri and get model type
-  std::string uri = parse("uri", _includeStr);
-  std::string fuelStr =
-      "https://fuel.ignitionrobotics.org/1.0/openrobotics/models/";
-  size_t fuelIdx = uri.find(fuelStr);
-  std::string modelType;
-  if (fuelIdx == std::string::npos)
-    return false;
-  modelType = uri.substr(fuelIdx + fuelStr.size());
-
-  // check if model type is recognized
-  if (subt::ConnectionHelper::connectionPoints.count(modelType) <= 0)
-    return false;
-  sdf::Model modelSdf;
-  modelSdf.SetName(name);
-  modelSdf.SetPose(pose);
-
-  static int tileId = 0;
-  // Try getting the tile id from the tile name first.
-  try
-  {
-    int numIndex = name.rfind("_");
-    _vd.id = std::stoi(name.substr(numIndex+1));
-  }
-  catch (...)
-  {
-    _vd.id = tileId++;
-  }
-  _vd.tileType = modelType;
-  _vd.tileName = name;
-  _vd.model = modelSdf;
-
-  return true;
-}
-
-/// \brief Main function to generat DOT from input sdf file
+/// \brief Main function to generate DOT from input sdf file
 /// \param[in] _sdfFile Input sdf file.
 void generateDOT(const std::string &_sdfFile)
 {
@@ -203,16 +120,24 @@ void generateDOT(const std::string &_sdfFile)
   std::string str((std::istreambuf_iterator<char>(file)),
       std::istreambuf_iterator<char>());
 
+  // filter tiles that do not have connections
+  std::function<bool(const std::string &, const std::string &)>
+      filter = [](const std::string &/*_name*/,
+      const std::string &_type)
+  {
+    return subt::ConnectionHelper::connectionPoints.count(_type) <= 0;
+  };
+
   std::vector<VertexData> vertexData;
   while (!str.empty())
   {
     size_t result = std::string::npos;
-    std::string includeStr = parse("include", str, result);
+    std::string includeStr = SdfParser::Parse("include", str, result);
     if (result == std::string::npos || result > str.size())
       break;
 
     VertexData vd;
-    bool filled = fillVertexData(includeStr, vd);
+    bool filled = SdfParser::FillVertexData(includeStr, vd, filter);
     if (filled)
       vertexData.push_back(vd);