Merge pull request #93 from pbelanger-avid/add-yaml-include-support

yaml_preprocessor: add support for $include and $[include]

docs/core_functions/yaml_preprocessor.rst

@@ -9,9 +9,60 @@ Yaml Preprocessor
 Flatland Server has a lua preprocessor for YAML with simple bindings for the environment variables and rosparam.
 The intent is to be able to build parametric models that are defined by dimensions and flags found in either environment variables or rosparam, in a similar way to xacro+urdf. Because this is parsed at model load time, any roslaunch rosparam loading will have completed and those parameters will be available.
 
+Including additional YAML files
+-------------------------------
+The YAML preprocessor allows for including of other YAML files using the `$include` keyword. When this string is encountered, the following filename is parsed as a YAML file and substituted for the `$include`-string. The file path bay be absolute or relative. Relative file paths are relative to the YAML file currently being processed.
+
+.. code-block:: yaml
+
+  # project/param/parent.yaml
+  foo: 123
+  bar: $include child.yaml # Looks in current directory (project/param/) for relative filenames
+  baz: $include /absolute/path/to/project/param/child.yaml # or an absolute path can be specified
+
+  # project/param/child.yaml
+  a: 1
+  b: 2
+
+  # result of YAML preprocessing parent.yaml:
+  foo: 123
+  bar:
+    a: 1
+    b: 2
+  baz:
+    a: 1
+    b: 2
+
+There is also a special form of `$include`, `$[include]`,  that can be used to populate a sequence with an arbitrary number of items. Each individual document in the specified YAML file (separated by `---` as is standard for multi-document YAML files) becomes its own element in the sequence. For example:
+
+.. code-block:: yaml
+
+  # parent.yaml
+  foo:
+    - first
+    - $[include] child.yaml
+    - last
+
+  #child.yaml
+  one
+  ---
+  a: foo
+  b: baz
+  ---
+  three
+
+  # result of YAML preprocessing of parent.yaml:
+  foo:
+    - first
+    - one
+    - a: foo
+      b: baz
+    - three
+    - last
+
 body/joint/plugin enabled flag
 ------------------------------
-Model bodies, plugins and joints now have a new flag `enabled` which can be set to true or false either directly in the yaml, or based on more complex logic from a lua `$eval` string that returns "true" or "false". Disabled bodies, plugins and joints are skipped during yaml loading, and as a result are never instantiated. From Flatland's perspective `enabled: false` causes the affected body/plugin/joint to be deleted. 
+Model bodies, plugins and joints now have a new flag `enabled` which can be set to true or false either directly in the yaml, or based on more complex logic from a lua `$eval` string that returns "true" or "false". Disabled bodies, plugins and joints are skipped during yaml loading, and as a result are never instantiated. From Flatland's perspective `enabled: false` causes the affected body/plugin/joint to be deleted.
 
 bindings for env and param
 -------------------------------

flatland_server/CMakeLists.txt

@@ -42,7 +42,7 @@ find_package(Lua 5.1 QUIET)
 find_package(OpenCV REQUIRED)
 
 # Boost
-find_package(Boost REQUIRED COMPONENTS date_time system thread)
+find_package(Boost REQUIRED COMPONENTS date_time system thread filesystem)
 find_package(Threads)
 
 ##############

flatland_server/include/flatland_server/yaml_preprocessor.h

