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 mtc = ::moveit::task_constructor;
|
||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
|
||||||
|
|
||||||
/** Implement PropertyFactory::createDefault(), creating an rviz::Property (tree)
|
/** Implement PropertyFactory::createDefault(), creating an rviz::Property (tree)
|
||||||
* from a YAML-serialized string.
|
* from a YAML-serialized string.
|
||||||
* As we cannot know the required data type for a field from YAML parsing,
|
* 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).
|
* 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.
|
// 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)) {
|
if (rviz::FloatProperty* p = dynamic_cast<rviz::FloatProperty*>(old)) {
|
||||||
// value should be a number. If not throws YAML::BadConversion
|
bool ok = true;
|
||||||
p->setValue(node.as<double>());
|
double v = value.toDouble(&ok);
|
||||||
return;
|
if (ok) p->setValue(v);
|
||||||
|
return ok;
|
||||||
}
|
}
|
||||||
if (rviz::StringProperty* p = dynamic_cast<rviz::StringProperty*>(old)) {
|
if (rviz::StringProperty* p = dynamic_cast<rviz::StringProperty*>(old)) {
|
||||||
// value should be an arbitrary string. If not throws YAML::BadConversion
|
// value should be an arbitrary string. If not throws YAML::BadConversion
|
||||||
p->setValue(QString::fromStdString(node.as<std::string>()));
|
p->setValue(value);
|
||||||
return;
|
return true;
|
||||||
}
|
}
|
||||||
throw YAML::BadConversion();
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update existing old rviz:Property or create a new one from scalar YAML node
|
// Update existing old rviz:Property or create a new one from scalar YAML node
|
||||||
rviz::Property* createFromScalar(const QString& name, const QString& description,
|
rviz::Property *Parser::createScalar(const QString &name, const QString &description,
|
||||||
const YAML::Node& node, rviz::Property* old)
|
const QByteArray &value, rviz::Property *old)
|
||||||
{
|
{
|
||||||
while (old) { // reuse existing Property?
|
// try to update value, expecting matching rviz::Property
|
||||||
try {
|
if (old && setValue(old, value)) {
|
||||||
// try to update value, expecting matching rviz::Property
|
// only if setValue succeeded, also update the rest
|
||||||
setScalarValue(old, node);
|
old->setName(name);
|
||||||
// only if setScalarValue succeeded, also update the rest
|
old->setDescription(description);
|
||||||
old->setName(name);
|
return old;
|
||||||
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
|
bool ok = true;
|
||||||
try { return new rviz::FloatProperty(name, node.as<double>(), description); }
|
double v = value.toDouble(&ok);
|
||||||
catch (const YAML::BadConversion&) {}
|
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
|
old->setReadOnly(true);
|
||||||
return new rviz::StringProperty(name, QString::fromStdString(node.as<std::string>()), description);
|
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
|
// 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)
|
// don't reuse float or string properties (they are for scalars)
|
||||||
if (dynamic_cast<rviz::FloatProperty*>(old) ||
|
if (dynamic_cast<rviz::FloatProperty*>(old) ||
|
||||||
dynamic_cast<rviz::StringProperty*>(old))
|
dynamic_cast<rviz::StringProperty*>(old))
|
||||||
old = nullptr;
|
old = nullptr;
|
||||||
if (!old)
|
if (!old) {
|
||||||
old = new rviz::Property(name, description);
|
old = new rviz::Property(name, QVariant(), description);
|
||||||
else {
|
old->setReadOnly(true);
|
||||||
|
} else {
|
||||||
old->setName(name);
|
old->setName(name);
|
||||||
old->setDescription(description);
|
old->setDescription(description);
|
||||||
}
|
}
|
||||||
@ -118,31 +195,58 @@ rviz::Property* createParent(const QString& name, const QString& description, rv
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Hierarchically create property from YAML map node
|
// Hierarchically create property from YAML map node
|
||||||
rviz::Property* createFromMap(const QString& name, const QString& description,
|
rviz::Property* Parser::processMapping(const QString& name, const QString& description, rviz::Property* root) const
|
||||||
const YAML::Node& node, rviz::Property* root)
|
|
||||||
{
|
{
|
||||||
root = createParent(name, description, root);
|
root = createParent(name, description, root);
|
||||||
int index = 0; // current child index in root
|
int index = 0; // current child index in root
|
||||||
for(YAML::const_iterator it=node.begin(); it!=node.end(); ++it) {
|
bool stop = false;
|
||||||
QString child_name = QString::fromStdString(it->first.as<std::string>());
|
while (!stop && noError()) { // parse all map items
|
||||||
int num = root->numChildren();
|
yaml_event_t event;
|
||||||
// find first child with name >= it->name
|
switch (parse(event)) { // parse key
|
||||||
int next = index;
|
case YAML_MAPPING_END_EVENT: // all fine, reached end of mapping
|
||||||
while (next < num && root->childAt(next)->getName() < child_name)
|
stop = true;
|
||||||
++next;
|
break;
|
||||||
// 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
|
case YAML_SCALAR_EVENT: { // key
|
||||||
rviz::Property *old_child = index < num ? root->childAt(index) : nullptr;
|
QByteArray key = byteArray(event);
|
||||||
if (old_child && old_child->getName() != child_name)
|
int num = root->numChildren();
|
||||||
old_child = nullptr;
|
// 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 names differ, insert a new child, otherwise reuse existing
|
||||||
if (new_child != old_child)
|
rviz::Property *old_child = index < num ? root->childAt(index) : nullptr;
|
||||||
root->addChild(new_child, index);
|
if (old_child && old_child->getName() != key)
|
||||||
++index;
|
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
|
// remove remaining children
|
||||||
root->removeChildren(index, root->numChildren()-index);
|
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 [#].
|
// Hierarchically create property from YAML sequence node. Items are named [#].
|
||||||
rviz::Property* createFromSequence(const QString& name, const QString& description,
|
rviz::Property* Parser::processSequence(const QString& name, const QString& description, rviz::Property* root) const
|
||||||
const YAML::Node& node, rviz::Property* root)
|
|
||||||
{
|
{
|
||||||
root = createParent(name, description, root);
|
root = createParent(name, description, root);
|
||||||
int index = 0; // current child index in root
|
int index = 0; // current child index in root
|
||||||
for (YAML::const_iterator it=node.begin(); it != node.end(); ++it) {
|
bool stop = false;
|
||||||
rviz::Property *old_child = root->childAt(index); // nullptr for invalid index
|
while (!stop && noError()) { // parse all map items
|
||||||
rviz::Property *new_child = createFromNode(QString("[%1]").arg(index), "", *it, old_child);
|
yaml_event_t event;
|
||||||
if (new_child != old_child)
|
switch (parse(event)) {
|
||||||
root->addChild(new_child, index);
|
case YAML_SEQUENCE_END_EVENT: // all fine, reached end of sequence
|
||||||
if (++index >= 10)
|
stop = true;
|
||||||
break; // limit number of entries
|
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
|
// remove remaining children
|
||||||
root->removeChildren(index, root->numChildren()-index);
|
root->removeChildren(index, root->numChildren()-index);
|
||||||
return root;
|
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,
|
rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type,
|
||||||
const std::string& description, const std::string& value,
|
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 qname = QString::fromStdString(name);
|
||||||
QString qdesc = QString::fromStdString(description);
|
QString qdesc = QString::fromStdString(description);
|
||||||
|
Parser parser(value);
|
||||||
yaml_parser_t parser;
|
return parser.process(qname, qdesc, old);
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user