From bb61513cb81ffb2a3e9922917b2256a38a5841f4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 9 Feb 2019 20:26:35 +0100 Subject: [PATCH] libyaml-based, incremental parsing of properties ... ensures display in document order --- .../properties/property_from_yaml.cpp | 324 +++++++++++------- 1 file changed, 196 insertions(+), 128 deletions(-) diff --git a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp index ce7b581c..a64b7741 100644 --- a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp +++ b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp @@ -41,76 +41,153 @@ 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). */ -#if 0 + +namespace { + +// Event-based YAML parser, creating an rviz::Property tree +// https://www.wpsoftware.net/andrew/pages/libyaml.html +class Parser { +public: + static const int YAML_ERROR_EVENT = 255; + + Parser(const std::string& value); + ~Parser(); + + rviz::Property* process(const QString& name, const QString& description, rviz::Property *old) const; + static rviz::Property* createScalar(const QString& name, const QString& description, + const QByteArray& value, rviz::Property *old); + +private: + // return true if there was no error so far + bool noError() const { return parser_.error == YAML_NO_ERROR; } + // parse a single event and return it's type, YAML_ERROR_EVENT on parsing error + int parse(yaml_event_t& event) const; + // process events: scalar, start mapping, start sequence + rviz::Property *process(const yaml_event_t &event, const QString &name, const QString &description, rviz::Property *old) const; + + inline static QByteArray byteArray(const yaml_event_t& event) { + assert(event.type == YAML_SCALAR_EVENT); + return QByteArray::fromRawData(reinterpret_cast(event.data.scalar.value), + event.data.scalar.length); + } + // Try to set value of existing rviz::Property (expecting matching types). Return false on error. + static bool setValue(rviz::Property *old, const QByteArray &value); + + static rviz::Property *createParent(const QString &name, const QString &description, rviz::Property *old); + rviz::Property* processMapping(const QString& name, const QString& description, rviz::Property *old) const; + rviz::Property* processSequence(const QString& name, const QString& description, rviz::Property *old) const; + +private: + mutable yaml_parser_t parser_; +}; + +Parser::Parser(const std::string &value) { + yaml_parser_initialize(&parser_); + yaml_parser_set_input_string(&parser_, reinterpret_cast(value.c_str()), value.size()); +} + +Parser::~Parser() { + yaml_parser_delete(&parser_); +} + +int Parser::parse(yaml_event_t &event) const { + if (!yaml_parser_parse(&parser_, &event)) { + yaml_event_delete(&event); + return YAML_ERROR_EVENT; + } + return event.type; +} + +// main processing function +rviz::Property *Parser::process(const QString &name, const QString &description, rviz::Property *old) const { + bool stop = false; + while (!stop) { + yaml_event_t event; + switch (parse(event)) { + case YAML_ERROR_EVENT: return Parser::createScalar(name, description, "YAML error", old); + case YAML_STREAM_END_EVENT: + stop = true; + break; + + case YAML_SEQUENCE_START_EVENT: + case YAML_MAPPING_START_EVENT: + case YAML_SCALAR_EVENT: + return process(event, name, description, old); + + default: break; + } + } + // if we get here, there was no content in the yaml stream + return createScalar(name, description, "undefined", old); +} + +// default processing for scalar, start mapping, start sequence events +rviz::Property* Parser::process(const yaml_event_t& event, + const QString& name, const QString& description, rviz::Property *old) const { + switch (event.type) { + case YAML_SEQUENCE_START_EVENT: return processSequence(name, description, old); + case YAML_MAPPING_START_EVENT: return processMapping(name, description, old); + case YAML_SCALAR_EVENT: return createScalar(name, description, byteArray(event), old); + } + assert(false); // should not be reached +} + // 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) +bool Parser::setValue(rviz::Property* old, const QByteArray& value) { if (rviz::FloatProperty* p = dynamic_cast(old)) { - // value should be a number. If not throws YAML::BadConversion - p->setValue(node.as()); - return; + bool ok = true; + double v = value.toDouble(&ok); + if (ok) p->setValue(v); + return ok; } 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; + p->setValue(value); + return true; } - throw YAML::BadConversion(); + return false; } // 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) +rviz::Property *Parser::createScalar(const QString &name, const QString &description, + const QByteArray &value, 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 - } + // try to update value, expecting matching rviz::Property + if (old && setValue(old, value)) { + // only if setValue succeeded, also update the rest + old->setName(name); + old->setDescription(description); + return old; } - // if value is a number, create a FloatProperty - try { return new rviz::FloatProperty(name, node.as(), description); } - catch (const YAML::BadConversion&) {} + bool ok = true; + double v = value.toDouble(&ok); + if (ok) // if value is a number, create a FloatProperty + old = new rviz::FloatProperty(name, v, description); + else // otherwise create a StringProperty + old = new rviz::StringProperty(name, value, description); - // otherwise create a StringProperty - return new rviz::StringProperty(name, QString::fromStdString(node.as()), description); + old->setReadOnly(true); + return old; } -// 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) +rviz::Property* Parser::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 { + if (!old) { + old = new rviz::Property(name, QVariant(), description); + old->setReadOnly(true); + } else { old->setName(name); old->setDescription(description); } @@ -118,31 +195,58 @@ rviz::Property* createParent(const QString& name, const QString& description, rv } // Hierarchically create property from YAML map node -rviz::Property* createFromMap(const QString& name, const QString& description, - const YAML::Node& node, rviz::Property* root) +rviz::Property* Parser::processMapping(const QString& name, const QString& description, rviz::Property* root) const { 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(); + bool stop = false; + while (!stop && noError()) { // parse all map items + yaml_event_t event; + switch (parse(event)) { // parse key + case YAML_MAPPING_END_EVENT: // all fine, reached end of mapping + stop = true; + break; - // 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; + case YAML_SCALAR_EVENT: { // key + QByteArray key = byteArray(event); + int num = root->numChildren(); + // find first child with name >= it->name + int next = index; + while (next < num && root->childAt(next)->getName() < key) + ++next; + // and remove all children in range [index, next) at once + root->removeChildren(index, next-index); + num = root->numChildren(); - rviz::Property *new_child = createFromNode(child_name, "", it->second, old_child); - if (new_child != old_child) - root->addChild(new_child, index); - ++index; + // 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() != key) + old_child = nullptr; + + rviz::Property *new_child = nullptr; + switch (parse(event)) { // parse value + case YAML_MAPPING_START_EVENT: + case YAML_SEQUENCE_START_EVENT: + case YAML_SCALAR_EVENT: + new_child = process(event, key, "", old_child); + break; + default: // all other events are an error + new_child = createScalar(key, "", parser_.problem, old_child); + root->setValue("YAML error"); + break; + } + + if (new_child != old_child) + root->addChild(new_child, index); + ++index; + break; + } + + default: // unexpected event + root->setValue("YAML error"); + stop = true; + break; + } } // remove remaining children root->removeChildren(index, root->numChildren()-index); @@ -150,42 +254,44 @@ rviz::Property* createFromMap(const QString& name, const QString& description, } // 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) +rviz::Property* Parser::processSequence(const QString& name, const QString& description, rviz::Property* root) const { 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 + bool stop = false; + while (!stop && noError()) { // parse all map items + yaml_event_t event; + switch (parse(event)) { + case YAML_SEQUENCE_END_EVENT: // all fine, reached end of sequence + stop = true; + break; + + case YAML_MAPPING_START_EVENT: + case YAML_SEQUENCE_START_EVENT: + case YAML_SCALAR_EVENT: { + rviz::Property *old_child = root->childAt(index); // nullptr for invalid index + rviz::Property *new_child = process(event, QString("[%1]").arg(index), "", old_child); + if (new_child != old_child) + root->addChild(new_child, index); + if (++index >= 10) + stop = true; // limit number of shown entries + break; + } + + default: // unexpected event + root->setValue("YAML error"); + stop = true; + break; + } } // 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; } -#endif -inline std::string indent(unsigned int depth) { return std::string(2*depth, ' '); } +namespace moveit_rviz_plugin { rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, const std::string& description, const std::string& value, @@ -193,46 +299,8 @@ rviz::Property* PropertyFactory::createDefault(const std::string& name, const st { QString qname = QString::fromStdString(name); QString qdesc = QString::fromStdString(description); - - yaml_parser_t parser; - yaml_parser_initialize(&parser); - yaml_parser_set_input_string(&parser, reinterpret_cast(value.c_str()), value.size()); - try { - unsigned int depth=0; - bool proceed = true; - while (proceed) { - yaml_event_t event; - if (!yaml_parser_parse(&parser, &event)) { - yaml_event_delete(&event); - throw std::runtime_error(parser.problem); - break; - } - - switch(event.type) - { - case YAML_NO_EVENT: std::cout << indent(depth) << "No event!" << std::endl; break; - /* Stream start/end */ - case YAML_STREAM_START_EVENT: std::cout << indent(depth++) << "STREAM START" << std::endl; break; - case YAML_STREAM_END_EVENT: std::cout << indent(--depth) << "STREAM END" << std::endl; break; - /* Block delimeters */ - case YAML_DOCUMENT_START_EVENT: std::cout << indent(depth++) << "Start Document" << std::endl; break; - case YAML_DOCUMENT_END_EVENT: std::cout << indent(--depth) << "End Document" << std::endl; break; - case YAML_SEQUENCE_START_EVENT: std::cout << indent(depth++) << "Start Sequence" << std::endl; break; - case YAML_SEQUENCE_END_EVENT: std::cout << indent(--depth) << "End Sequence" << std::endl; break; - case YAML_MAPPING_START_EVENT: std::cout << indent(depth++) << "Start Mapping" << std::endl; break; - case YAML_MAPPING_END_EVENT: std::cout << indent(--depth) << "End Mapping" << std::endl; break; - /* Data */ - case YAML_ALIAS_EVENT: std::cout << indent(depth) << "alias anchor=" << event.data.alias.anchor << std::endl; break; - case YAML_SCALAR_EVENT: std::cout << indent(depth) << "scalar: " << event.data.scalar.value << std::endl; break; - } - proceed = event.type != YAML_STREAM_END_EVENT; - yaml_event_delete(&event); - } - return nullptr; - } catch (const std::exception &e) { - std::cout << e.what() << std::endl; - return nullptr; - } + Parser parser(value); + return parser.process(qname, qdesc, old); } }