Merge pull request avidbots#45 from avidbots/lua-preprocessor-for-yaml

Lua preprocessor for yaml

docs/core_functions/models.rst

@@ -17,6 +17,9 @@ shown here. Examples can be found in flatland_server/tests.
       # required, name of the body, must be unique within the model
     - name: base_link
 
+      # optional, default true. If false (see yaml preprocessor for details), this body is skipped
+      enabled: true
+
       # optional, defaults to [0, 0, 0], in the form of [x, y, theta] w.r.t the
       # model pose specified in world yaml. Pose is simply the initial pose
       # of the body, it does not add additional constrains in any way
@@ -115,6 +118,9 @@ shown here. Examples can be found in flatland_server/tests.
       # joint types
       name: rear_wheel_revolute
 
+      # optional, default true. If false (see yaml preprocessor for details), this joint is skipped
+      enabled: true
+
       # optional, default to false, specifies whether two bodies connected a
       # this joint should collide with each other, applies to all joint types
       collide_connected: false
@@ -186,6 +192,9 @@ shown here. Examples can be found in flatland_server/tests.
       # property to be discovered. See the Writing Model Plugins page
     - type: Laser
 
+      # optional, default true. If false (see yaml preprocessor for details), this plugin is skipped
+      enabled: true
+
       # required, name of the plugin to load, must be unique in a model
       name: kinect
 

docs/core_functions/yaml_preprocessor.rst

@@ -0,0 +1,81 @@
+.. image:: ../_static/flatland_logo2.png
+    :width: 250px
+    :align: right
+    :target: ../_static/flatland_logo2.png
+
+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.
+
+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. 
+
+bindings for env and param
+-------------------------------
+
+Additional lua function bindings (beyond the normal standard libraries such as string, math, etc.):
+
+.. code-block:: lua
+
+  env(EnvName)  -- blank string + warning if not found
+  env(EnvName, Default)
+  param(ParamPath)  -- blank string + warning if not found
+  param(ParamPath, Default)
+
+Sample expressions
+------------------------------
+
+.. code-block:: yaml
+
+  foo: $eval "Some arbitrary LUA expression"
+  bar: |  # Multiline string
+    $eval  -- $eval flag required to trigger LUA parsing
+    if env("FRONT_WHEEL",true) then
+      return 1.2
+    else
+      return 2.4
+    end
+
+Lua expressions can explicitly `return` their value, but if no `return` is given, one will be prepended to the statement.
+
+env + param examples
+-----------------------------
+
+.. code-block:: yaml
+
+  # in: (SOME_ENV not set)
+  foo: $eval env("SOME_ENV")
+  # out:
+  foo: ""
+
+.. code-block:: yaml
+
+  # in: (SOME_ENV not set)
+  foo: $eval env("SOME_ENV", false)
+  # out:
+  foo: false
+
+.. code-block:: yaml
+
+  # in: (export SOME_ENV=true)
+  foo: $eval env("SOME_ENV")
+  # out:
+  foo: true
+
+.. code-block:: yaml
+
+  # in: (rosparam /test/param not set)
+  foo: $eval param("/test/param", 0)/2.0
+  # out:
+  foo: 0
+
+.. code-block:: yaml
+
+  # in: (rosparam /test/param set to 5.0)
+  foo: $eval param("/test/param", 0)/2.0 + 1
+  # out:
+  foo: 2.5
+

docs/index.rst

@@ -36,6 +36,7 @@ Class APIs are documented `here <http://flatland-simulator-api.readthedocs.io/>`
    core_functions/world
    core_functions/layers
    core_functions/models
+   core_functions/yaml_preprocessor
    core_functions/ros_services
    core_functions/model_plugins
    core_functions/joystick

flatland_server/CMakeLists.txt

@@ -33,6 +33,9 @@ find_library(YAML_CPP_LIBRARY
              PATHS ${YAML_CPP_LIBRARY_DIRS})
 link_directories(${YAML_CPP_LIBRARY_DIRS})
 
+# lua5.1
+find_package(Lua 5.1 QUIET)
+
 # OpenCV
 find_package(OpenCV REQUIRED)
 
@@ -71,6 +74,7 @@ include_directories(
   ${YAML_CPP_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}
+  ${LUA_INCLUDE_DIR}
   thirdparty
 )
 