@@ -65,9 +65,11 @@ namespace YamlPreprocessor {
 /**
  * @brief Preprocess with a given node
  * @param[in/out] node A Yaml node to parse
+ * @param[in] ref_path The path the file was loaded from; used to locate
+ * include files with relative filenames.
  * @return The parsed YAML::Node
  */
-void Parse(YAML::Node &node);
+void Parse(YAML::Node &node, const std::string &ref_path);
 
 /**
  * @brief Constructor with a given path to a yaml file, throws exception on
@@ -80,25 +82,63 @@ YAML::Node LoadParse(const std::string &path);
 /**
  * @brief Find and run any $eval nodes
  * @param[in/out] node A Yaml node to recursively parse
+ * @param[in] ref_path The path the file was loaded from; used to locate
+ * include files with relative filenames.
  */
-void ProcessNodes(YAML::Node &node);
+void ProcessNodes(YAML::Node &node, const std::string &ref_path);
 
 /**
  * @brief Find and run any $eval expressions
  * @param[in/out] node A Yaml string node to parse
  */
-void ProcessScalarNode(YAML::Node &node);
+void ProcessEvalNode(YAML::Node &node);
 
 /**
-  * @brief Get an environment variable with an optional default value
-  * @param[in/out] lua_State The lua state/stack to read/write to/from
-  */
+ * @brief Resolve a given filename to an absolute path, resolving relative
+ * filenames relative to ref_path
+ * @param[in] filename The filename to find.
+ * @param[in] ref_path Filename of the YAML file containing the `$include`, used
+ * to locate relative filenames
+ * @return The absolute path of the file to be included.
+ */
+std::string ResolveIncludeFilePath(const std::string &filename,
+                                   const std::string &ref_path);
+
+/**
+ * @brief Potentially process an $include expression.
+ * @param[in/out] node A Yaml string node to parse. If an include is processed,
+ * the node is replaced with the contents of the specified file.
+ * @param[in] ref_path The path the file was loaded from; used to locate
+ * include files with relative filenames.
+ */
+void ProcessIncludeNode(YAML::Node &node, const std::string &ref_path);
+
+/**
+ * @brief Process a node, converting sequence include expression
+ * ('$[include]')to a series of nodes.
+ * @param[in/out] out_elems Vector that will be populated with parsed nodes
+ * from the included file.
+ * @param[in] node Node to parse. Should be a scalar node.
+ * @param ref_path Reference path used for locating relative filenames.
+ * @return If the node is an include expression, returns True. Otherwise,
+ * False.
+ *
+ * If the function returns true, then `node` should be replaced by out_elems in
+ * its sequence.
+ */
+bool ProcessSequenceIncludeNode(std::vector<YAML::Node> &out_elems,
+                                YAML::Node &node, const std::string &ref_path);
+
+/**
+ * @brief Get an environment variable with an optional default value
+ * @param[in/out] lua_State The lua state/stack to read/write to/from
+ */
 int LuaGetEnv(lua_State *L);
 
 /**
-  * @brief Get a rosparam with an optional default value
-  * @param[in/out] lua_State The lua state/stack to read/write to/from
-  */
+ * @brief Get a rosparam with an optional default value
+ * @param[in/out] lua_State The lua state/stack to read/write to/from
+ */
 int LuaGetParam(lua_State *L);
 };
 }

flatland_server/src/yaml_preprocessor.cpp

@@ -45,46 +45,84 @@
  */
 
 #include "flatland_server/yaml_preprocessor.h"
+
 #include <ros/ros.h>
+#include <yaml-cpp/exceptions.h>
+#include <yaml-cpp/node/parse.h>
+#include <yaml-cpp/node/type.h>
+#include <yaml-cpp/null.h>
+
 #include <boost/algorithm/string/trim.hpp>
+#include <boost/filesystem/path.hpp>
 #include <boost/lexical_cast.hpp>
-
 #include <cstdlib>
 #include <cstring>
 
