added YAML-based parameter loading and saving. Added yaml reader and …

…emitter example

CMakeLists.txt

@@ -16,6 +16,7 @@ set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/Modules)
 find_package(Eigen3 3.2 REQUIRED)
 #message("eigen version:" ${EIGEN3_VERSION})
 find_package(Boost 1.58 REQUIRED COMPONENTS system)
+find_package(YamlCpp 0.5.2 REQUIRED) # Using a custom FindYamlcpp.cmake 
 
 ###################################
 ## catkin specific configuration ##
@@ -57,6 +58,8 @@ SET (HELPER_SOURCES
 	${PROJECT_SOURCE_DIR}/src/avatar_locomanipulation/helpers/hermite_quaternion_curve.cpp
 	${PROJECT_SOURCE_DIR}/src/avatar_locomanipulation/helpers/rkf45.cpp
 	${PROJECT_SOURCE_DIR}/src/avatar_locomanipulation/helpers/pseudo_inverse.cpp
+	${PROJECT_SOURCE_DIR}/src/avatar_locomanipulation/helpers/param_handler.cpp
+	${PROJECT_SOURCE_DIR}/src/avatar_locomanipulation/helpers/yaml_data_saver.cpp
 )
 
 SET (ROS_BRIDGE_SOURCES
@@ -73,4 +76,9 @@ SET(PROJECT_SOURCES
 	${ROS_BRIDGE_SOURCES}
 	${FEASIBILITY_SOURCES})
 
+# Grab all the libraries
+SET(PROJECT_LIBRARIES
+	${catkin_LIBRARIES}
+	${YAML_CPP_LIBRARIES})
+
 add_subdirectory(test)

cmake/Modules/FindYamlCpp.cmake

@@ -0,0 +1,87 @@
+# Shamelessly stolen from: 
+# https://gitlab.com/nasa-jsc-robotics/nasa_common_cmake/blob/137926bbdb5fe3a7ccb77d187c33e97c61b8602c/cmake/Modules/FindYamlCpp.cmake
+
+
+# The file was modified to
+#  1. Define cmake environment variable names to match with how yaml-cpp 
+#     usually defines the include and library variables if it was built locally.
+#  2. Perform a package version check
+#  3. Explicitly find a shared library (libyaml.so)
+
+# - Try to find YamlCpp
+# Once done this will define
+#  YamlCpp_FOUND - System has YamlCpp
+#  YamlCpp_INCLUDE_DIRS - The YamlCpp include directories
+#  YamlCpp_LIBRARIES - The libraries needed to use YamlCpp
+
+#  as well renaming it to what the yaml-cpp package usually defines:
+
+#  YAML_CPP_INCLUDE_DIR - The YamlCpp include directories 
+#  YAML_CPP_LIBRARIES - The libraries needed to use YamlCpp
+
+
+find_package(PkgConfig)
+pkg_check_modules(PC_YAMLCPP yaml-cpp)
+# message(STATUS "PKG_CONFIG_FOUND: ${PKG_CONFIG_FOUND}")
+# message(STATUS "PKG_CONFIG_EXECUTABLE: ${PKG_CONFIG_EXECUTABLE}")
+# message(STATUS "PKG_CONFIG_VERSION_STRING: ${PKG_CONFIG_VERSION_STRING}")
+# message(STATUS "PC_YAMLCPP_FOUND: ${PC_YAMLCPP_FOUND}")
+# message(STATUS "PC_YamlCpp_INCLUDE_DIRS: ${PC_YamlCpp_INCLUDE_DIRS}")
+# message(STATUS "PC_YAMLCPP_LIBRARY_DIRS: ${PC_YAMLCPP_LIBRARY_DIRS}")
+
+# YamlCpp_FIND_VERSION is the argument from the call find_package(YamlCpp version_numer)
+# PC_YAMLCPP_VERSION is the version of the module if it was found using pkg_check_modules
+# 	see: https://cmake.org/cmake/help/latest/module/FindPkgConfig.html
+
+# If the version argument was not given, this sets it to version 0.0.0 as a default
+if(NOT YamlCpp_FIND_VERSION)
+  if(NOT YamlCpp_FIND_VERSION_MAJOR)
+    set(YamlCpp_FIND_VERSION_MAJOR 0)
+  endif(NOT YamlCpp_FIND_VERSION_MAJOR)
+  if(NOT YamlCpp_FIND_VERSION_MINOR)
+    set(YamlCpp_FIND_VERSION_MINOR 0)
+  endif(NOT YamlCpp_FIND_VERSION_MINOR)
+  if(NOT YamlCpp_FIND_VERSION_PATCH)
+    set(YamlCpp_FIND_VERSION_PATCH 0)
+  endif(NOT YamlCpp_FIND_VERSION_PATCH)
+
+  set(YamlCpp_FIND_VERSION "${YamlCpp_FIND_VERSION_MAJOR}.${YamlCpp_FIND_VERSION_MINOR}.${YamlCpp_FIND_VERSION_PATCH}")
+endif(NOT YamlCpp_FIND_VERSION)
+
+if ( ${PC_YAMLCPP_VERSION} VERSION_LESS ${YamlCpp_FIND_VERSION} )
+    message(SEND_ERROR "yaml-cpp version ${PC_YAMLCPP_VERSION} found in ${PC_YAMLCPP_LIBRARY_DIRS}, "
+                   "but at least version ${YamlCpp_FIND_VERSION} is required")
+	set(PC_YAMLCPP_FOUND FALSE)
+
+else ( ${PC_YAMLCPP_VERSION} VERSION_LESS ${YamlCpp_FIND_VERSION} )
+
+	find_path(YAMLCPP_INCLUDE_DIR yaml-cpp/yaml.h
+	          HINTS ${PC_YamlCpp_INCLUDE_DIRS}
+	          PATH_SUFFIXES include)
+
+	find_library(YAMLCPP_LIBRARY NAMES libyaml-cpp.so
+	             HINTS ${PC_YAMLCPP_LIBRARY_DIRS})
+
+	set(YamlCpp_LIBRARIES ${YAMLCPP_LIBRARY})
+	set(YamlCpp_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIR})
+	# message(STATUS "YAMLCPP_LIBRARY: ${YAMLCPP_LIBRARY}")
+	# message(STATUS "YAMLCPP_INCLUDE_DIR: ${YAMLCPP_INCLUDE_DIR}")
+
+	include(FindPackageHandleStandardArgs)
+	# handle the QUIETLY and REQUIRED arguments and set YAMLCPP_FOUND to TRUE
+	# if all listed variables are TRUE
+	find_package_handle_standard_args(YamlCpp DEFAULT_MSG
+	                                  YamlCpp_LIBRARIES YamlCpp_INCLUDE_DIRS)
+
+	mark_as_advanced(YamlCpp_INCLUDE_DIRS YamlCpp_LIBRARIES)
+
+	# ensure that they are cached
+	SET(YamlCpp_INCLUDE_DIRS ${YamlCpp_INCLUDE_DIRS} CACHE INTERNAL "The yaml-cpp include path")
+	SET(YamlCpp_LIBRARIES ${YamlCpp_LIBRARIES} CACHE INTERNAL "The libraries needed to use yaml-cpp library")
+
+	set(YAML_CPP_INCLUDE_DIR ${YamlCpp_INCLUDE_DIRS})
+	set(YAML_CPP_LIBRARIES ${YamlCpp_LIBRARIES})
+
+
+endif( ${PC_YAMLCPP_VERSION} VERSION_LESS ${YamlCpp_FIND_VERSION} )
+