@@ -98,13 +102,15 @@ add_library(flatland_lib
   src/yaml_reader.cpp
   src/dummy_model_plugin.cpp 
   src/dummy_world_plugin.cpp
+  src/yaml_preprocessor.cpp
 )
 
 add_dependencies(flatland_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(flatland_lib
   ${catkin_LIBRARIES}
   ${OpenCV_LIBRARIES}
   ${Boost_LIBRARIES}
+  ${LUA_LIBRARIES}
   flatland_Box2D
   yaml-cpp
 )
@@ -123,6 +129,7 @@ target_link_libraries(flatland_server
   ${catkin_LIBRARIES}
   ${OpenCV_LIBRARIES}
   ${Boost_LIBRARIES}
+  ${LUA_LIBRARIES}
   flatland_Box2D
   yaml-cpp
   flatland_lib
@@ -226,5 +233,11 @@ if(CATKIN_ENABLE_TESTING)
   target_link_libraries(dummy_world_plugin_test
                         flatland_lib yaml-cpp) 
 
+  add_rostest_gtest(yaml_preprocessor_test
+                    test/yaml_preprocessor/yaml_preprocessor_test.test 
+                    test/yaml_preprocessor/yaml_preprocessor_test.cpp) 
+  target_link_libraries(yaml_preprocessor_test
+                        flatland_lib yaml-cpp)
+
 
 endif()

flatland_server/include/flatland_server/yaml_preprocessor.h

@@ -0,0 +1,106 @@
+/*
+ *  ______                   __  __              __
+ * /\  _  \           __    /\ \/\ \            /\ \__
+ * \ \ \L\ \  __  __ /\_\   \_\ \ \ \____    ___\ \ ,_\   ____
+ *  \ \  __ \/\ \/\ \\/\ \  /'_` \ \ '__`\  / __`\ \ \/  /',__\
+ *   \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\
+ *    \ \_\ \_\ \___/  \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/
+ *     \/_/\/_/\/__/    \/_/\/__,_ /\/___/  \/___/  \/__/\/___/
+ * @copyright Copyright 2018 Avidbots Corp.
+ * @name	 yaml_preprocessor.h
+ * @brief	 Yaml preprocessor using Lua
+ * @author Joseph Duchesne
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, Avidbots Corp.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *      copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Avidbots Corp. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef FLATLAND_SERVER_YAML_PREPROCESSOR_H
+#define FLATLAND_SERVER_YAML_PREPROCESSOR_H
+
+#include <flatland_server/exceptions.h>
+#include <yaml-cpp/yaml.h>
+
+#include <lauxlib.h>
+#include <lua.h>
+#include <lualib.h>
+#include <set>
+#include <string>
+#include <vector>
+
+namespace flatland_server {
+
+/**
+ */
+namespace YamlPreprocessor {
+/**
+ * @brief Preprocess with a given node
+ * @param[in/out] node A Yaml node to parse
+ * @return The parsed YAML::Node
+ */
+void Parse(YAML::Node &node);
+
+/**
+ * @brief Constructor with a given path to a yaml file, throws exception on
+ * failure
+ * @param[in] path Path to the yaml file
+ * @return The parsed YAML::Node
+ */
+YAML::Node LoadParse(const std::string &path);
+
+/**
+ * @brief Find and run any $eval nodes
+ * @param[in/out] node A Yaml node to recursively parse
+ */
+void ProcessNodes(YAML::Node &node);
+
+/**
+ * @brief Find and run any $eval expressions
+ * @param[in/out] node A Yaml string node to parse
+ */
+void ProcessScalarNode(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
+  */
+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
+  */
+int LuaGetParam(lua_State *L);
+};
+}
+
+#endif  // FLATLAND_SERVER_YAML_PREPROCESSOR_H

flatland_server/package.xml

@@ -26,6 +26,7 @@
   <depend>visualization_msgs</depend>
   <depend>interactive_markers</depend>
   <depend>flatland_msgs</depend>
+  <depend>lua-dev</depend>
 
   <export>
     <flatland_server plugin="${prefix}/flatland_plugins.xml" />

flatland_server/src/layer.cpp

@@ -301,7 +301,11 @@ void Layer::LoadFromBitmap(const cv::Mat &bitmap, double occupied_thresh,
 }
 
 void Layer::DebugVisualize() const {
-  ROS_WARN("======== 3d visualize? ===========");
+  // Don't try to visualized uninitalized layers
+  if (viz_name_.length() == 0) {
+    return;
+  }
+
   DebugVisualization::Get().Reset(viz_name_);
   DebugVisualization::Get().Reset(viz_name_ + "_3d");
 

flatland_server/src/model.cpp

@@ -106,6 +106,13 @@ void Model::LoadBodies(YamlReader &bodies_reader) {
   } else {
     for (int i = 0; i < bodies_reader.NodeSize(); i++) {
       YamlReader body_reader = bodies_reader.Subnode(i, YamlReader::MAP);
+      if (!body_reader.Get<bool>("enabled", "true")) {
+        ROS_INFO_STREAM("Body "
+                        << Q(name_) << "."
+                        << body_reader.Get<std::string>("name", "unnamed")
+                        << " disabled");
+        continue;
+      }
       ModelBody *b =
           ModelBody::MakeBody(physics_world_, cfr_, this, body_reader);
       bodies_.push_back(b);
@@ -125,6 +132,13 @@ void Model::LoadJoints(YamlReader &joints_reader) {
   if (!joints_reader.IsNodeNull()) {
     for (int i = 0; i < joints_reader.NodeSize(); i++) {
       YamlReader joint_reader = joints_reader.Subnode(i, YamlReader::MAP);
+      if (!joint_reader.Get<bool>("enabled", "true")) {
+        ROS_INFO_STREAM("Joint "
+                        << Q(name_) << "."
+                        << joint_reader.Get<std::string>("name", "unnamed")
+                        << " disabled");
+        continue;
+      }
       Joint *j = Joint::MakeJoint(physics_world_, this, joint_reader);
       joints_.push_back(j);
 

flatland_server/src/plugin_manager.cpp

@@ -116,6 +116,21 @@ void PluginManager::LoadModelPlugin(Model *model, YamlReader &plugin_reader) {
                         " already exists");
   }
 
+  try {
+    if (!plugin_reader.Get<bool>("enabled", "true")) {
+      ROS_WARN_STREAM("Plugin "
+                      << Q(model->name_) << "."
+                      << plugin_reader.Get<std::string>("name", "unnamed")
+                      << " disabled");
+      return;
+    }
+  } catch (...) {
+    ROS_WARN_STREAM("Body " << Q(model->name_) << "."
+                            << plugin_reader.Get<std::string>("name", "unnamed")
+                            << " enabled because flag failed to parse: "
+                            << plugin_reader.Get<std::string>("enabled"));
+  }
+
   // remove the name and type of the YAML Node, the plugin does not need to know
   // about these parameters, remove method is broken in yaml cpp 5.2, so we
   // create a new node and add everything