From e71376743cbfa173a84b71ce8f98d5d319edaa07 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 8 Feb 2019 17:24:17 +0100 Subject: [PATCH] yamp-cpp parsing --- core/src/properties.cpp | 2 +- .../properties/CMakeLists.txt | 14 +- .../properties/property_factory.cpp | 2 + .../properties/property_from_yaml.cpp | 200 ++++++++++++++++++ 4 files changed, 215 insertions(+), 3 deletions(-) create mode 100644 visualization/motion_planning_tasks/properties/property_from_yaml.cpp diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 393fdcf5..a5297fe5 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -159,7 +159,7 @@ std::string Property::serialize(const boost::any& value) boost::any Property::deserialize(const std::string& type_name, const std::string& wire) { - if (wire.empty()) + if (type_name != Property::typeName(typeid(std::string)) && wire.empty()) return boost::any(); else return registry_singleton_.entry(type_name).deserialize_(wire); diff --git a/visualization/motion_planning_tasks/properties/CMakeLists.txt b/visualization/motion_planning_tasks/properties/CMakeLists.txt index 266e15ac..9632287d 100644 --- a/visualization/motion_planning_tasks/properties/CMakeLists.txt +++ b/visualization/motion_planning_tasks/properties/CMakeLists.txt @@ -3,14 +3,24 @@ set(MOVEIT_LIB_NAME motion_planning_tasks_properties) set(SOURCES property_factory.cpp ) + +include(FindPkgConfig) +pkg_check_modules(YAML yaml-cpp>=0.5) +if (YAML_FOUND) + # Only cmake > 3.12 provides XXX_LINK_LIBRARIES. Find the absolute path manually + find_library(YAML_LIBRARIES ${YAML_LIBRARIES} PATHS ${YAML_LIBRARY_DIRS}) + list(APPEND SOURCES property_from_yaml.cpp) + add_definitions(-DHAVE_YAML) +endif() + add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES}) target_link_libraries(${MOVEIT_LIB_NAME} - ${QT_LIBRARIES} + ${QT_LIBRARIES} ${YAML_LIBRARIES} ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $ - PRIVATE ${catkin_INCLUDE_DIRS} + PRIVATE ${catkin_INCLUDE_DIRS} ${YAML_INCLUDE_DIRS} ) install(TARGETS ${MOVEIT_LIB_NAME} diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index 375d0df8..bb124b47 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -151,6 +151,7 @@ void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::Property new rviz::Property("no properties", QVariant(), QString(), root); } +#ifndef HAVE_YAML rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, const std::string& description, const std::string& value, rviz::Property* old) @@ -168,5 +169,6 @@ rviz::Property* PropertyFactory::createDefault(const std::string& name, const st return result; } } +#endif } diff --git a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp new file mode 100644 index 00000000..ecf3fdbe --- /dev/null +++ b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp @@ -0,0 +1,200 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Bielefeld University + * 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 Bielefeld University 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. + *********************************************************************/ + +/* Author: Robert Haschke */ + +#include "property_factory.h" +#include +#include +#include + +namespace mtc = ::moveit::task_constructor; + +namespace moveit_rviz_plugin { + +/** Implement PropertyFactory::createDefault(), creating an rviz::Property (tree) + * from a YAML-serialized string. + * As we cannot know the required data type for a field from YAML parsing, + * we only distinguish numbers (FloatProperty) and all other YAML scalars (StringProperty). + */ + +// Try to set numeric or arbitrary scalar value from YAML node. Needs to match old's type. +void setScalarValue(rviz::Property* old, const YAML::Node& node) +{ + if (rviz::FloatProperty* p = dynamic_cast(old)) { + // value should be a number. If not throws YAML::BadConversion + p->setValue(node.as()); + return; + } + if (rviz::StringProperty* p = dynamic_cast(old)) { + // value should be an arbitrary string. If not throws YAML::BadConversion + p->setValue(QString::fromStdString(node.as())); + return; + } + throw YAML::BadConversion(); +} + +// Update existing old rviz:Property or create a new one from scalar YAML node +rviz::Property* createFromScalar(const QString& name, const QString& description, + const YAML::Node& node, rviz::Property* old) +{ + while (old) { // reuse existing Property? + try { + // try to update value, expecting matching rviz::Property + setScalarValue(old, node); + // only if setScalarValue succeeded, also update the rest + old->setName(name); + old->setDescription(description); + return old; + } catch (const YAML::BadConversion&) { + break; // on error, break from loop and create a new property + } + } + + // if value is a number, create a FloatProperty + try { return new rviz::FloatProperty(name, node.as(), description); } + catch (const YAML::BadConversion&) {} + + // otherwise create a StringProperty + return new rviz::StringProperty(name, QString::fromStdString(node.as()), description); +} + +// Create a scalar YAML node with given content value +YAML::Node dummyNode(const std::string& content) { + YAML::Node dummy; + dummy=content; + return dummy; +} + +// forward declaration +rviz::Property* createFromNode(const QString& name, const QString& description, + const YAML::Node& node, rviz::Property* old); + +// Reuse old property (or create new one) as parent for a sequence or map +rviz::Property* createParent(const QString& name, const QString& description, rviz::Property* old) +{ + // don't reuse float or string properties (they are for scalars) + if (dynamic_cast(old) || + dynamic_cast(old)) + old = nullptr; + if (!old) + old = new rviz::Property(name, description); + else { + old->setName(name); + old->setDescription(description); + } + return old; +} + +// Hierarchically create property from YAML map node +rviz::Property* createFromMap(const QString& name, const QString& description, + const YAML::Node& node, rviz::Property* root) +{ + root = createParent(name, description, root); + int index = 0; // current child index in root + for(YAML::const_iterator it=node.begin(); it!=node.end(); ++it) { + QString child_name = QString::fromStdString(it->first.as()); + int num = root->numChildren(); + // find first child with name >= it->name + int next = index; + while (next < num && root->childAt(next)->getName() < child_name) + ++next; + // and remove all children in range [index, next) at once + root->removeChildren(index, next-index); + num = root->numChildren(); + + // if names differ, insert a new child, otherwise reuse existing + rviz::Property *old_child = index < num ? root->childAt(index) : nullptr; + if (old_child && old_child->getName() != child_name) + old_child = nullptr; + + rviz::Property *new_child = createFromNode(child_name, "", it->second, old_child); + if (new_child != old_child) + root->addChild(new_child, index); + ++index; + } + // remove remaining children + root->removeChildren(index, root->numChildren()-index); + return root; +} + +// Hierarchically create property from YAML sequence node. Items are named [#]. +rviz::Property* createFromSequence(const QString& name, const QString& description, + const YAML::Node& node, rviz::Property* root) +{ + root = createParent(name, description, root); + int index = 0; // current child index in root + for (YAML::const_iterator it=node.begin(); it != node.end(); ++it) { + rviz::Property *old_child = root->childAt(index); // nullptr for invalid index + rviz::Property *new_child = createFromNode(QString("[%1]").arg(index), "", *it, old_child); + if (new_child != old_child) + root->addChild(new_child, index); + if (++index >= 10) + break; // limit number of entries + } + // remove remaining children + root->removeChildren(index, root->numChildren()-index); + return root; +} + +// Create a property from any YAML node. +rviz::Property* createFromNode(const QString& name, const QString& description, + const YAML::Node& node, rviz::Property* old) +{ + rviz::Property* result = nullptr; + switch(node.Type()) { + case YAML::NodeType::Scalar: result = createFromScalar(name, description, node, old); break; + case YAML::NodeType::Sequence: result = createFromSequence(name, description, node, old); break; + case YAML::NodeType::Map: result = createFromMap(name, description, node, old); break; + case YAML::NodeType::Null: result = createFromScalar(name, description, dummyNode("undefined"), old); break; + default: result = createFromScalar(name, description, dummyNode("unknown YAML node"), old); break; + } + result->setReadOnly(true); + return result; +} + +rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, + const std::string& description, const std::string& value, + rviz::Property* old) +{ + QString qname = QString::fromStdString(name); + QString qdesc = QString::fromStdString(description); + try { + return createFromNode(qname, qdesc, YAML::Load(value), old); + } catch (const std::exception &e) { + return createFromNode(qname, qdesc, dummyNode("YAML parse error"), old); + } +} + +}