mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
libyaml-based, incremental parsing of properties
... ensures display in document order
This commit is contained in:
parent
9810e3bb4b
commit
bb61513cb8
@ -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<const char*>(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<const yaml_char_t*>(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<rviz::FloatProperty*>(old)) {
|
||||
// value should be a number. If not throws YAML::BadConversion
|
||||
p->setValue(node.as<double>());
|
||||
return;
|
||||
bool ok = true;
|
||||
double v = value.toDouble(&ok);
|
||||
if (ok) p->setValue(v);
|
||||
return ok;
|
||||
}
|
||||
if (rviz::StringProperty* p = dynamic_cast<rviz::StringProperty*>(old)) {
|
||||
// value should be an arbitrary string. If not throws YAML::BadConversion
|
||||
p->setValue(QString::fromStdString(node.as<std::string>()));
|
||||
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<double>(), 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<std::string>()), 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<rviz::FloatProperty*>(old) ||
|
||||
dynamic_cast<rviz::StringProperty*>(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<std::string>());
|
||||
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<const yaml_char_t*>(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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user