include/avatar_locomanipulation/helpers/param_handler.hpp

@@ -0,0 +1,25 @@
+#ifndef ALM_PARAMETER_HANDLER_H
+#define ALM_PARAMETER_HANDLER_H
+
+#include <yaml-cpp/yaml.h>
+#include <string>
+#include <vector>
+
+class ParamHandler{
+public:
+  ParamHandler();	
+  ParamHandler(const std::string & file_name);
+  virtual ~ParamHandler();
+
+  void load_yaml_file(const std::string & file_name);
+
+  bool getString(const std::string & key, std::string & str_value);
+  bool getVector(const std::string & key, std::vector<double> & vec_value);
+  bool getValue(const std::string & key, double & double_value);
+  bool getNestedValue(const std::vector<std::string> & nested_key, double & double_value);
+  bool getBoolean(const std::string & key, bool & bool_value);
+  bool getInteger(const std::string & key, int & int_value);
+
+  YAML::Node config;
+};
+#endif

include/avatar_locomanipulation/helpers/yaml_data_saver.hpp

@@ -0,0 +1,13 @@
+#ifndef ALM_YAML_DATA_SAVER
+#define ALM_YAML_DATA_SAVER
+
+#include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
+
+namespace data_saver{
+  void emit_position(YAML::Emitter & out, const std::string & key, const Eigen::Vector3d & pos);  
+  void emit_orientation(YAML::Emitter & out, const std::string & key, const Eigen::Quaterniond & quat);
+  void emit_joint_configuration(YAML::Emitter & out, const Eigen::VectorXd & q_in);
+}
+
+#endif