+#include "flatland_server/exceptions.h"
+
 namespace flatland_server {
 
-void YamlPreprocessor::Parse(YAML::Node &node) {
-  YamlPreprocessor::ProcessNodes(node);
+static const char *kEvalMarker = "$eval";
+static const char *kIncludeMarker = "$include";
+static const char *kSequenceIncludeMarker = "$[include]";
+
+void YamlPreprocessor::Parse(YAML::Node &node, const std::string &ref_path) {
+  YamlPreprocessor::ProcessNodes(node, ref_path);
 }
 
-void YamlPreprocessor::ProcessNodes(YAML::Node &node) {
+void YamlPreprocessor::ProcessNodes(YAML::Node &node,
+                                    const std::string &ref_path) {
   switch (node.Type()) {
-    case YAML::NodeType::Sequence:
+    case YAML::NodeType::Sequence: {
+      // copy the elements to a new sequence, checking for $[include] expressions
+      // as we go. If an include is found, replace it with one or more nodes
+      // parsed from the included file.
+      YAML::Node new_sequence;
       for (YAML::Node child : node) {
-        YamlPreprocessor::ProcessNodes(child);
+        std::vector<YAML::Node> included_nodes = {};
+        if (ProcessSequenceIncludeNode(included_nodes, child, ref_path)) {
+          ROS_INFO_STREAM("Sequence include yielded " << included_nodes.size()
+                                                      << " nodes");
+          // node was an $include
+          for (auto &include_child : included_nodes) {
+            // ProcessSequenceIncludeNode handles processing of children itself.
+            new_sequence.push_back(include_child);
+          }
+        } else {
+          // not an include, just process and copy over normally.
+          // the sequence itself is the parent.
+          YamlPreprocessor::ProcessNodes(child, ref_path);
+          new_sequence.push_back(child);
+        }
       }
+      node = new_sequence;
       break;
+    }
     case YAML::NodeType::Map:
       for (YAML::iterator it = node.begin(); it != node.end(); ++it) {
-        YamlPreprocessor::ProcessNodes(it->second);
+        YamlPreprocessor::ProcessNodes(it->second, ref_path);
       }
       break;
-    case YAML::NodeType::Scalar:
-      if (node.as<std::string>().compare(0, 5, "$eval") == 0) {
-        ProcessScalarNode(node);
+    case YAML::NodeType::Scalar: {
+      auto s = node.as<std::string>();
+      if (s.compare(0, strlen(kEvalMarker), kEvalMarker) == 0) {
+        ProcessEvalNode(node);
+      } else if (s.compare(0, strlen(kIncludeMarker), kIncludeMarker) == 0) {
+        ProcessIncludeNode(node, ref_path);
       }
       break;
+    }
     default:
       ROS_DEBUG_STREAM(
           "Yaml Preprocessor found an unexpected type: " << node.Type());
       break;
   }
 }
 
-void YamlPreprocessor::ProcessScalarNode(YAML::Node &node) {
-  std::string value = node.as<std::string>().substr(5);  // omit the $parse
-  boost::algorithm::trim(value);                         // trim whitespace
+void YamlPreprocessor::ProcessEvalNode(YAML::Node &node) {
+  std::string value =
+      node.as<std::string>().substr(strlen(kEvalMarker));  // omit the $parse
+  boost::algorithm::trim(value);                           // trim whitespace
   ROS_INFO_STREAM("Attempting to parse lua " << value);
 
   if (value.find("return ") == std::string::npos) {  // Has no return statement
@@ -127,6 +165,110 @@ void YamlPreprocessor::ProcessScalarNode(YAML::Node &node) {
     ROS_ERROR_STREAM("Lua error in: " << value);
   }
 }
+std::string YamlPreprocessor::ResolveIncludeFilePath(
+    const std::string &filename, const std::string &ref_path) {
+  namespace fs = boost::filesystem;
+  fs::path f(filename);
+  // already an absolute path, return as-is.
+  if (f.is_absolute()) {
+    ROS_DEBUG_STREAM("Path is already absolute.");
+    return filename;
+  }
+  // if we're not loading from a file, then we can't resolve relative paths.
+  // just pass along and hope the caller is requesting something in the CWD.
+  if (ref_path.empty()) {
+    ROS_WARN_STREAM(
+        "$include specified a relative path but no original filename "
+        "specified");
+    return filename;
+  }
+
+  fs::path rel(ref_path);
+  if (fs::is_regular_file(rel)) {
+    rel = rel.parent_path();
+  }
+
+  fs::path result = rel / f;
+  return result.string();
+}
+void YamlPreprocessor::ProcessIncludeNode(YAML::Node &node,
+                                          const std::string &ref_path) {
+  // omit the $include
+  std::string value = node.as<std::string>().substr(strlen(kIncludeMarker));
+  ROS_INFO_STREAM("Attempting to parse include: " << value);
+  boost::algorithm::trim(value);  // remove whitespace
+
+  // format the common file & include info for any thrown exceptions.
+  const auto format_error_info = [&]() {
+    return "path=" + ref_path + ", include=" + value;
+  };
+
+  try {
+    auto path = ResolveIncludeFilePath(value, ref_path);
+    node = YAML::LoadFile(path);
+    // recursively process the included file, too
+    ProcessNodes(node, path);
+    ROS_INFO_STREAM("Successfully loaded include file " + path);
+  } catch (const YAML::BadFile &) {
+    throw YAMLException("File specified in $include does not exist," +
+                        format_error_info());
+  } catch (const YAML::ParserException &e) {
+    throw YAMLException(
+        "Malformatted file specified as include, " + format_error_info(), e);
+  } catch (const YAML::Exception &e) {
+    throw YAMLException("Error loading include file, " + format_error_info(),
+                        e);
+  }
+}
+
+bool YamlPreprocessor::ProcessSequenceIncludeNode(
+    std::vector<YAML::Node> &out_elems, YAML::Node &node,
+    const std::string &ref_path) {
+  if (node.Type() != YAML::NodeType::Scalar) {
+    return false;
+  }
+  auto node_string = node.as<std::string>();
+  // check for the actual sequence include marker
+  if (node_string.compare(0, strlen(kSequenceIncludeMarker),
+                          kSequenceIncludeMarker) != 0) {
+    return false;
+  }
+  // omit the $include
+  std::string value = node_string.substr(strlen(kSequenceIncludeMarker));
+  boost::algorithm::trim(value);
+  out_elems.clear();
+
+  ROS_INFO_STREAM("Attempting to parse sequence include: " << value);
+
+  // format the common file & include info for any thrown exceptions.
+  const auto format_error_info = [&]() {
+    return "path=" + ref_path + ", include=" + value;
+  };
+
+  try {
+    auto path = ResolveIncludeFilePath(value, ref_path);
+    out_elems = YAML::LoadAllFromFile(path);
+
+    // recursively process the included nodes, too
+    for (auto &included_node : out_elems) {
+      ProcessNodes(included_node, path);
+    }
+    ROS_INFO_STREAM("Successfully loaded sequence include file " + path);
+
+  } catch (const YAML::BadFile &) {
+    throw YAMLException("File specified in $[include] does not exist," +
+                        format_error_info());
+  } catch (const YAML::ParserException &e) {
+    throw YAMLException(
+        "Malformatted file specified as include, " + format_error_info(), e);
+  } catch (const YAML::Exception &e) {
+    throw YAMLException("Error loading include file, " + format_error_info(),
+                        e);
+  }
+
+  // parsed include successfully
+  return true;
+}
 
 YAML::Node YamlPreprocessor::LoadParse(const std::string &path) {
   YAML::Node node;
@@ -141,7 +283,7 @@ YAML::Node YamlPreprocessor::LoadParse(const std::string &path) {
     throw YAMLException("Error loading file, path=" + path, e);
   }
 
-  YamlPreprocessor::Parse(node);
+  YamlPreprocessor::Parse(node, path);
   return node;
 }
 
@@ -214,4 +356,4 @@ int YamlPreprocessor::LuaGetParam(lua_State *L) {
 
   return 1;  // 1 return value
 }
-}
+}  // namespace flatland_server