From a8060422cbdf1c85c7257df0d92e9685f735a2df Mon Sep 17 00:00:00 2001 From: gilwoolee Date: Sat, 22 Apr 2017 21:24:51 -0400 Subject: [PATCH] Yaml extension for Eigen::Matrix and std::unordered_map (#175) Resolves https://github.com/personalrobotics/aikido/pull/175 --- cmake/FindYamlCpp.cmake | 49 ++++ cmake/version.hpp.in | 8 +- .../uniform/detail/RnBoxConstraint-impl.hpp | 12 +- include/aikido/perception/AprilTagsModule.hpp | 2 +- .../perception/YamlAprilTagsDatabase.hpp | 2 +- include/aikido/perception/eigen_yaml.hpp | 183 ------------- include/aikido/statespace/detail/Rn-impl.hpp | 6 +- include/aikido/util/detail/yaml_extension.hpp | 251 ++++++++++++++++++ include/aikido/util/yaml.hpp | 15 ++ src/CMakeLists.txt | 4 +- src/perception/CMakeLists.txt | 6 +- src/util/CMakeLists.txt | 24 ++ .../planner/ompl/test_GeometricStateSpace.cpp | 1 - tests/util/CMakeLists.txt | 3 + tests/util/test_yaml_extension.cpp | 133 ++++++++++ 15 files changed, 493 insertions(+), 206 deletions(-) create mode 100644 cmake/FindYamlCpp.cmake delete mode 100644 include/aikido/perception/eigen_yaml.hpp create mode 100644 include/aikido/util/detail/yaml_extension.hpp create mode 100644 include/aikido/util/yaml.hpp create mode 100644 tests/util/test_yaml_extension.cpp diff --git a/cmake/FindYamlCpp.cmake b/cmake/FindYamlCpp.cmake new file mode 100644 index 0000000000..b5bfe4a887 --- /dev/null +++ b/cmake/FindYamlCpp.cmake @@ -0,0 +1,49 @@ +# Locate yaml-cpp +# +# This module defines +# YAMLCPP_FOUND - System has yaml-cpp +# YAMLCPP_INCLUDE_DIRS - The yaml-cpp include directories +# YAMLCPP_LIBRARIES - The libraries needed to use yaml-cpp +# YAMLCPP_DEFINITIONS - Compiler switches required for using yaml-cpp +# YAMLCPP_VERSION - The version of yaml-cpp +# +# By default, the dynamic libraries of yaml-cpp will be found. To find the +# static ones instead, set the YAMLCPP_STATIC_LIBRARY variable to TRUE before +# calling find_package(YamlCpp ...). +# +# If yaml-cpp is not installed in a standard path, you can use the YAMLCPP_DIR +# CMake variable to tell CMake where yaml-cpp is. + +# Attempt to find static library first if this is set +if(YAMLCPP_STATIC_LIBRARY) + set(YAMLCPP_STATIC libyaml-cpp.a) +endif() + +# Set up pkg-config to find yaml-cpp. +find_package(PkgConfig) +pkg_check_modules(PC_YAMLCPP QUIET yaml-cpp) +set(YAMLCPP_DEFINITIONS ${PC_YAMLCPP_CFLAGS_OTHER}) + +# Find the yaml-cpp include directory. +find_path(YAMLCPP_INCLUDE_DIR yaml-cpp/yaml.h + HINTS ${PC_YAMLCPP_INCLUDEDIR} ${PC_YAMLCPP_INCLUDE_DIRS} + PATHS ${YAMLCPP_DIR}/include/ + PATH_SUFFIXES include) + +# Find the yaml-cpp library. +find_library(YAMLCPP_LIBRARY NAMES ${YAMLCPP_STATIC} yaml-cpp + HINTS ${PC_YAMLCPP_LIBDIR} ${PC_YAMLCPP_LIBRARY_DIRS} + PATHS ${YAMLCPP_DIR}/lib/ + PATH_SUFFIXES lib64 lib) + +set(YAMLCPP_LIBRARIES ${YAMLCPP_LIBRARY}) +set(YAMLCPP_INCLUDE_DIRS ${YAMLCPP_INCLUDE_DIR}) + +set(YAMLCPP_VERSION ${PC_YAMLCPP_VERSION}) + +# Handle the QUIETLY and REQUIRED arguments and set YAMLCPP_FOUND to TRUE +# if all listed variables are TRUE. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(YAMLCPP DEFAULT_MSG + YAMLCPP_INCLUDE_DIR YAMLCPP_LIBRARY) +mark_as_advanced(YAMLCPP_INCLUDE_DIR YAMLCPP_LIBRARY) diff --git a/cmake/version.hpp.in b/cmake/version.hpp.in index 65363b6e6f..730677a101 100644 --- a/cmake/version.hpp.in +++ b/cmake/version.hpp.in @@ -1,6 +1,6 @@ -/* config.hpp. Generated by CMake for @PROJECT_NAME@. */ -#ifndef AIKIDO_CONFIG_HPP_ -#define AIKIDO_CONFIG_HPP_ +/* version.hpp. Generated by CMake for @PROJECT_NAME@. */ +#ifndef AIKIDO_VERSION_HPP_ +#define AIKIDO_VERSION_HPP_ /* Version number */ #define AIKIDO_MAJOR_VERSION @AIKIDO_MAJOR_VERSION@ @@ -27,4 +27,4 @@ (AIKIDO_MAJOR_VERSION < x || (AIKIDO_MAJOR_VERSION <= x && \ (AIKIDO_MINOR_VERSION < y || (AIKIDO_MINOR_VERSION <= y)))) -#endif // #ifndef AIKIDO_CONFIG_HPP_ +#endif // #ifndef AIKIDO_VERSION_HPP_ diff --git a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp index b224efcc1a..4f438b4173 100644 --- a/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp +++ b/include/aikido/constraint/uniform/detail/RnBoxConstraint-impl.hpp @@ -86,7 +86,7 @@ bool RnBoxConstraintSampleGenerator::sample( { Vectord value(mDistributions.size()); - for (size_t i = 0; i < value.size(); ++i) + for (size_t i = 0; i < static_cast(value.size()); ++i) value[i] = mDistributions[i](*mRng); mSpace->setValue(static_cast::State*>(_state), value); @@ -124,7 +124,7 @@ RBoxConstraint::RBoxConstraint(std::shared_ptr> _space, const auto dimension = mSpace->getDimension(); - if (mLowerLimits.size() != dimension) + if (static_cast(mLowerLimits.size()) != dimension) { std::stringstream msg; msg << "Lower limits have incorrect dimension: expected " @@ -132,7 +132,7 @@ RBoxConstraint::RBoxConstraint(std::shared_ptr> _space, throw std::invalid_argument(msg.str()); } - if (mUpperLimits.size() != dimension) + if (static_cast(mUpperLimits.size()) != dimension) { std::stringstream msg; msg << "Upper limits have incorrect dimension: expected " @@ -184,7 +184,7 @@ bool RBoxConstraint::isSatisfied( const auto value = mSpace->getValue( static_cast::State*>(state)); - for (size_t i = 0; i < value.size(); ++i) + for (size_t i = 0; i < static_cast(value.size()); ++i) { if (value[i] < mLowerLimits[i] || value[i] > mUpperLimits[i]) return false; @@ -201,7 +201,7 @@ bool RBoxConstraint::project( Vectord value = mSpace->getValue( static_cast::State*>(_s)); - for (size_t i = 0; i < value.size(); ++i) + for (size_t i = 0; i < static_cast(value.size()); ++i) { if (value[i] < mLowerLimits[i]) value[i] = mLowerLimits[i]; @@ -250,7 +250,7 @@ void RBoxConstraint::getJacobian( const size_t dimension = mSpace->getDimension(); _out = Eigen::MatrixXd::Zero(dimension, dimension); - for (size_t i = 0; i < _out.rows(); ++i) + for (size_t i = 0; i < static_cast(_out.rows()); ++i) { if (stateValue[i] < mLowerLimits[i]) _out(i, i) = -1.; diff --git a/include/aikido/perception/AprilTagsModule.hpp b/include/aikido/perception/AprilTagsModule.hpp index d7ce0f303a..f75f366e78 100644 --- a/include/aikido/perception/AprilTagsModule.hpp +++ b/include/aikido/perception/AprilTagsModule.hpp @@ -8,9 +8,9 @@ #include #include #include +#include #include #include -#include #include diff --git a/include/aikido/perception/YamlAprilTagsDatabase.hpp b/include/aikido/perception/YamlAprilTagsDatabase.hpp index 52b7a6d04d..c9276d10be 100644 --- a/include/aikido/perception/YamlAprilTagsDatabase.hpp +++ b/include/aikido/perception/YamlAprilTagsDatabase.hpp @@ -2,8 +2,8 @@ #define AIKIDO_PERCEPTION_YAML_APRILTAGS_DATABASE_H #include "yaml-cpp/yaml.h" -#include #include +#include #include #include #include diff --git a/include/aikido/perception/eigen_yaml.hpp b/include/aikido/perception/eigen_yaml.hpp deleted file mode 100644 index 3d5b818718..0000000000 --- a/include/aikido/perception/eigen_yaml.hpp +++ /dev/null @@ -1,183 +0,0 @@ -#ifndef AIKIDO_PERCEPTION_EIGEN_YAML_HPP -#define AIKIDO_PERCEPTION_EIGEN_YAML_HPP - -#include -#include -#include -#include -#include - - -template -inline void deserialize( - YAML::Node const &node, - Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> &matrix) -{ - typedef Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> MatrixType; - typedef typename MatrixType::Index Index; - - using boost::format; - using boost::str; - using std::runtime_error; - - if (node.Type() != YAML::NodeType::Sequence) { - throw runtime_error("Matrix or vector must be a sequence."); - } - - Index const rows = node.size(); - if (MatrixType::RowsAtCompileTime != Eigen::Dynamic - && rows != MatrixType::RowsAtCompileTime) { - throw runtime_error(str( - format("Matrix has incorrect number of rows: expected %d; got %d.") - % MatrixType::RowsAtCompileTime % rows)); - } - - if (node[0].Type() == YAML::NodeType::Scalar) { - matrix.resize(rows, 1); - - for (Index i = 0; i < rows; ++i) { - matrix(i, 0) = node[i].template as< _Scalar >(); - } - } else if (node[0].Type() == YAML::NodeType::Sequence) { - Index const cols = node[0].size(); - - if (MatrixType::ColsAtCompileTime != Eigen::Dynamic - && cols != MatrixType::ColsAtCompileTime) { - throw runtime_error(str( - format("Matrix has incorrect number of cols: expected %d; got %d.") - % MatrixType::ColsAtCompileTime % cols)); - } - - matrix.resize(rows, cols); - - for (Index r = 0; r < node.size(); ++r) { - if (node[r].Type() != YAML::NodeType::Sequence) { - throw runtime_error(str( - format("Row %d of the matrix must be a sequence.") % r)); - } else if (node[r].size() != cols) { - throw runtime_error(boost::str( - format("Expected row %d to have %d columns; got %d.") - % r % cols % node[r].size())); - } - - for (Index c = 0; c < cols; ++c) { - matrix(r, c) = node[r][c].template as<_Scalar>(); - } - } - } else { - throw runtime_error(str( - format("Unknown type of matrix "))); - } -} - -template -inline void deserialize(YAML::Node const &node, - Eigen::Transform<_Scalar, Dim, Mode, _Options> &pose) -{ - deserialize(node, pose.matrix()); -} - -template -inline bool has_child(YAML::Node const &node, T const &key) -{ - return node[key].IsDefined(); -} - -namespace YAML { - -namespace detail { - -template -struct encode_impl {}; - -template -struct encode_impl { - static Node encode(MatrixType const &matrix) - { - typedef typename MatrixType::Index Index; - - Node node; - node.SetTag("Vector"); - - for (Index i = 0; i < matrix.size(); ++i) { - node.push_back(Node(matrix[i])); - } - return node; - } -}; - -template -struct encode_impl { - static Node encode(MatrixType const &matrix) - { - typedef typename MatrixType::Index Index; - - Node node; - node.SetTag("Matrix"); - - for (Index r = 0; r < matrix.rows(); ++r) { - Node row(NodeType::Sequence); - - for (Index c = 0; c < matrix.cols(); ++c) { - row.push_back(matrix(r, c)); - } - - node.push_back(row); - } - return node; - } -}; - -} //namespace detail - -template -struct convert > { - typedef Eigen::Matrix<_Scalar, _Dim, _Mode, _Options> MatrixType; - - static Node encode(MatrixType const &matrix) - { - YAML::Node node(NodeType::Sequence); - return detail::encode_impl< - MatrixType, MatrixType::IsVectorAtCompileTime>::encode(matrix); - } - - static bool decode( - YAML::Node const &node, - Eigen::Matrix<_Scalar, _Dim, _Mode, _Options> &matrix) - { - if (node[0].Type() == YAML::NodeType::Scalar || node[0].Type() == YAML::NodeType::Sequence) { - deserialize(node, matrix); - return true; - } else { - return false; - } - } -}; - -template -struct convert > { - typedef Eigen::Transform<_Scalar, _Dim, _Mode, _Options> TransformType; - typedef typename TransformType::MatrixType MatrixType; - - static Node encode(TransformType const &transform) - { - return convert::encode(transform.matrix()); - } - - static bool decode(Node const &node, TransformType &transform) - { - return convert::decode(node, transform.matrix()); - } -}; - -template -inline void operator>>(Node const &node, T &value) -{ - value = node.as(); -} - - -} // namespace YAML - -#endif // ifndef YAML_UTILS_H_ diff --git a/include/aikido/statespace/detail/Rn-impl.hpp b/include/aikido/statespace/detail/Rn-impl.hpp index deb5b5d9ca..1e2948f3b0 100644 --- a/include/aikido/statespace/detail/Rn-impl.hpp +++ b/include/aikido/statespace/detail/Rn-impl.hpp @@ -147,7 +147,7 @@ template void R::setValue(State *_state, const typename R::Vectord &_value) const { // TODO: Skip this check in release mode. - if (_value.size() != getDimension()) { + if (static_cast(_value.size()) != getDimension()) { std::stringstream msg; msg << "Value has incorrect size: expected " << getDimension() << ", got " << _value.size() << "."; @@ -245,7 +245,7 @@ template void R::expMap(const Eigen::VectorXd&_tangent, StateSpace::State *_out) const { // TODO: Skip this check in release mode. - if (_tangent.size() != getDimension()) { + if (static_cast(_tangent.size()) != getDimension()) { std::stringstream msg; msg << "Tangent vector has incorrect size: expected " << getDimension() << ", got " << _tangent.size() << "."; @@ -260,7 +260,7 @@ void R::expMap(const Eigen::VectorXd&_tangent, StateSpace::State *_out) const template void R::logMap(const StateSpace::State *_in, Eigen::VectorXd& _tangent) const { - if (_tangent.size() != getDimension()) + if (static_cast(_tangent.size()) != getDimension()) _tangent.resize(getDimension()); auto in = static_cast(_in); diff --git a/include/aikido/util/detail/yaml_extension.hpp b/include/aikido/util/detail/yaml_extension.hpp new file mode 100644 index 0000000000..4830a7e62c --- /dev/null +++ b/include/aikido/util/detail/yaml_extension.hpp @@ -0,0 +1,251 @@ +#ifndef AIKIDO_UTIL_DETAIL_YAMLEIGENEXTENSION_HPP_ +#define AIKIDO_UTIL_DETAIL_YAMLEIGENEXTENSION_HPP_ + +#include +#include +#include +#include +#include + +namespace aikido { +namespace util { +namespace detail { + +//============================================================================== +template +struct encode_impl +{ + // Nothing defined. This class should be always specialized. + + // The reason this exists is so we can use template specialization to switch + // between serializing vectors and serializing matrices based on the + // IsVectorAtCompileTime flag. This is a "nice to have" that lets us generate + // !Vector [1, 2, 3] instead of the syntactic travesty like + // !Matrix [[1], [2] ,[3]] when serializing an Eigen vector. +}; + +//============================================================================== +// Specialization for vector type +template +struct encode_impl +{ + static YAML::Node encode(const MatrixType& matrix) + { + using Index = typename MatrixType::Index; + + YAML::Node node; + node.SetTag("!Vector"); + + for (Index i = 0; i < matrix.size(); ++i) + node.push_back(YAML::Node(matrix[i])); + + return node; + } +}; + +//============================================================================== +// Specialization for matrix type +template +struct encode_impl +{ + static YAML::Node encode(const MatrixType& matrix) + { + using Index = typename MatrixType::Index; + + YAML::Node node; + node.SetTag("!Matrix"); + + for (Index r = 0; r < matrix.rows(); ++r) + { + YAML::Node row(YAML::NodeType::Sequence); + + for (Index c = 0; c < matrix.cols(); ++c) + row.push_back(matrix(r, c)); + + node.push_back(row); + } + + return node; + } +}; + +inline YAML::Mark getMark(const YAML::Node& node) +{ +#ifdef YAMLCPP_NODE_HAS_MARK + return node.Mark(); +#else + DART_UNUSED(node); + return YAML::Mark::null_mark(); +#endif + // We need this because Node::Mark() was introduced since yaml-cpp 0.5.3. + // By using null_mark(), the error message wouldn't tell the exact location + // (i.e., line number and column in the yaml file) where the error occurred. +} + +} // namespace detail +} // namespace util +} // namespace aikido + +namespace YAML { + +//============================================================================== +// Specialization for Eigen::Matrix<...>. This enables to use YAML::Node +// with std::unordered_map as `Node(Eigen::Matrix3d::Identity())` and +// `node.as()`. +template +struct convert> +{ + using MatrixType + = Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>; + using Index = typename MatrixType::Index; + + static Node encode(const MatrixType& matrix) + { + using aikido::util::detail::encode_impl; + return encode_impl::encode( + matrix); + } + + /// Reads a YAML::Node that encodes a vector or a matrix; and stores it into + /// `matrix`. If `matrix` is dynamic size Eigen object, resizes it + /// accordingly. + static bool decode(const YAML::Node& node, MatrixType& matrix) + { + using aikido::util::detail::getMark; + + if (node.Type() != YAML::NodeType::Sequence) + { + throw YAML::RepresentationException( + getMark(node), "Matrix or vector must be a sequence."); + } + + const Index rows = node.size(); + if (MatrixType::RowsAtCompileTime != Eigen::Dynamic + && rows != MatrixType::RowsAtCompileTime) + { + std::stringstream ss; + ss << "Matrix has incorrect number of rows: expected " + << MatrixType::RowsAtCompileTime << "; got " << rows << "."; + throw YAML::RepresentationException(getMark(node), ss.str()); + } + + if (node.Tag() == "!Vector") + { + matrix.resize(rows, 1); + for (Index i = 0; i < rows; ++i) + matrix(i, 0) = node[i].template as<_Scalar>(); + } + else if (node.Tag() == "!Matrix") + { + const auto cols = node[0].size(); + + if (MatrixType::ColsAtCompileTime != Eigen::Dynamic + && cols != MatrixType::ColsAtCompileTime) + { + std::stringstream ss; + ss << "Matrix has incorrect number of cols: expected " + << MatrixType::ColsAtCompileTime << "; got " << cols << "."; + throw YAML::RepresentationException(getMark(node), ss.str()); + } + + matrix.resize(rows, cols); + + for (auto r = 0u; r < node.size(); ++r) + { + if (node[r].Type() != YAML::NodeType::Sequence) + { + std::stringstream ss; + ss << "Row " << r << " of the matrix must be a sequence."; + throw YAML::RepresentationException(getMark(node), ss.str()); + } + else if (node[r].size() != cols) + { + std::stringstream ss; + ss << "Expected row " << r << " to have " << cols << " columns; got " + << node[r].size() << "."; + throw YAML::RepresentationException(getMark(node), ss.str()); + } + + for (auto c = 0u; c < cols; ++c) + matrix(r, c) = node[r][c].template as<_Scalar>(); + } + } + else + { + std::stringstream ss; + ss << "Unknown type of matrix '" << node.Tag() << "'."; + throw YAML::RepresentationException(getMark(node), ss.str()); + } + + return true; + } +}; + +//============================================================================== +// Specialization for Eigen::Isometry<...>. This enables to use YAML::Node +// with std::unordered_map as `Node(Eigen::Isometry3d::Identity())` and +// `node.as()`. +template +struct convert> +{ + using TransformType = Eigen::Transform<_Scalar, _Dim, _Mode, _Options>; + using MatrixType = typename TransformType::MatrixType; + + static Node encode(const TransformType& transform) + { + return convert::encode(transform.matrix()); + } + + static bool decode(const Node& node, TransformType& transform) + { + return convert::decode(node, transform.matrix()); + } +}; + +//============================================================================== +// Specialization for std::unordered_map<...>. This enables to use YAML::Node +// with std::unordered_map as `Node(std::unorderd_map<...>())` and +// `node.as>()`. +template +struct convert> +{ + using UnorderedMap = std::unordered_map<_Key, _Tp, _Hash, _Pred, _Alloc>; + + static Node encode(const UnorderedMap& map) + { + Node node(NodeType::Map); + + for (const auto& it : map) + node.force_insert(it.first, it.second); + + return node; + } + + static bool decode(const Node& node, UnorderedMap& map) + { + if (!node.IsMap()) + return false; + + map.clear(); + map.reserve(node.size()); + + for (const auto& it : node) + map[it.first.as<_Key>()] = it.second.as<_Tp>(); + + return true; + } +}; + +} // namespace YAML + +#endif // AIKIDO_UTIL_YAMLUTILS_HPP_ diff --git a/include/aikido/util/yaml.hpp b/include/aikido/util/yaml.hpp new file mode 100644 index 0000000000..4b0b935fd8 --- /dev/null +++ b/include/aikido/util/yaml.hpp @@ -0,0 +1,15 @@ +#ifndef AIKIDO_UTIL_YAML_HPP_ +#define AIKIDO_UTIL_YAML_HPP_ + +#include + +#include "aikido/util/detail/yaml_extension.hpp" +// The above header allows extended value types for yaml-cpp. +// +// Supported types: +// - Eigen::Matrix<...> +// - Eigen::Isometry<...> +// - std::unordered_map +// + +#endif // AIKIDO_UTIL_YAML_HPP_ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 7f71f90aae..200921820a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -3,10 +3,10 @@ add_subdirectory("external/hauser_parabolic_smoother") -add_subdirectory("perception") # boost, dart, yaml-cpp, geometry_msgs, roscpp, std_msgs, visualization_msgs +add_subdirectory("util") # boost, dart, tinyxml2, yaml-cpp add_subdirectory("rviz") # boost, dart, roscpp, geometry_msgs, interactive_markers, std_msgs, visualization_msgs, libmicrohttpd add_subdirectory("statespace") # dart -add_subdirectory("util") # boost, dart, tinyxml2 +add_subdirectory("perception") # [util], boost, dart, yaml-cpp, geometry_msgs, roscpp, std_msgs, visualization_msgs add_subdirectory("distance") # [statespace], dart add_subdirectory("trajectory") # [statespace], [util] add_subdirectory("constraint") # [statespace], [util] diff --git a/src/perception/CMakeLists.txt b/src/perception/CMakeLists.txt index c89015349d..cf8cf73dd9 100644 --- a/src/perception/CMakeLists.txt +++ b/src/perception/CMakeLists.txt @@ -7,9 +7,6 @@ if(NOT DART_utils-urdf_FOUND) return() endif() -pkg_check_modules(YAMLCPP QUIET yaml-cpp) -aikido_check_package(YAMLCPP "perception" "yaml-cpp") - find_package(geometry_msgs QUIET) aikido_check_package(geometry_msgs "perception" "geometry_msgs") @@ -44,7 +41,6 @@ target_include_directories("${PROJECT_NAME}_perception" SYSTEM PUBLIC ${Boost_INCLUDE_DIRS} ${DART_INCLUDE_DIRS} - ${YAMLCPP_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} ${roscpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS} @@ -54,8 +50,8 @@ target_include_directories("${PROJECT_NAME}_perception" SYSTEM ) target_link_libraries("${PROJECT_NAME}_perception" PUBLIC + "${PROJECT_NAME}_util" ${DART_LIBRARIES} - ${YAMLCPP_LIBRARIES} ${geometry_msgs_LIBRARIES} ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES} diff --git a/src/util/CMakeLists.txt b/src/util/CMakeLists.txt index 6b907f3d5f..82b6f467d3 100644 --- a/src/util/CMakeLists.txt +++ b/src/util/CMakeLists.txt @@ -4,6 +4,24 @@ # TODO: Move CatkinResourceRetriever into a separate, optional component. find_package(TinyXML2 REQUIRED) +find_package(YamlCpp REQUIRED) + +include(CheckCXXSourceCompiles) +set(CMAKE_REQUIRED_DEFINITIONS "") +set(CMAKE_REQUIRED_FLAGS "") +set(CMAKE_REQUIRED_INCLUDES ${YAMLCPP_INCLUDE_DIRS}) +set(CMAKE_REQUIRED_LIBRARIES ${YAMLCPP_LIBRARIES}) +check_cxx_source_compiles( + " + #include + int main() + { + YAML::Node node; + node.Mark(); + } + " + YAMLCPP_NODE_HAS_MARK) + #============================================================================== # Libraries # @@ -25,6 +43,7 @@ target_include_directories("${PROJECT_NAME}_util" SYSTEM PUBLIC ${Boost_INCLUDE_DIRS} ${DART_INCLUDE_DIRS} + ${YAMLCPP_INCLUDE_DIRS} PRIVATE ${TinyXML2_INCLUDE_DIRS} ) @@ -32,12 +51,17 @@ target_link_libraries("${PROJECT_NAME}_util" PUBLIC ${Boost_FILESYSTEM_LIBRARY} ${DART_LIBRARIES} + ${YAMLCPP_LIBRARIES} PRIVATE ${TinyXML2_LIBRARIES} ) target_compile_options("${PROJECT_NAME}_util" PUBLIC ${AIKIDO_CXX_STANDARD_FLAGS} ) +if(YAMLCPP_NODE_HAS_MARK) + target_compile_definitions("${PROJECT_NAME}_util" + PUBLIC YAMLCPP_NODE_HAS_MARK) +endif() add_component(${PROJECT_NAME} util) add_component_targets(${PROJECT_NAME} util "${PROJECT_NAME}_util") diff --git a/tests/planner/ompl/test_GeometricStateSpace.cpp b/tests/planner/ompl/test_GeometricStateSpace.cpp index d29d04bb4d..8115e8fb87 100644 --- a/tests/planner/ompl/test_GeometricStateSpace.cpp +++ b/tests/planner/ompl/test_GeometricStateSpace.cpp @@ -207,7 +207,6 @@ TEST_F(GeometricStateSpaceTest, SatisfiesBoundsTrue) TEST_F(GeometricStateSpaceTest, SatisfiesBoundsFalseNullOmplState) { constructStateSpace(); - EXPECT_FALSE(nullptr); } TEST_F(GeometricStateSpaceTest, SatisfiesBoundsFalseNullAikidoState) diff --git a/tests/util/CMakeLists.txt b/tests/util/CMakeLists.txt index 52606a96ad..9fdcace358 100644 --- a/tests/util/CMakeLists.txt +++ b/tests/util/CMakeLists.txt @@ -56,3 +56,6 @@ target_link_libraries(test_SplineProblem "${PROJECT_NAME}_util") aikido_add_test(test_string test_string.cpp) target_link_libraries(test_string "${PROJECT_NAME}_util") + +aikido_add_test(test_yaml_extension test_yaml_extension.cpp) +target_link_libraries(test_yaml_extension "${PROJECT_NAME}_util") diff --git a/tests/util/test_yaml_extension.cpp b/tests/util/test_yaml_extension.cpp new file mode 100644 index 0000000000..1ec5bd7963 --- /dev/null +++ b/tests/util/test_yaml_extension.cpp @@ -0,0 +1,133 @@ +#include +#include + +//============================================================================== +TEST(YamlEigenExtension, Errors) +{ + std::string notSequential = "name: PRL"; + EXPECT_THROW(YAML::Load(notSequential)["name"].as(), + YAML::RepresentationException); + + std::string incorrectSize = "vector: [1, 2, 3]"; + EXPECT_THROW(YAML::Load(incorrectSize)["vector"].as(), + YAML::RepresentationException); +} + +//============================================================================== +TEST(YamlEigenExtension, LoadVectorMatrix3dIsometry3d) +{ + std::string yamlString + = "closed: !Vector [2.443, 2.443, 2.443, 0.0] \n" + "relative_pose: !Matrix \n" + "- [1.0, 0.0, 0.0] \n" + "- [0.0, 1.0, 0.0] \n" + "- [0.0, 0.0, 1.0] \n" + "absolute_pose: !Matrix \n" + "- [1.0, 0.0, 0.0, 0.0] \n" + "- [0.0, 1.0, 0.0, 0.0] \n" + "- [0.0, 0.0, 1.0, 0.0] \n" + "- [0.0, 0.0, 0.0, 1.0] \n"; + + auto root = YAML::Load(yamlString); + auto closed = root["closed"].as(); + auto relative_pose = root["relative_pose"].as(); + auto absolute_pose = root["absolute_pose"].as(); + + EXPECT_TRUE(closed.isApprox(Eigen::Vector4d(2.443, 2.443, 2.443, 0.0))); + EXPECT_TRUE(relative_pose.isApprox(Eigen::Matrix3d::Identity())); + EXPECT_TRUE(absolute_pose.matrix().isApprox( + Eigen::Isometry3d::Identity().matrix())); + + EXPECT_TRUE(YAML::Node(closed).as() == closed); + EXPECT_TRUE(YAML::Node(relative_pose).as() == relative_pose); + EXPECT_TRUE( + YAML::Node(absolute_pose).as().matrix().isApprox( + absolute_pose.matrix())); +} + +//============================================================================== +TEST(YamlEigenExtension, UnorderedMap) +{ + using UnorderedMap = std::unordered_map; + + std::string yamlString + = "key_map: \n" + " key1: value1\n" + " key2: value2\n"; + + auto root = YAML::Load(yamlString); + auto key_map = root["key_map"]; + auto map = key_map.as(); + EXPECT_TRUE(map.size() == 2u); + EXPECT_TRUE(map["key1"] == "value1"); + EXPECT_TRUE(map["key2"] == "value2"); + + EXPECT_TRUE(YAML::Node(map).as() == map); +} + +//============================================================================== +TEST(YamlEigenExtension, TsrTransforms) +{ + using TransformMap = std::unordered_map< + std::string, + Eigen::Isometry3d, + std::hash, + std::equal_to, + Eigen::aligned_allocator> + >; + + using TransformsMap = std::unordered_map; + + std::string yamlString + = "--- \n" + "# BH280 endeffector frame to palm offset = 0.18 m\n" + "left: \n" + " default: !Matrix &left_default \n" + " [[0., 0., 1., 0.], \n" + " [1., 0., 0., 0.], \n" + " [0., 1., 0., .18], \n" + " [0., 0., 0., 1.]] \n" + " cylinder: *left_default \n" + "# BH282 endeffector frame to palm offset = ? \n" + "right: \n" + " default: !Matrix &right_default \n" + " [[0., 0., 1., 0.], \n" + " [1., 0., 0., 0.], \n" + " [0., 1., 0., .18], \n" + " [0., 0., 0., 1.]] \n" + " cylinder: *right_default \n"; + + auto root = YAML::Load(yamlString); + auto map = root.as(); + EXPECT_TRUE(map.size() == 2u); + + auto left = map["left"]; + EXPECT_TRUE(left.size() == 2u); + + auto left_default = left["default"]; + Eigen::Isometry3d expectedLeftDefault = Eigen::Isometry3d::Identity(); + expectedLeftDefault.linear() + << 0.0, 0.0, 1.0, + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0; + expectedLeftDefault.translation() << 0.0, 0.0, 0.18; + EXPECT_TRUE(left_default.matrix().isApprox(expectedLeftDefault.matrix())); + + auto left_cylinder = left["cylinder"]; + EXPECT_TRUE(left_cylinder.matrix().isApprox(expectedLeftDefault.matrix())); + + auto right = map["right"]; + EXPECT_TRUE(left.size() == 2u); + + auto right_default = right["default"]; + Eigen::Isometry3d expectedRightDefault = Eigen::Isometry3d::Identity(); + expectedRightDefault.linear() + << 0.0, 0.0, 1.0, + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0; + expectedRightDefault.translation() << 0.0, 0.0, 0.18; + EXPECT_TRUE(right_default.matrix().isApprox(expectedRightDefault.matrix())); + + auto right_cylinder = left["cylinder"]; + EXPECT_TRUE(right_cylinder.matrix().isApprox(expectedRightDefault.matrix())); +}