src/avatar_locomanipulation/helpers/param_handler.cpp

@@ -0,0 +1,59 @@
+#include <avatar_locomanipulation/helpers/param_handler.hpp>
+#include <iostream>
+
+ParamHandler::ParamHandler(){
+}
+
+ParamHandler::ParamHandler(const std::string & file_name){
+  load_yaml_file(file_name);
+}
+
+void ParamHandler::load_yaml_file(const std::string & file_name){
+	config = YAML::LoadFile(file_name);
+}
+
+ParamHandler::~ParamHandler(){}
+
+bool ParamHandler::getString(const std::string & key, std::string& str_value) {
+  str_value = config[key].as<std::string>();
+  return true;
+}
+
+bool ParamHandler::getVector(const std::string & key,
+                             std::vector<double> & vec_value) {
+  vec_value = config[key].as<std::vector<double> >();
+  return true;
+}
+
+bool ParamHandler::getValue(const std::string & key, double & double_value) {
+  double_value = config[key].as<double>();
+  return true;
+}
+
+bool ParamHandler::getNestedValue(const std::vector<std::string> & nested_key, double & double_value){
+	// Create a recursive implementation of this.
+	YAML::Node current_node = Clone(config);
+	for(int i = 0; i < nested_key.size(); i++){
+		if (current_node[nested_key[i]]){
+			current_node = current_node[nested_key[i]];			
+		}
+		else{
+			//std::cerr << "Warning. The key " << nested_key[i] << " doesn't exist" << std::endl;
+			return false;
+		}
+	}
+	double_value = current_node.as<double>();
+	return true;
+}
+
+
+bool ParamHandler::getBoolean(const std::string & key, bool & bool_value) {
+  bool_value = config[key].as<bool>();
+  return true;
+}
+
+bool ParamHandler::getInteger(const std::string & key, int & int_value) {
+  int_value = config[key].as<int>();
+  return true;
+}
+

src/avatar_locomanipulation/helpers/yaml_data_saver.cpp

@@ -0,0 +1,38 @@
+#include <avatar_locomanipulation/helpers/yaml_data_saver.hpp>
+
+namespace data_saver{
+
+  void emit_position(YAML::Emitter & out, const std::string & key, const Eigen::Vector3d & pos){
+    out << YAML::Key << key;
+    out << YAML::Value;
+      out << YAML::BeginMap;
+      out << YAML::Key << "x" << YAML::Value << pos[0];
+      out << YAML::Key << "y" << YAML::Value << pos[1];
+      out << YAML::Key << "z" << YAML::Value << pos[2];    
+      out << YAML::EndMap;  
+  }
+
+  void emit_orientation(YAML::Emitter & out, const std::string & key, const Eigen::Quaterniond & quat){
+    out << YAML::Key << key;
+    out << YAML::Value;
+      out << YAML::BeginMap;
+      out << YAML::Key << "x" << YAML::Value << quat.x();
+      out << YAML::Key << "y" << YAML::Value << quat.y();
+      out << YAML::Key << "z" << YAML::Value << quat.z();    
+      out << YAML::Key << "w" << YAML::Value << quat.w();    
+      out << YAML::EndMap;    
+  }
+
+
+  void emit_joint_configuration(YAML::Emitter & out, const Eigen::VectorXd & q_in){
+    out << YAML::Key << "configuration";
+    out << YAML::Value;
+      out << YAML::Flow;    
+      out << YAML::BeginSeq;
+      for (size_t i = 0; i < q_in.size(); i++){
+        out << q_in[i] ;            
+      }
+      out << YAML::EndSeq;    
+  }
+
+}

test/CMakeLists.txt

@@ -6,13 +6,15 @@ add_executable(test_rkf45 test_rkf45.cpp ${PROJECT_SOURCES})
 add_executable(test_rviz_valkyrie test_rviz_valkyrie.cpp ${PROJECT_SOURCES})
 add_executable(test_viz_feasibility test_viz_feasibility.cpp ${PROJECT_SOURCES})
 add_executable(test_prioritized_ik test_prioritized_ik.cpp ${PROJECT_SOURCES})
+add_executable(test_yaml_emitter test_yaml_emitter.cpp ${PROJECT_SOURCES})
 
+target_link_libraries(test_load_urdf_model ${PROJECT_LIBRARIES})
+target_link_libraries(test_valkyrie_model  ${PROJECT_LIBRARIES})
+target_link_libraries(test_walking_pattern_generator ${PROJECT_LIBRARIES})
+target_link_libraries(test_mihir_working_file ${PROJECT_LIBRARIES})
+target_link_libraries(test_rkf45 ${PROJECT_LIBRARIES})
+target_link_libraries(test_rviz_valkyrie ${PROJECT_LIBRARIES})
+target_link_libraries(test_viz_feasibility ${PROJECT_LIBRARIES})
+target_link_libraries(test_prioritized_ik ${PROJECT_LIBRARIES})
+target_link_libraries(test_yaml_emitter ${PROJECT_LIBRARIES})
 
-target_link_libraries(test_load_urdf_model ${catkin_LIBRARIES})
-target_link_libraries(test_valkyrie_model  ${catkin_LIBRARIES})
-target_link_libraries(test_walking_pattern_generator ${catkin_LIBRARIES})
-target_link_libraries(test_mihir_working_file ${catkin_LIBRARIES})
-target_link_libraries(test_rkf45 ${catkin_LIBRARIES})
-target_link_libraries(test_rviz_valkyrie ${catkin_LIBRARIES})
-target_link_libraries(test_viz_feasibility ${catkin_LIBRARIES})
-target_link_libraries(test_prioritized_ik ${catkin_LIBRARIES})

test/test_yaml_emitter.cpp

@@ -0,0 +1,33 @@
+#include <avatar_locomanipulation/helpers/yaml_data_saver.hpp>
+#include <avatar_locomanipulation/helpers/param_handler.hpp>
+
+#include <iostream>
+#include <fstream>
+
+int main(int argc, char ** argv){
+  Eigen::Vector3d pelvis_pos(0,0,1);
+  Eigen::Quaterniond pelvis_ori(0.707,0,0,0.707);
+  Eigen::VectorXd q = Eigen::VectorXd::Zero(10); 
+
+  YAML::Emitter out;
+  out << YAML::BeginMap;
+    data_saver::emit_position(out, "pelvis_position", pelvis_pos);
+    data_saver::emit_orientation(out, "pelvis_orientation", pelvis_ori);
+    data_saver::emit_joint_configuration(out, q);
+  out << YAML::EndMap;
+
+  std::cout << "Here's the output YAML:\n" << out.c_str() << "\n" << std::endl;
+  std::ofstream file_output_stream("sample_emission.yaml");
+  file_output_stream << out.c_str();
+
+  // Example loading of the file
+  ParamHandler param_handler;
+  param_handler.load_yaml_file("sample_emission.yaml");
+
+  // Example of retreiving a nested value
+  double stored_val;
+  stored_val = param_handler.getNestedValue({"pelvis_position", "z"}, stored_val);
+  std::cout << "pelvis_position z = " << stored_val << std::endl;
+
+  return 0;
+}